基于双渐消因子调节的自适应卡尔曼滤波器

朱文超， 何 飞

（1．中国电子科技集团公司 第三十八研究所，安徽 合肥 230041；2．中国科学技术大学，安徽 合肥 230027；3．中国科学院 合肥智能机械研究所，安徽 合肥 230031）

0 引言

在已知系统和量测数学模型、噪声统计特性、状态初值的情况下，卡尔曼（Kalman）滤波器可利用输出信号的量测数据获取状态变量的估计值，具有计算量小，可递推、实时性高等特点［1］。然而，标准Kalman滤波器过分依赖于系统状态空间模型的准确性，当状态参数发生扰动时，往往无法精确跟踪系统突变状态，甚至会引起滤波发散现象［2］。针对这一问题，文献［3-4］在预测协方差中引入单渐消因子，通过Sage-Husa估计法重新解算量测残差，增强新息在状态估计中的权重。文献［5］通过强跟踪中心查分滤波器实时估计汽车前后轴等效时变侧偏刚度，对标称模型的不精确与状态扰动等因素具有强鲁棒性。文献［6-7］在强跟踪滤波器中引入了限定记忆的思想，通过次优渐消因子修正滤波增益，增强跟踪突变状态的能力。文献［8-9］将强跟踪滤波器和UKF滤波器相融合，引入时变渐消因子调节卡尔曼增益，强迫输出残差序列保持正交，提取残差有效信息，提升突变状态的跟踪能力。文献［10］在预测协方差中融合弱化因子与渐消因子，依据模糊控制理论，膨胀状态突变下的量测新息，提升跟踪精度，已成功应用于GPS定位解算领域。

然而，上述研究均未对状态突变程度进行等级划分，致使收敛速度慢。针对这一问题，本文基于滤波发散判据，分析Sage窗量测新息与标准Kalman量测新息的关系，以储备系数为等级，判断状态突变程度，利用双渐消因子调整预测协方差，增权量测新息，提升突变状态的估计精度。

1 Kalman滤波简介

1.1 线性离散系统状态空间模型

k历元下，线性离散时间系统状态方程和量测方程可表示为

式中，Xk为m 维状态向量；Φk，k-1为m×m 维k-1历元至k历元的状态转移矩阵；Uk-1为p 维输入控制量；Γk，k-1为m×p 维控制输入系数矩阵；Wk-1为q维系统过程噪声序列；Ψk，k-1为m×q维过程噪声系数矩阵。Zk为j 维量测向量；Hk为j×m 维量测系数矩阵；Vk为j 维系统量测噪声序列。

1.2 Kalman滤波鲁棒性

Kalman滤波是一种线性、无偏且误差方差最小的随机系统最优估计算法［11］，但鲁棒性较差，无法精准跟踪系统突变状态。究其原因，主要有2点：①状态扰动下的先验预测偏差较大；②稳态Kalman滤波器的增益为定量，状态突变时，无法实时增权量测新息。假设状态扰动（系统控制方式改变、噪声特性变化等）于k历元加载至稳态系统，则突变信息将直接作用于状态方程中的系数矩阵，如扰动方程（3）中ΔΦk，k-1、ΔΓk，k-1、ΔΨk，k-1所示。此时，若仍依靠旧状态方程进行先验预测，必然产生较大的偏差。然而，稳态Kalman的滤波增益无法实时增权量测新息，则最优估计将逐渐偏离真实值，最终致使滤波器发散。

精确跟踪系统突变状态，避免滤波器发散的方法有2种：①分析状态扰动的统计特性，建立精确的数学模型，获取准确先验预测。②寻找激活稳态滤波增益方法，当状态扰动来临时，实时增权测量新息。

然而，状态扰动种类繁多，统计特性复杂，难以准确建立数学模型，故本文针对不同程度的突变状态，利用双渐消因子优化预测协方差，激活稳态滤波增益，采用实时调整量测新息在最优估计中权重的策略，精确跟踪系统突变状态。

2 自适应Kalman滤波

2.1 递推规则设计

鉴于线性离散系统状态空间模型，在Kalman递推规则中，引入双渐消因子，实时调节先验预测协方差，获取自适应Kalman滤波迭代公式。

状态预测：

状态一步预测

一步预测协方差

观测更新：

量测新息

滤波增益

最优估计

后验协方差

式中，Qk-1、Rk分别为过程噪声Wk-1、量测噪声Vk的协方差矩阵。

2.2 双渐消因子的求解

最优Kalman滤波具备量测新息处处正交的特性，新息序列Y（k）的正交方程可用自相关函数表示为［12］

式中，Ck为量测新息协方差矩阵，上标opt代表最优矩阵。

依据Kalman滤波准则，化简式（11）为

若系统状态未发生突变，则标准Kalman滤波器的预测协方差Pbasek，k-1可满足正交方程

式中，上标base代表标准Kalman滤波器产生的协方差矩阵。

然而，随着时间的推移，Kalman滤波器逐渐进入稳态，滤波增益及后验误差协方差的取值固定。倘若此时系统状态发生突变，需激活滤波增益，利用优化预测协方差满足正交方程，则式（12）可化为

根据Sage开窗估计法［13］，由量测新息获取Coptk在k 历元下的估计值Ck，即

联立式（13）、式（15）、式（16）进行矩阵的迹运算，获取λk的函数解析式

需要说明的是，更新λk+1时，需将k历元下的后验方差作为Pkbase，代入式（13）与式（14），求解Ckba+se1，判断其与的关系，构造边界条件，再进入k+1历元的迭代估计阶段。

综合式（6）、式（14）、式（18）进行矩阵的迹运算，得到γk函数解析式

综合式（17）～式（19），获得因子λk与γk的终解，即

3 试验验证

本节以中科院智能机械研究所自行研制的双E 型弹性体六维力传感器为静态标定对象，研究标准Kalman滤波器（SKF）、抗差Kalman滤波器［15］（RKF）、自适应Kalman滤波器（AKF）的鲁棒特性。

六维力传感器标定实验台如图1所示。依次从Fz方向标定数据库中抽取3组量测值进行分析。其中，第一组为恒载输出电压（理想值25 m V）；第二组为卸载输出电压（理想值10 m V）；第三组为加载输出电压（理想值60 m V）。假设前47历元，传感器持续进行恒载输出。第48历元时，分别进行不同量级的卸载或加载操作，则系统状态将发生不同程度突变，理想趋势如图2所示。状态突变原因为传感器的控制载荷发生变化，故k历元下的扰动方程（3）可化为式（21），分别利用SKF、RKF、AKF对输出信号的量测数据进行滤波处理，结果如图3～图10所示。

图2 传感器阶跃输出信号

图3～图5反映了传感器输出由恒载向卸载转变时，3种算法的滤波效果。从图4中可以看出，前47历元，即系统状态未突变前，量测新息序列满足正交性要求，故AKF与RKF均退化为SKF。3种算法状态收敛速度与精度均相同。

图3 卸载前后3种算法跟踪效果

图4 卸载前3种算法跟踪效果

然而，当系统状态在第48历元发生突变时，如图3所示，SKF算法未更新滤波增益，逐渐偏离真实状态，估计误差无限增大。反观AKF与RKF，两者均能有效地跟踪系统突变状态。然而，由于状态突变程度较浅，AKF退化为RKF，两者滤波精度相同，如图5所示。

图6反映了状态突变前后渐消因子的时变趋势。不难发现，因子γk处于冷却状态，AKF仅用因子λk进行状态调节。λk的时变趋势分为5个阶段。

（1）阶段1（47历元至48历元）。状态于此阶段突变，突变信息作用于量测新息协方差，致使估计值骤然变大，λk由冷却骤变为激活态。

（2）阶段2（48历元至49历元）。该阶段的先验估计涵盖了量测新息，其效果优于阶段1。然而，依据图5所示，跟踪初期的后验误差大于突变前的稳态误差。故减小，Ckbase增大，λk处于急速下降状态。

（3）阶段3（49历元至71历元）。该阶段状态后验误差开始减小，估计精度开始提升，则与Ckbase均开始减小，λk的下降速率减缓。

（4）阶段4（71历元至76历元）。该阶段状态后验误差缓慢减小，估计精度缓慢提升，则与Ckbase均缓慢减小，λk的下降速率进一步减缓。

（5）阶段5（76历元至100历元）。该阶段后验误差稳定，则λk仅在稳态值范围进行微调。

图5 卸载后3种算法跟踪效果

图6 卸载后AKF渐消因子时变趋势

图7～图8反映了传感器输出由恒载向加载转变时，3种算法的滤波效果。当系统状态在第48历元发生突变时，SKF滤波发散。RKF仅利用因子λk调节滤波增益，增权量测新息，能够有效跟踪突变状态。然而，AKF 的总体滤波效果优于RKF。尤其在第72 历元至第100 历元阶段，AKF 状态收敛速度优于RKF。

图7 加载前后3种算法跟踪效果

图8 加载后3种算法跟踪效果

从图9～图10可以看出，状态突变后，AKF持续激活双渐消因子λk与γk，能够更深层次地利用量测新息纠正估计值。加载环境下，γk的时变趋势分为4个阶段；λk的时变趋势分为6个阶段。

图9 加载后AKF因子λk 时变趋势

图10 加载后AKF因子γk 时变趋势

γk的时变趋势解析：

（1）阶段1（47 历元至48 历元）。状态于此阶段突变，突变信息体现于量测数据，致使后验协方差PLSW48骤然变大，γk由冷却骤变为激活态。

（2）阶段2（48历元至49历元）。该阶段的先验估计涵盖了量测新息，先验误差Pbase48，48的精度回升，则相较于阶段1，γk处于急速下降状态。

（3）阶段3（49历元至72历元）。该阶段后验误差开始减小，先验估计精度稳步提升，则相较于阶段2，γk的下降速率减缓。

（4）阶段4（72历元至100历元）。该阶段状态后验误差继续减小，第72历元与Cbkase之间的关系到达临界条件，γk由激活变为冷却态。

λk的时变趋势解析：

（1）阶段1、阶段2、阶段3的趋势分析同卸载环境λk。

（2）阶段4（72历元至81历元）。该阶段γk处于冷却状态，则Ckbase的衰减速率变慢。相较于阶段3，λk的下降速率加快。

（3）阶段5（81历元至94历元）。该阶段后验误差缓慢减小，估计精度缓慢提升与Cbkase均缓慢减小，相较于阶段4，λk的下降速率有所减缓。

（4）阶段6（94历元至100历元）。该阶段后验误差稳定，则λk仅在稳态值范围进行微调。

通过式（22）解算72历元至第100历元3种算法估计结果的均方根误差X~kRMSE，详见表1。结果显示，SKF误差较大，处于发散状态。相较于RKF，AKF稳态精度提升了44．76%。

表1 滤波算法性能对比

4 结论

为解决标准Kalman滤波无法实时精确估计突变状态的问题，设计了双渐消因子自适应Kalman滤波器。面对不同程度的状态突变，采用合适的渐消因子组合，实时调节预测协方差，变权量测新息。实验结果表明，自适应Kalman滤波器，具有较强的鲁棒性，稳态精度优于抗差Kalman滤波，可运用于六维力传感器静态标定领域。然而，本文仅研究标量渐消因子，对于多维度渐消因子的求解方法，还有待于进一步深入研究。