[发明专利]一种复杂环境下的三维定位追踪方法

权利要求书

1.一种复杂环境下的三维定位追踪方法，其特征在于，包括如下步骤：

步骤1，读取一条含有n个及以上基站的到达时间差TDOA数据和主基站的到达时间TOF数据；

步骤2，将TDOA数据和TOF数据利用极大似然法联立方程解算出2个三维解算结果，并将2个三维解算结果的x、y轴结果放入数组A；

步骤3，由数组A中2个x、y轴三维解算结果分别反推出主基站的TOF数据和从基站的TDOA数据，并分别求反推结果与实际主基站TOF和从基站TDOA数据差值的平方和，选出平方和结果最小1个的x、y轴结果，将结果存入MS数组；

步骤4，对MS数组进行二维Taylor迭代，得到优化的x、y轴结果，将结果存入数组X；

步骤5，根据数组X中的x、y轴结果，通过最小二乘法解算，如果无实数解，则认为z轴结果为0，如果有实数解，则解出两个z轴结果，分别记为z(1)、z(2)；

步骤6，将z(1)、z(2)分别通过最小二乘法反推出主基站的z轴结果，并分别求反推结果与实际主基站z轴结果差值的平方，选出平方结果最小的z，并将这个平方结果最小的z记为zz；

步骤7，将数组X和zz联合进行三维Taylor解算，将解算出的三维结果放入最终定位结果的数组Fn；

步骤8，对数组Fn进行平均滤波，将滤波结果放入轨迹数组fr，用于显示实时轨迹；

步骤9，该条数据处理完毕，回到步骤1，读取下一条数据；

步骤1中，读入的TDOA数据data的形式为：

data＝(x1，y1，z1，d1；

x2，y2，z2，d12；

...

xn，yn，zn，d1n)，

中n表示该条TDOA数据中基站的个数，x1、y1、z1分别表示主基站的x、y、z轴坐标；1表示主基站，d1表示当前主基站的TOF数据，d1i表示当前待求坐标到主基站的距离和到序号为i的基站的距离之差，2＝i＝n，即d1i＝d1-di，xi、yi、zi分别表示基站i的x、y、z轴坐标；该步骤要求基站数大于等于4，即n＝4；

步骤2包括：

将TDOA数据和TOF数据做变换，变换后的TOF数据为tof_data，变换后的TDOA数据为tdoa_data：

tof_data＝d1，

tdoa_data＝(d12，d13，d14，...，d1n)，

其中d1i表示当前坐标到主基站和基站i之间距离的差值，2＝i＝n；

设定基站的坐标为(x1，y1，z1)，(x2，y2，z2)，…，(xn，yn，zn)，其中(xi，yi，zi)表示基站i的三维坐标，该坐标已知，则改进的极大似然算法表示为：

目标位置与基站的距离关系为：(d1+di1)^2＝(x-xi)^2+(y-yi)^2+(z-zi)^2，

设定：ki＝xi^2+yi^2+zi^2；s＝x^2+y^2+z^2，将上述等式转化为：

d1^2-k1＝-2(x1*x+y1*y+z1*z)+s，

di1^2+2di1*d1＝-2(xi1*x+yi1*y+zi1*z)+ki-k1，

设定Za＝[x，y，z，d1]'，则上式转化为：

G＝-2*[x1，y1，z1，-s/(2d1)；x21，y21，z21，d12；...；xn1，yn1，zn1，d1n]，h＝[d1^2；d12^2+K1-K2；d13^2+K1-K3；...；d1n^2+K1-Kn]；

设定[d1，d21，...，dn1]'＝[r1，r21，...，rn1]'+e，得到：

误差φ＝2B*e+e^2≈2B*e，B＝diag(d1，d21，...，dn1)；

由于噪声未知，设定噪声为高斯噪声，得到高斯协方差矩阵为：

Q＝E[ee']＝diag[σ^1，σ^2，…，σ^n)]＝(0.5*ones(size(h，1))+0.5*eye(size(h，1)))*(delta^2)，

得到

则得到损失函数为：(h-G*Za)'pinv(Φ)(h-G*Za)，

将损失函数最小化，得出的Za值才为所求结果，根据极大似然法得到：

Za＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*h；

因此得到Za的协方差矩阵：

cov(Za)＝E[ΔzΔz']＝pinv(G'*pinv(Φ)*G)，

Δz＝pinv(G'*pinv(Φ)*G)*G'*pinv(Φ)*B*e；

再次进行最大似然估计，将上述所得结果Za设为：

Za＝[t1，t2，t3，t4]'，t1＝x+e1，t2＝y+e2，t3＝z+e3，t4＝d1+e4，

使用POS1保存Za向量的前三维，即Za＝[t1，t2，t3]'，对t1，t2，t3，t4求平方得到新的损失函数为：

h1＝[(t1-x1)^2，(t2-y1)^2，(t3-z1)^2，t4^2]'，

G1＝[1 0 0；0 1 0；0 0 1；1 1 1]，

Za1＝[(x-x1)^2，(y-y1)^2，(z-z1)^2]'，

2(y-y1)*e2+e2^2，2(z-z1)*e3+e3^2，2d1*e4+e4^2]'≈[2(x-x1)*e1，2(y-y1)*e2，2(z-z1)*e3，2d1*e4]'，

得到:B1＝diag(x-x1，y-y1，z-z1，d1)，

再次根据最大似然估计得到：

Za1＝argmin(h1-G1*Za1)'*pinv(Φ1)*(h1-G1*Za1)＝pinv(G1'pinv(Φ1)*G1)*G1'pinv(Φ1)*h1，

最终得到POS2＝[x，y，z)]'＝[x1，y1，z1)]'±sqrt(Za1)；

其中x、y、z分别表示目标位置三维坐标；ki表示基站i的三维坐标平方和；s表示目标位置的三维坐标平方和，在第一次极大似然法求解中，Za表示目标位置坐标和TOF数据凑成的向量，φ表示结果存在的实际误差，h、G都为等式转化之后的矩阵，r1，r21，...，rn1分别对应表示d1，d21，...，dn1的实际精确值，e表示实际误差；B表示由d1，d21，...，dn1组成的对角矩阵，Q表示误差的协方差矩阵，Φ表示φ实际误差的协方差矩阵，cov(Za)表示Za的协方差矩阵，Δz表示Za与实际目标位置之间、实际目标位置和主基站距离的值的误差，POS1用于表示第一次极大似然法解算之后得到的三维目标位置坐标；在第二次极大似然法求解中，POS2表示第二次求解的最终目标位置的坐标，Za1表示为POS2与POS1误差的平方，t1、t2、t3、t4为Za向量中的四个分量，e1、e2、e3、e4分别表示t1、t2、t3、t4与实际的x、y、z、d1的误差，φ1表示Za与实际结果的大概误差，h1、G1都为等式转化之后凑成的矩阵，B1表示以x-x1，y-y1，z-z1，d1依次为对角线元素的对角矩阵，Φ1表示误差的协方差矩阵；pinv表示矩阵的求逆操作，矩阵右上角的'表示矩阵转置，sqrt表示开方运算；diag{r2，r3，…，rn}表示以r2，r3，…，rn依次为对角线元素的对角矩阵；

最终求得的POS1和POS2为含有三个元素的向量，表示为POS1＝(t1，t2，t3)，POS2＝(x，y，z)，其中(t1，t2，t3)和(x，y，z)都为所求的坐标估计值。