一种基于中间过渡状态的卡尔曼滤波算法

本发明属于目标跟踪领域，提出了一种基于中间过渡状态的卡尔曼滤波算法。

背景技术：

1、随着科技的进步，目标跟踪已广泛应用于军事和民用等领域当中。雷达是获取信息的重要设备，实际雷达系统中，目标的距离、角度、径向速度等量测信息通常在极坐标系或球坐标系下获得，而系统状态方程往往在笛卡尔坐标系下建立，因此雷达目标跟踪过程中存在量测的非线性问题。量测非线性问题一般有两种解决方法：非线性滤波和量测转换方法。

2、扩展卡尔曼滤波(extended kalman filter，ekf)是一种基于泰勒展开的非线性滤波算法(见文献：lcondes,c.t.,control and dynamic system,nonlinear and kalmanfiltering technique.academic press,1983)。ekf解决了量测非线性问题，但当量测相对于系统状态的非线性程度较高时，将导致跟踪误差较大。文献(julier s，uhlmann j,durrant whyte h f.anew method for the nonlinear transformation of means andcovariances in filters and estimations[j].ieee transactions on automaticcontrol,2000,45(3)：477-482.)提出了无迹卡尔曼滤波(unscented kalman filter，ukf)，ukf是通过无迹变换对非线性系统状态的后验概率密度进行近似。ukf不需要对非线性函数进行线性化，计算精度可达到至少三阶以上的泰勒精度，即ukf具有滤波精度高，收敛性好等优点。但对于高维系统，ukf则需选择合理的参数才能达到高精度。粒子滤波(particle filter，pf)算法是基于序列蒙特卡罗的一种滤波方法(见文献：咎孟恩，周航，韩丹.粒子滤波目标跟踪算法综述[j].计算机工程与应用，2019，55(5)：8-17.)。pf不受线性化误差或者高斯噪声假定的限制，性能与ukf算法相近，但迭代若干次后，大部分粒子有较大的衰减，需要选取大量的粒子，或进行重抽样，计算量大。文献(arasaratnam i,haykins,elliott r j.discrete-time nonlinear filtering algorithms using gauss-hermite quadrature[j].proceedings of the ieee,2007,95(5)：953-977.)提出了高斯-厄尔米特积分滤波(gauss-hermite quadrature filter，ghqf)算法，ghqf采用高斯-厄尔米特积分规则进行数值逼近，可获得较好的目标跟踪精度，但ghqf将单变量高斯积分扩展到多维积分，其计算复杂度随系统维数指数增长，导致跟踪不具有实时性。文献(i.arasaratnam,s.haykin.cubature kalman filters[j].ieee transactions onautomatic control,2009,54(6):1254-1269)提出了容积卡尔曼滤波(cubature kalmanfilter，ckf)算法，容积卡尔曼滤波基于三阶球面-径向容积准则，各容积点权值相等，具有更高的滤波精度，且计算复杂度较低。为进一步提ckf的稳定性，文献(i.arasaratnam,shaykin,and t.r.hurd.cubature kalman filtering for continuous-discretesystems:theory and simulations[j].ieee trans.signal process,2010,58(10):4977-4993.)将正交分解引入ckf的协方差矩阵中，形成ckf的平方根形式，即平方根容积卡尔曼滤波(square-root cubature kalman filter，srckf)算法。srckf迭代运算中使用协方差矩阵的平方根，保证了协方差矩阵的对称性和正(半)定性，进一步提高了滤波算法的跟踪精度和稳定性。

3、解决量测非线性的另一个方法是量测转换。传统的量测转换卡尔曼滤波(converted measurement kalman filter，cmkf)算法是运用坐标变换将极坐标系或球坐标系下的量测信息转换到笛卡尔坐标系下，通过kf进行滤波处理(见文献：p.suchomski.explicit expressions for debiased statistics of 3d convertedmeasurements[j].ieee trans.aerospace and electronic system,35(1):368-370.)。然而由于坐标转换的非线性，转换结果是有偏的，跟踪精度较低。去偏量测转换卡尔曼滤波(debiased converted measurement kalman filter，dcmkf)算法是对cmkf进行加性去偏处理，消除了传统量测转换卡尔曼滤波算法的误差(见文献：bordonaro s,willett p,bar-shalom y.decorrelated unbiased converted measurement kalman filter[j].ieeetransactions on aerospace and electronic systems,2014,50(2):1431-144)。然而上述关于量测转换的研究内容仅包含位置量测信息，文献(a.farina and f.a.studer.raderdata processing.vol.ⅰ:introduction and tracking,vol.ⅱ:advanced topics andapplications[m].research studies press,letchworth,1985)提出如果充分利用多普勒等量测信息，则算法的跟踪精度可以得到进一步提升。当引入径向速度时，所面临的问题是径向速度量测与目标状态的非线性程度很大，同时，文献(y.bar-shalom.negativecorrelation and optimal tracking with doppler measurements[j].ieeetrans.aerospace and electronic systems,2001,37(3):1117-1120)指出，对于一些波形来说径向距离与径向速度的误差统计特性是相关的。序贯滤波(sequential filter，sq)利用径向距离与径向速度的乘积构造伪量测来降低径向速度相对于目标运行状态的非线性，再将位置量测和伪量测分别进行滤波处理：在对位置量测和伪量测转换误差进行去相关处理后，位置量测通过卡尔曼滤波处理，伪量测通过二阶扩展卡尔曼滤波处理(见文献：duanz,han cli x r 2007sequential nonlinear tracking filter with range-ratemeasurements in spherical coordinates ieee trans.aerospace and electronicsystems 43(1)239-250.)。但是采用以量测值为条件推导的转换误差统计特性，导致滤波增益与量测误差存在一定的相关性，文献(彭瀚，程婷.基于预测信息的量测转换序贯滤波目标跟踪[j].系统工程与电子技术，2019,41(3):549-554)采用了基于预测信息的量测转换序贯滤波算法，利用嵌套条件法，消除了量测信息与状态估计之间的相关性，进一步提高滤波算法的跟踪精度。

4、然而当雷达测距精度较高，测角精度较低时，笛卡尔坐标系下量测误差的统计特性与假设的高斯分布相悖(见文献：k.romeo,p.willett and y.bar-shalom,"particlefilter tracking for banana and contact lens problems,"in ieee transactions onaerospace and electronic systems,vol.51,no.2,pp.1098-1110,april 2015,doi:10.1109/taes.2014.130670.)。尽管此文献提出了一种改进的pf算法来解决非高斯问题，但由于pf需要进行大量粒子的采样和筛选过程，导致此算法计算较复杂且跟踪实时性较低。同时当此高斯假设违背时，量测转换方法的跟踪性能就会急剧下降，甚至出现滤波发散等情况。因此，针对上述问题，本发明提出一种基于中间过渡状态的卡尔曼滤波算法，将径向速度量测考虑其中，预测和迭代过程在笛卡尔坐标系下进行，而量测更新过程基于中间过渡状态在量测坐标系下进行，本发明算法解决了量测非线性的问题，同时提高了滤波跟踪精度和在量测非线性程度较高时滤波算法的稳定性。

技术实现思路

1、假设在k-1时刻,已经获得目标的状态估计和估计误差协方差矩阵分别为和pk-1|k-1，其中和分别表示系统对目标在k-1时刻沿x,y,z方向的位置估计，和为所对应方向的速度估计。假设在k时刻获得目标的量测向量为其中和分别为距离、俯仰角、方位角和径向速度量测，量测噪声方差分别为和并假设方位角、俯仰角量测噪声和径向速度量测噪声相互独立，距离和径向速度量测噪声之间的相关系数为ρ，则一种基于中间过渡状态的卡尔曼滤波算法从k-1到k时刻的一次迭代过程具体步骤如下：

2、步骤1：时间更新：

3、(1)对k时刻的状态进行预测：

4、

5、式中，f为状态转移矩阵，为k时刻的状态预测向量。

6、(2)计算预测误差协方差矩阵：

7、pk|k-1＝fpk-1|k-1ft+qk-1     (2)

8、式中，qk-1为k-1时刻的过程噪声协方差矩阵，pk|k-1为预测误差协方差矩阵。

9、步骤2：状态转换：

10、(1)对预测估计误差协方差矩阵进行分解：

11、sk|k-1＝chol(pk|k-1)       (3)

12、式中，sk|k-1为预测误差协方差矩阵的平方根，chol(·)为对矩阵进行cholesky分解。

13、(2)构造等权重采样点：

14、

15、式中，为k时刻所构造的预测采样点，m为采样点数，满足m＝2n，n为状态向量的维数，[1]i为点集[1]的第i列，点集[1]为：

16、

17、(3)将预测采样点表示为其中和为沿x,y和z方向的位置预测分量，和为对应方向的速度预测分量，则将其转换到量测坐标系下：

18、

19、

20、

21、

22、

23、

24、式中，和分别为距离、俯仰角和方位角预测分量，和为径向距离、俯仰角速度和方位角速度分量，将这些分量用转换后在量测坐标系下的中间过渡状态预测采样点表示

25、(4)在量测坐标系下，计算中间过渡状态的预测值：

26、

27、式中，为量测坐标系下中间过渡状态的预测值。

28、(5)计算量测坐标系下的预测误差协方差矩阵：

29、

30、式中，为量测坐标系下的预测估计误差协方差矩阵。

31、步骤3：量测更新：

32、(1)新息协方差矩阵：

33、

34、式中，为新息协方差矩阵，hmid和rk分别为量测矩阵和量测噪声协方差矩阵，可以表示为：

35、

36、

37、(2)计算卡尔曼增益：

38、

39、式中，kmid为卡尔曼增益矩阵。

40、(3)计算在量测坐标系下对中间过渡状态的估计向量：

41、

42、式中，为k时刻的中间过渡状态估计向量。

43、(4)计算量测坐标系下的估计误差协方差矩阵：

44、

45、步骤4：状态转换：

46、(1)对量测坐标系下的估计误差协方差矩阵进行分解：

47、

48、式中，为误差协方差矩阵的平方根。

49、(2)构造等权重采样点：

50、

51、式中，为k时刻所构造的估计采样点。

52、(3)将估计采样点表示为其中和为距离、俯仰角和方位角估计分量，和为径向距离、俯仰角速度和方位角速度分量，则将其转换到原笛卡尔坐标系下：

53、

54、

55、

56、

57、

58、

59、式中，和为沿x,y和z方向的位置估计分量，和为对应方向的速度估计分量，将这些分量用原笛卡尔坐标系下的状态估计采样点表示

60、(4)计算k时刻的状态估计向量：

61、

62、式中，为k时刻的状态估计。

63、(5)计算估计误差协方差矩阵：

64、

65、式中，pk|k为k时刻的估计误差协方差矩阵。

66、发明原理

67、在目标跟踪过程中，通常假设量测噪声是服从零均值的高斯分布的，然而量测在经过从量测坐标系到直角坐标系的转换后，噪声不再服从高斯分布。而量测相对于状态的非线性程度可以由γ来衡量(见文献：d.lerro and y.bar-shalom,"tracking withdebiased consistent converted measurements versus ekf,"in ieee transactionson aerospace and electronic systems,vol.29,no.3,pp.1015-1022,july 1993,doi:10.1109/7.220948.)，非线性度量指标γ表示为

68、

69、如图1(a)所示，当γ较小时，量测非线性程度较小，当转换到笛卡尔坐标系下时，其噪声分布相对符合高斯分布。如图1(b)所示，而当γ较大时，量测的非线性程度较大，转换到笛卡尔坐标系后的量测不再服从高斯分布。因此，当量测非线性较小时，采用传统的非线性滤波和量测转换方法可以有效对目标进行跟踪；而当量测非线性程度较大时，基于假设零均值高斯量测噪声的滤波方法失效。基于以上问题，本发明提出一种基于中间过渡状态的卡尔曼滤波算法。

70、基于中间过渡状态的卡尔曼滤波算法(mid-kf)是一种基于采样点的非线性滤波算法。mid-kf时间更新在原坐标系下进行，量测更新利用状态转换在量测坐标系下进行，避免了非线性量测的线性化带来的误差，同时不需要对复杂的量测转换误差统计特性进行推导，既提高了跟踪精度，同时降低了滤波算法的复杂度。

71、mid-kf算法首先进行时间更新，得到预测状态以及预测误差协方差矩阵，如步骤1所示。由于目标运动状态在笛卡尔坐标系下进行建模，然而量测在球坐标系下获取，因此量测非线性问题存在。为了解决这一问题，同时保持量测噪声的高斯分布，mid-kf算法通过在量测坐标系下建立一个中间过渡状态来对量测进行更新。若设目标运动的状态为：

72、

73、构造中间过渡状态为：

74、

75、由上式可知，中间过渡状态xmid和量测z是线性关系，量测矩阵如式(15)所示。因此，在中间过渡状态下的滤波是线性滤波。状态x到过渡状态xmid的转换关系为：

76、

77、

78、

79、

80、

81、

82、于是，得到步骤2中的式(6)。

83、状态转换完成后，在量测坐标系下进行量测更新，如步骤3所示。由于量测与过渡状态之间不存在非线性关系，且量测噪声符合假设的高斯分布，所以此过程是标准的卡尔曼滤波更新过程。因此，在此状态下得到对过渡状态的估计是最优线性估计。在得到过渡状态xmid的最优估计后，需要将此过渡状态再次转换到笛卡尔坐标系下，以进行下一次的迭代运算。中间过渡状态xmid原状态x的转换关系为：

84、x＝rcosβcosθ  (39)

85、

86、y＝rcosβsinθ  (41)

87、

88、z＝rsinβ  (43)

89、

90、于是，得到步骤4中的式(22)。

91、在转换后得到原坐标系下目标状态的最优估计和估计误差协方差矩阵，如步骤4中的式(28)和式(29)所示。