本发明涉及无线定位技术领域，特别涉及一种异构网络下快速精准的目标定位与跟踪方法。
背景技术：
目标的定位与跟踪是观测站通过测量目标信号的相关参数，对运动目标的位置和速度进行实时估算和预测的过程，是科研工作者研究的热点问题之一。目标跟踪技术无论是在军事还是在民用领域都有广泛的应用，主要用于雷达、救援、监测、传感器网络以及军事目标的定位与跟踪。由于依据各种测量技术观测的量与状态量之间的非线性关系，导致由观测量与状态量建立的定位与跟踪方程为非线性方程，故对目标的定位与跟踪属于非线性的滤波问题。目前现存的目标定位与跟踪方法在各自的定位场景下都有不错的定位与跟踪精度，但它们都是在同构网络环境中对目标实施定位与跟踪。但是随着无线通信技术的飞速发展，目前对目标的定位与跟踪通常由异构网络协同完成，现有的定位方法已经不能满足异构网络下的复杂定位场景。
定位与跟踪目标的过程中，由于依据各种测量技术观测的量与状态量之间的非线性关系，导致由观测量与状态量建立的定位与跟踪方程为非线性方程，故对目标的定位与跟踪属于非线性的滤波问题。解决非线性滤波问题主要的滤波技术有扩展卡尔曼滤波(Extended Kalman Filter,EKF)、无迹卡尔曼滤波(Unscented KF,UKF)，容积卡尔曼滤波(Cubature KF,CKF)和粒子滤波(Particle Filter,PF)等。EKF把非线性函数做一阶Taylor级数展开得到线性化方程，求解线性化方程实现对目标的跟踪。把多模型测量数据输入系统经过信息融合之后再进行线性EKF滤波，虽然定位精度较单一的EKF有所提高，但增加了运算量。同时由于EKF线性化时只保留了Taylor级数展开式的一阶项而带来较大的跟踪误差。PF能较好地解决EKF的缺陷，但PF面临严重的粒子退化和粒子耗尽问题，为了提高算法性能需采用大量的粒子，故增大了运算量和复杂度，因此对目标定位与跟踪时不宜单独使用PF技术。Julier和Uhlman结合EKF和PF的特点，通过无迹变换的思想在估计值附近确定采样点，用样本点表示的高斯密度来近似状态的概率密度函数，提出了UKF技术，精度可以达到泰勒级数展开的三阶近似，且避免了函数求导与雅克比矩阵的计算，缩减了运算量。但UKF需要对协方差矩阵进行开方运算，可能导致矩阵奇异引起滤波发散，导致跟踪性能较差。基于CKF通过分析舍弃误差的大小，引入特定的正交矩阵来改进容积采样，以减小CKF中的舍弃误差进一步提高算法精度，提出了OCKF算法，OCKF在没有增加运算量的前提下提高了跟踪精度。上述跟踪算法在各自的条件下都有不错的跟踪精度，但它们都是在同构网络环境中对目标实施定位与跟踪。混合级联位置跟踪(Hybrid Cascade Location Tracking,HCLT)算法是在异构网络环境下对目标进行跟踪，但HCLT使用了级联技术而增加了跟踪系统的成本，且算法先利用两步最小二乘法粗略估计目标的初始位置，然后利用卡尔曼滤波对线性部分进行滤波实施目标跟踪。由于算法中使用最小二乘线性化，故造成较大的跟踪误差。基于HCLT和OCKF算法文中提出了一致性正交容积卡尔曼跟踪(Unified Orthogonal Cubature Kalman Target Tracking,UOCKT)算法。所谓一致性卡尔曼滤波是指每个传感器都不在局部估计处理信号，来自异构网络的不同类型信号统一被送到信息融合中心，由融合中心对不同的信号进行统一估计处理，实现对目标的跟踪与状态估计。
在异构网络中，定位与跟踪目标时所使用的测量数据来自于不同类型的异构网络，测量数据的兼容与融合问题是目标定位与跟踪技术中必须面对的问题，如何处理测量数据的兼容与融合将直接影响目标的定位与跟踪精度。本发明提出了一致性正交容积卡尔曼滤波算法可以很好地解决上述问题，提高定位与跟踪目标的精度。
技术实现要素：
本发明提供一种异构网络下快速精准的目标定位与跟踪方法，可以解决现有技术中的上述问题。
本发明提供了一种异构网络下快速精准的目标定位与跟踪方法，包括以下步骤：
S1、通过异构网络获取用于目标定位与跟踪的测量数据；
假定二维平面内分布着由异构网络组成的观测站，依据观测站得到的到达时间(Time of Arrival，TOA)和到达时间差(Time Difference of Arrival，TDOA)的测量值对目标定位与跟踪，假设目标初始位置为u＝(x y)，初始速度为v＝(vx vy)，si＝(xi yi)为第i个观测站的位置坐标，N为用于测量TOA值的观测站数目，若目标信号到第i个观测站的TOA测量值为ti，则有
式(1)中，ri表示测量距离，ti表示信号传播时间，c为信号传播速度，为目标到第i个观测站的真实距离，ni为测量误差，通常为零均值的高斯随机过程；ei是由非视距(Non Line of Sight,NLOS)传播引起的误差；因此得到N组相互独立的TOA测量值；
同样假设平面内有M个测量TDOA值的蜂窝基站，通常把观测站j＝1作为参考基站，则目标信号到第j个观测站与到第1个观测站的时间差为tj1＝tj-t1，结合式(1)得到
式(2)中，为目标到参考基站j＝1的真实距离；则由M个观测站得到(M-1)组相互独立的TDOA测量值；通过上面得到的N组TOA测量值和(M-1)组TDOA测量值组建目标的状态方程，利用一致性正交容积卡尔曼获得目标的位置；
S2、构建含有目标位置信息的状态方程和测量方程；
所设计的一致性正交容积卡尔曼滤波跟踪(Unified Orthogonal Cubature Kalman Target Tracking,UOCKT)算法是将TOA和TDOA测量值输入到跟踪系统统一处理；然后在k时刻引入一个附加变量来代表系统中目标状态向量中的非线性项，即此状态向量包含目标的位置(xk yk)、速度(vx,k vy,k)、加速度(ax,k ay,k)以及附加变量结合和(1)式整理得到k时刻关于TOA的测量方程
式(3)中，同理k时刻关于TDOA的测量方程为
为了简化计算，设参考观测站为直角坐标系的原点，因此式(4)乘以λk/rj1,k与式(3)相加，整理得到
显然式(5)是包含目标位置的线性方程，可以用线性卡尔曼滤波来求解；由目标的状态量和测量值可得到卡尔曼滤波系统的状态方程和观测方程
x(k+1)＝Fx(k)+u(k)+w(k) (6)
z(k+1)＝Hx(k)+v(k) (7)
式(6)和式(7)中，x(k)＝[xk,yk,vx,k,vy,k,ax,k,ay,k]T为系统状态向量，u(k)为状态方程的输入控制，w(k)为状态噪声，v(k)为观测噪声，均服从零均值的高斯噪声且互不相关，w(k)的协方差矩阵为Q＝E[w(k)wT(k)]，v(k)的协方差矩阵为R＝E[v(k)vT(k)]，F为转移矩阵，H为观测矩阵，z(k+1)＝[r1,k,r2,k,…rN,k,r21,k,r31,k,…rM1,k]T为观测向量；结合式(5)，式(7)中的观测向量z(k+1)和观测矩阵H可以表示为
可见式(8)和式(9)中都具有(N+M-2)个相互独立的线性方程，全部的TOA测量值(r1,k→rN,k)和第一个TDOA测量值(r21,k)组成N个混合线性方程，剩余的TDOA测量值(r31,k→rM1,k)和第一个TOA测量值(r1,k)组成M-2个混合线性方程；因此，为了获得目标的精确位置，目标位置向量和附加向量的更新方法将至关重要；
S3、目标位置向量和附加向量的更新；
定义非线性变量λk为新引入的调整不同类型信号输入系统比例的加权因子，其主要作用是调整TOA和TDOA测量值输入系统输的比重以及对异构网络、不同测量技术的信息融合；假设目标在恒定的加速度下运动，则k时刻目标的位置坐标更新为：
式(10)和式(11)中T为采样时间间隔，为了获得附加变量估计和预测，结合式(7)加权N个TOA和M-2个TDOA测量方程，则关于和的表达式为：
参照式(10)和式(11)坐标更新方法，的更新形式为：
因此式(6)中的状态转移态矩阵F变为：
同时定义u(k-1)＝[0 0 (Wk-Wk-1) 0 0 0 0]T为输入控制，经分析可知系统状态向量的更新是由来自异构网络观测站不同信号的更新来实现的；结合系统中各参量的更新方法，通过正交容积卡尔曼滤波算法实现对目标的定位与跟踪；
S4、采用正交容积卡尔曼滤波算法更新系统参量获取目标位置信息；
对目标的定位与跟踪目标要实现异构网络之间、TOA和TDOA测量值之间的信息相互融合；结合引入的动态加权因子λk，通过UOCKT算法对目标定位与跟踪过程如下：
1)系统状态方程的一步预测为：
式(18)中nx为系统状态量的维数，文中nx＝7；x(i)(k+1|k)＝Fx(i)(k|k)，x(i)(k|k)为采样向量，式(18)中权值函数
2)系统状态协方差的一步预测为：
3)系统观测方程的一步预测为：
4)系统观测的预测协方差矩阵S(k+1)为：
5)系统的增益矩阵G(k+1)为：
6)下一时刻系统状态的协方差矩阵更新为：
P(k+1|k+1)＝P(k+1|k)-G(k+1)S(k+1)G(k+1)T (23)
7)下一时刻系统状态方程的估计为：
式(21)中和分别代表k+1时刻TOA和TDOA测量中观测噪声的估计协方差矩阵，矩阵B和满足H矩阵的要求，具体表达式为
式(25)中IN为单位阵；C(M-2)×N为第一列元素为1，其它元素全为0的矩阵；D(N-1)×M为第一列元素为-1，第二列元素为1，其它(M-2)列元素全为0的矩阵；E(M-1)×M为第一列元素为-1，其它元素组成I(M-1)的单位阵且单位阵中第一个元素为0；而k+1时刻TOA和TDOA测量误差的协方差矩阵分别为：
式(26)中：σ2为测量误差的方差。
较佳地，所述步骤S2中的加权因子λk为混合加权因子，混合加权因子的更新方法包括：UOCKT跟踪算法定位与跟踪目标的过程中，在k时刻引入加权因子λk，其主要作用是调整TOA和TDOA测量值作为系统输入量的比重以及对异构网络、不同测量技术的信息融合，通过线性方程(7)可知加权因子λk的正负号对跟踪系统的性能影响不大，而加权因子λk绝对值的大小对跟踪系统的性能影响较大，加权因子λk绝对值越大，TDOA测量值输入系统的比重就越大，然而在异构网络环境中不同时刻将会出现一种网络观测站比较充足或一种测量比较精确，因此固定的加权因子λk将会给跟踪系统带来较大的误差，为了减小跟踪系统的估计误差，需要一种能动态调整自身大小的自适应加权因子λk，基于卡尔曼滤波状态更新理论设计自适应加权因子λk，基于卡尔曼滤波状态更新理论设计自适应加权因子λk，
且自适应加权因子λk与目标的估计位置有密切联系，由于引入的非线性附加变量中包含自适应加权因子λk和待估计的状态向量(xk yk)，加权因子λk同样会被预测和更新，以此提高跟踪系统的性能，即使系统存在测量误差时系统的性能也能得到较好改善。根据后验概率估计可得k-1时刻的状态向量：
结合非线性附加变量加权因子λk在k时刻的预测为：
与现有技术相比，本发明的有益效果在于：
本发明针对异构网络采集的数据采用集中式多传感器信息融合结构，任何一个传感器都可以与融合中心进行信息交互，每个传感器都可以利用到全局信息，同时，引入自适应加权因子来增加优质信号输入系统的比重，以高质量的信号来提高目标定位与跟踪精度。
本发明基于一致性原理和正交容积卡尔曼滤波技术，提出了一致性正交容积卡尔曼滤波算法来定位与跟踪目标，通过引入的动态加权因子来加大优质信号输入系统的比重，以高质量的信号来提高目标定位与跟踪精度。
附图说明
图1为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法的流程图。
图2为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中目标运动轨迹及观测站分布示意图。
图3为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中目标x方向位置跟踪对比图。
图4为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中目标x方向位置跟踪RMSE对比图。
图5为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中目标x方向速度跟踪对比图。
图6为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中目标x方向速度跟踪RMSE对比图。
图7为本发明提供的一种异构网络下快速精准的目标定位与跟踪方法中自适应加权因子λ随跟踪时间的变化曲线，以及加权因子λ对目标估计位置RMSE的影响。
图8为EKF、HCLT、OCKF和UOCKT四种算法定位精度的对比曲线。
具体实施方式
下面结合附图，对本发明的一个具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。
一致性正交容积卡尔曼目标跟踪算法的流程如下：
步骤1首先假设目标的初始状态初始协方差矩阵P(0|0)；
步骤2结合式(16)，将初值代入(18)式得到状态向量的一步预测
步骤3将误差协方差矩阵代入(19)式得到系统状态协方差的一步预测P(k+1|k)；
步骤4将式(18)，(20)，(21)代入式(22)得到系统的增益矩阵G(k+1)；
步骤5将式(19)，(21)，(22)代入式(23)得到状态协方差矩阵的更新P(k+1|k+1)；
步骤6将步骤2、3、4和观测向量代入式(24)得到下一时刻系统状态方程的预测
步骤7转至步骤2进行下一时刻滤波。
最终通过正交容积卡尔曼滤波得到目标的各个时刻的位置坐标，实现对目标的定位与跟踪，具体算法流程如图1所示。
为了验证所提UOCKT算法的有效性，基于MATLAB平台用蒙特卡洛实验对算法进行测试，文中分别对EKF、HCLT、OCKF和UOCKT算法进行仿真实验并分析对比，以此验证UOCKT算法的性能。使用相同的计算机(4核酷睿i5处理器，内存4GB，MATLAB 7.1)运行1000次蒙特卡洛实验，EKF、HCLT、OCKF和UOCKT算法完成80s的滤波过程需要的时间分别为0.0437s、0.0846s、0.0754s和0.0791s。对比滤波所用时间：(1)EKF算法完成滤波用时最少，计算复杂度最低，故EKF算法对目标跟踪误差较大；(2)HCLT算法滤波所用时间几乎为EKF算法的两倍，是因为在HCLT算法中TDOA和TOA的测量数据被分开各自滤波，然后再送到数据融合中心。在四种算法中用时最长，计算复杂度最高，对目标跟踪误差较EKF算法有所降低；(3)OCKF算法滤波所用时间介于EKF和HCLT算法之间，且该算法的计算复杂度也在两者之间。但在EKF、HCLT和OCKF三种算法中OCKF算法的跟踪误差最小；(4)UOCKT在OCKF算法基础上引入非线性附加变量和自适应加权因子，将两种不同类型的信号按不同的比例输入融合中心统一滤波，故UOCKT算法滤波所用时间比OCKF算法略有增加，但跟踪精度比OCKF算法有较大提高。
为了更详细地分析所提算法的整体性能，从不同角度进行实验分析。设定式(1)中的测量误差服从均值为零，方差为σ2的标准正态分布ni～N(0,σ2)。对于NLOS引起误差ei，通常ei是由电波折射引起的正均值附加时延误差，考虑指数分布的模型时概率密度函数为
式中为均方根时延扩展，di为目标和观测站之间的距离，ε为路径损耗指数。假设仿真场景为城市中对目标定位与跟踪，且观测站是由异构网络组成，包括用于测量TDOA据规则分布的蜂窝网络基站，测量TOA数据随机分布的DSRC网络。目标的真实位置为(-300,300)m，做等速螺旋运动，初速度为(30,2.4)m/s。目标真实运动轨迹以及由异构网络组成的观测站分布如图2所示。因测量一组TDOA数据需要两个观测站且信号的传播距离大于测量TOA数据时信号的传播距离，故设定TOA和TDOA测量误差的标准差分别为σTOA＝60m和σTDOA＝160m，均为零均值的随机高斯噪声。城市环境中A＝0.4，TOA和TDOA测量路径损耗指数分别为εTOA＝0.2和εTDOA＝0.4，ζ为零均值对数正态分布的衰减因子变量且标准差σζ＝4dB,设初始滤波时目标位置为(0,0)m，初始速度为(0,0)m/s，采样时间间隔T＝1s，观测时间为80s，用均方根误差(Root Mean Square Error,RMSE)来表征各算法的跟踪性能。鉴于篇幅有限仅对目标x方向的位置和速度进行分析。
图3展示了四种算法在x方向对目标位置跟踪的情况，图4描绘了四种算法对目标x方向位置跟踪的RMSE曲线。从图中不难看出四种算法经过一段时间的跟踪滤波，最终都能以较小的位置误差锁定目标。由于EKF采用了泰勒级数展开运算且只保留了展开式的一阶项而忽略了高阶项，故引起较大的算法误差，且跟踪速度较慢，滤波稳定后误差较大，由图可知EKF明显差于HCLT、OCKF以及所提的UOCKT算法的跟踪性能。HCLT在异构网络中利用两步最小二乘算法结合KF跟踪目标，其跟踪性能明显优于EKF算法，定位精度提升了10m左右。OCKF在CKF算法的基础上，引入一个特定正交矩阵改进容积采样，使舍弃误差变小，定位精度较HCLT又提升了3～4m。文中UOCKT算法采用TDOA和TOA混合测量技术，引入动态加权因子λk实时调整目标跟踪过程中TDOA和TOA测量数据输入系统的比重，最后进行一致性正交容积卡尔曼滤波输出。UOCKT明显优于EKF、HCLT和OCKF算法的跟踪性能，定位精度较OCKF又有3～4m的提升。由此说明UOCKT定位精度较高，跟踪性能优越。
图5展示的是四种算法对目标x方向速度的跟踪结果，图6描绘了四种算法对目标x方向速度跟踪的RMSE曲线。由仿真曲线知，UOCKT算法对目标运动速度的跟踪性能明显优于其它三种算法。其它三种算法在速度跟踪的初期都有明显的抖动，性能不稳定。尤其EKF和HCLT算法跟踪目标初期抖动较为严重，跟踪速度较为缓慢且性能较差，约经过30s才能锁定稳定的误差。虽然OCKF算法较EKF和HCLT算法在跟踪速度，定位精度上有了较为明显的提高，但依然存在收敛速度较慢，定位误差较大等缺点。UOCKT算法较其它三种算法在收敛速度上均有较大的提高，减小了定位误差，速度的跟踪性能更强。
图7给出了自适应加权因子λ随跟踪时间的变化曲线，以及加权因子λ对目标估计位置RMSE的影响。选择HCLT算法和所提UOCKT算法来比较目标估计位置的RMSE，是因为这两种算法都是针对异构网络下的目标跟踪。区别在于所提UOCKT算法引入了自适应加权因子λ来改变不同类型信号输入系统的比重，从而提高跟踪精度；而HCLT算法是两种类型信号同时输入，没有对输入的信号进行优化。通过图中曲线看到，跟踪时间在10～20秒之间加权因子λ变化较大，说明这段时间内异构网络传播环境变化较大，导致输入系统的信号质量较差，从而使HCLT和UOCKT算法在这段时刻内估计位置的RMSE增大。通过比较图中三条曲线不难发现，加权因子λ对HCLT算法估计位置的RMSE没有任何影响。然而所提UOCKT算法估计位置的RMSE受加权因子λ的影响较大。在25～40秒这段时间内，加权因子λ的绝对值逐渐变小，表明输入系统的TDOA测量数据比重减小，TOA测量数据比重增大，导致估计位置的RMSE逐渐变大，表明定位目标时TDOA测量数据优于TOA测量数据。同时，由仿真曲线知加权因子λ的符号对估计位置的RMSE影响不大，加权因子λ的绝对值对目标定位误差影响较大。总之引入加权因子λ的UOCKT算法的定位性能优于HCLT算法。
图8描绘了EKF、HCLT、OCKF和UOCKT四种算法定位精度的对比曲线。从图中可以看出定位精度概率为67％时，四种算法跟踪目标的平均位置误差依次约为170m、100m、90m和43m.由图中定位精度曲线可知EKF算法定位精度较差，而其它三种算法基本可以满足E-911的定位要求(E-911是为手机用户提供紧急求助服务，即提供呼叫者的位置以便及时救援)，定位精度在100m以内的概率不低于67％。尤其是文中所提的UOCKT算法利用蜂窝基站和网络辅助设备组成的异构网络，经过一致性正交容积卡尔曼滤波可以实时更新状态量，从而很大程度上减小了跟踪平均位置误差。
本发明针对异构网络采集的数据采用集中式多传感器信息融合结构，任何一个传感器都可以与融合中心进行信息交互，每个传感器都可以利用到全局信息，同时，引入自适应加权因子来增加优质信号输入系统的比重，以高质量的信号来提高目标定位与跟踪精度。
本发明基于一致性原理和正交容积卡尔曼滤波技术，提出了一致性正交容积卡尔曼滤波算法来定位与跟踪目标，通过引入的动态加权因子来加大优质信号输入系统的比重，以高质量的信号来提高目标定位与跟踪精度。
以上公开的仅为本发明的几个具体实施例，但是，本发明实施例并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。