李春辉，马健，杨永建，甘轶

（空军工程大学航空工程学院，西安 710038）

确定采样型滤波算法估计精度高，实现简单，是近年来非线性系统状态估计领域的一个研究热点，并广泛应用于目标跟踪、金融、信号处理和导航等领域［1］。确定采样型滤波算法包括无迹卡尔曼滤波（unscented Kalman filter，UKF）［2］、中心差分卡尔曼滤波（central difference Kalman filter，CDKF）［3］及容积卡尔曼滤波（cubature Kalman filter，CKF）［4-5］，理论上已经证明，这些算法能至少以二阶泰勒精度逼近各种非线性系统的后验协方差和均值［6］，因此不仅具有比扩展卡尔曼滤波（extended Kalman filter，EKF）更高的滤波精度，还能克服EKF需要计算雅可比矩阵、强非线性时较差的数值稳定性等不足。相比于粒子滤波（particle filter，PF），确定采样型滤波在计算量上具有很大优势，而且不会出现 PF粒子退化和贫化问题［1］。

Arasaratnam和Haykin［4-5］利用球面径向容积准则逼近最优估计的状态后验分布而提出的CKF算法，不仅能克服UKF在高维度和强非线性系统中的应用局限，同时具备更好的滤波精度及数值稳定性［7］。

在降低CKF算法复杂度方面，文献［20］提出了一种简化CKF算法，但是该算法对克服建模不确定性的提高并不大。文献［17］将STF与假设检验理论结合以检测不确定性，在运行时间上有很大优化，但是求解渐消因子的计算量仍然不可避免。

为了能使CKF算法具有对目标模型不匹配及状态突变时的鲁棒性，同时能够避免强跟踪CKF算法较大的计算量，提出了一种低复杂度自适应CKF算法。首先，设立了基于新息的自适应修正判决准则及修正方式，然后，对系统状态预测值进行基于新息的直接补偿修正，使得滤波算法能够及时“跟上”目标真实状态，进而减小滤波误差。本文算法具有良好的滤波性能，同时在算法复杂度和仿真运行时间上有明显改善。

1 容积卡尔曼滤波

对目标建模时考虑如下非线性离散加性噪声系统：

式中：f和h分别为非线性的状态转移函数和量测函数；xk+1∈Rnx和yk+1∈Rny分别为系统状态向量和量测向量；wk和vk+1分别为系统过程噪声和量测噪声，两者是相互独立的零均值白噪声且对应的协方差矩阵分别为Qk和Rk。系统初始状态为x0～N（0，P0），与wk、vk+1互不相关，P0为初始协方差矩阵。

CKF算法采用球面径向容积规则来逼近最优框架中的状态后验均值和后验协方差，根据文献［5，11］，将CKF算法流程总结如下。

1.1 时间更新

1.2 量测更新

2 自适应修正容积卡尔曼滤波

2.1 误差分析

2.2 基于新息的自适应修正判决准则和修正方式

由2.1节的分析可知，当目标模型不匹配或者目标状态发生突变时，新息ek+1能反映系统状态预测值和量测预测值出现的较大偏差，同时也能反映出预测值偏离实际状态的“趋势”和“程度”。因此，基于新息来判决是否修正及如何修正。

图1 基于新息的修正判决准则Fig.1 Amending judgment criteria based on innovation sequence

修正系数的选取首先要遵循避免过度修正基本原则，其次主要考虑两方面：一是修正系数与系统量测精度相关；二是新息ek+1所提供的信息维度，如假设新息中只含有位置的信息，则在选取时c1或c3可以选取得稍大些，速度的修正可以通过位置变化来传递，但是对应的系数c2或c4选取得稍小些。

2.3 低复杂度自适应CKF算法实现

根据建立的判决准则和修正方式总结了本文算法的流程，如图2所示。图中在判别之后的虚线是低复杂度自适应CKF算法的路径，实线是标准CKF算法的路径，具体步骤总结如下：

图2 低复杂度自适应CKF算法流程Fig.2 Flowchart of low-complexity adaptive CKF algorithm

步骤1 通过式（2）～式（12）完成时间更新及量测更新的一部分，并得到新息ek+1。

步骤2 将新息中对应变量的绝对值与门限进行比较，若满足修正判决条件，则用式（18）进行状态预测值修正，之后用式（9）～式（17）完成滤波，并用式（19）进行预测误差协方差补偿。

步骤3 若不需要进行修正，则按式（13）～式（17）完成标准CKF滤波。

步骤4 重复步骤1～步骤3。

3 算法复杂度分析

使用浮点操作数（flops）来分析算法复杂度是直观有效的方法，根据文献［21-22］，定义以下4种矩阵运算的flops：

1）矩阵加减法。A∈Rn×m，B∈Rn×m，A±B运算的复杂度为nm flops。

2）矩阵乘法。A∈Rn×m，B∈Rm×l，AB运算的复杂度为2nm l-nl flops。

3）矩阵求逆。A∈Rn×n，A-1运算的复杂度为n flops。

4）Cholesky分解。A∈Rn×n，chol（A）运算的复杂度为n3／3 flops。

假设系统状态变量为n维，量测变量为l维，则利用上述矩阵运算复杂度定义，对标准CKF算法、强跟踪CKF算法［13］及本文提出的低复杂度自适应CKF算法的复杂度进行计算，结果如表1所示。

表1 三种算法复杂度计算结果Table 1 Computational results of complexity of three algorithms

CKF算法的复杂度分析具体过程可见文献［21］，参照该过程得到了其他2种算法的复杂度计算结果。

强跟踪CKF算法主要考虑文献［13］中式（3）、式（6）、式（10）和式（11）求解渐消因子上，4个公式对应的算法复杂度依次为2 l、2 l2、2n3+6n2l+2 l2n+l2-3nl和2 l2flops。

低复杂度自适应CKF算法主要考虑式（12）新息的计算及式（18）和式（19）的计算，对应步骤的算法复杂度依次为l、n和n2+l flops。

尽管文献［13］中的强跟踪CKF算法已经进行了改进，求解时避免了计算雅可比矩阵，但是相比而言，本文算法在复杂度上仍有明显改善。将强跟踪CKF算法与低复杂度自适应CKF算法的复杂度求差可得Δflops＝n（2n2-n-1）+3nl（2n-1）+2 l2n+5 l2，在n和l都大于1时，这一差值会随着n和l的增大而变大，即随着状态向量、量测向量维度的增加，低复杂度自适应CKF算法的优势会更大。为了更直观地体现这一差异，根据得到的算法复杂度公式绘制了3种算法的复杂度随状态向量、量测向量维度分别变化时的曲线，如图3和图4所示。

图3 三种算法的复杂度比较（l＝8）Fig.3 Comparison of three algorithms complexity（l＝8）

图4 三种算法的复杂度比较（n＝8）Fig.4 Comparison of three algorithms complexity（n＝8）

4 仿真与分析

4.1 建模与仿真条件设置

仿真中，目标运动模型为匀速运动和匀速转弯运动的混合机动。系统状态向量为X＝［x，˙x，y，˙y］T，其中匀速运动模型的状态转移矩阵为FCV，匀速转弯运动模型的状态转移矩阵为FCT，分别如下：

为了验证低复杂度自适应CKF算法在目标建模不匹配时的滤波性能，在跟踪时与CKF算法、强跟踪CKF算法进行对比，同时3种算法在目标建模时都仅使用匀速运动模型FCV。

在目标运动仿真参数的设置上，转弯率ω＝5°／s，采样时间T＝1 s，目标在0～250 s的时间内保持交替变换运动状态，其中匀速直线运动时间段为［0，70］s，（120，145］s，（160，200］s，（225，250］s；匀速转弯运动时间段为（70，120］s，（145，160］s，（200，225］s，共3次转弯。

目标初始状态为x0＝［1 000 m，200 m／s，1 000 m，10 m／s］T，系统初始协方差P0＝diag（3，1，3，1）；系统过程噪声协方差矩阵Q＝diag（1，1，1，1），量测噪声协方差矩阵R＝diag（100，100）；强跟踪CKF算法的遗忘因子取0.95。

仿真结果以目标x轴和y轴位置上的均方根误差（root mean square error，RMSE）来评价，并进行150次蒙特卡罗仿真，均方根误差的定义为

4.2 仿真结果分析

图5为其中一次蒙特卡罗仿真的目标真实运动轨迹及3种算法的滤波轨迹。可以看到，低复杂度自适应CKF算法及强跟踪CKF算法均能很好地跟踪目标，但是由于滤波建模时使用的始终是匀速运动模型，CKF算法在3次机动转弯时跟踪性能就会下降。

图6和图7分别为滤波阶段x轴和y轴位置的均方根误差，左上角为对10～20 s阶段放大显示。可以看出，整体上低复杂度自适应CKF算法及强跟踪CKF算法能够在建模不准确时始终保持较高的滤波精度，但是CKF算法只有在目标匀速运动阶段即建模匹配时才能发挥很好的滤波性能，在目标转弯阶段时其滤波精度受到很大影响。但是从图6和图7中放大显示部分来看，CKF算法在目标运动建模准确时滤波精度要比另外2种算法要高。

表2为3种算法在滤波时间段里的平均均方根误差。结合其中的数据及图6和图7可以看出，低复杂度自适应CKF算法能够保持和强跟踪CKF算法基本一致的滤波精度，可以较好地克服由于目标建模不准确或者目标状态突变带来的不利影响。

图6 x轴位置均方根误差Fig.6 RMSE of position in x axis

图7 y轴位置均方根误差Fig.7 RMSE of position in y axis

表2 不同算法平均均方根误差对比Table 2 Comparison of mean RMSE among different algorithms

在硬件配置为Intel i7 4710处理器和4 GB内存的笔记本电脑上使用MATLAB 2017a进行了试验。3种算法的单次仿真平均运行时间和运行时间增加百分比如表3所示。运行时间增量计算方式为（Ti-T0）／T0，T0为CKF算法的运行时间，Ti为其他2种算法的运行时间。

由表3可见，CKF算法耗时最短，低复杂度自适应CKF算法在运行时间上略微增加了1.29%，而强跟踪CKF算法由于需要求解渐消因子，耗时最多，相比较CKF算法增加了29.53%。运行时间对比数据和前文算法复杂度计算结果相吻合。

表3 不同算法运行时间对比Table 3 Comparison of running time among different algorithms

为了验证低复杂度自适应CKF算法的滤波数值稳定性，绘制3种算法每次蒙特卡罗仿真中x轴和y轴位置的平均均方根误差，如图8和图9所示。可以看出，低复杂度自适应CKF算法及强跟踪CKF算法每次仿真中的位置平均均方根误差都低于CKF算法，这是因为CKF算法在每次仿真中目标建模不匹配即转弯阶段时的滤波误差会剧增。同时相比于CKF算法，另外2种算法的位置平均均方根误差没有出现某次仿真发散的情况，而且曲线波动幅度更小，数值稳定性更优。

图8 x轴位置平均均方根误差Fig.8 Mean RMSE of position in x axis

图9 y轴位置平均均方根误差Fig.9 Mean RMSE of position in y axis

5 结 论

1）仿真结果表明，在目标建模不准确的情况下，低复杂度自适应CKF算法能够保持和强跟踪CKF算法基本一致的滤波精度和数值稳定性，有效提高了CKF算法的自适应性，同时也验证了所提算法的有效性。

2）使用等效浮点操作数进行了算法复杂度计算，低复杂度自适应CKF算法相比较强跟踪CKF算法具有更低的算法复杂度，且随着状态向量、量测向量维度的增加，所提算法的优势会更大。

3）仿真结果表明，在算法运行时间上，相比较CKF算法，强跟踪CKF算法运行时间增加了29.53%，而低复杂度自适应CKF算法仅增加了1.29%，具有明显优势。仿真运行时间结果与复杂度计算结果保持一致。

为了实现相关参数的自动选取，需要进一步考虑与智能寻优算法的结合或者相关理论的推导。