目录
1 问题由来
应用卡尔曼滤波(Kalman Filter)的一个基础前提是：准确建立模型
可实际哪能如人所愿，若使用的模型与实际不符，卡尔曼滤波器将会有什么样的性能表现呢？以此问题为切入点，深入了解卡尔曼滤波内部机制。以便后续研究卡尔曼滤波在故障检测，机动目标跟踪中的应用。
2 卡尔曼滤波
2.1 建立方程
卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。正如前所说，卡尔曼滤波离不开模型的建立。首先，对系统状态变化建立状态方程 X k + 1 = F X k + G v k X_{k+1}=FX_{k}+Gv_{k} Xk+1=FXk+Gvk这里 F F F为状态转移矩阵， G G G为状态噪声增益矩阵， v k v_{k} vk为过程噪声。状态方程里的状态 X i X_{i} Xi是待估计量，我们能得到的是观测数据。观测方程如下 Z k + 1 = H X k + 1 + w k + 1 Z_{k+1}=HX_{k+1}+w_{k+1} Zk+1=HXk+1+wk+1其中， H H H为观测矩阵， w k + 1 w_{k+1} wk+1为观测噪声。这里假设 v k v_{k} vk和 w k w_{k} wk是互不相关的高斯噪声。各自的协方差矩阵为 Q k Q_{k} Qk和 R k R_k Rk，我们要做的是给定观测序列，对系统状态进行估计。
2.2 滤波过程
具体的卡尔曼滤波推导步骤详见另一篇文章，这里不再赘述。只列出主要步骤：
- Step1：一步状态预测 X ^ k + 1 ∣ k = F X ^ k ∣ k \hat{X}_{k+1|k}=F\hat{X}_{k|k} X^k+1∣k=FX^k∣k
- Step2：一步观测预测 Z ^ k + 1 ∣ k = H X ^ k + 1 ∣ k \hat{Z}_{k+1|k}=H\hat{X}_{k+1|k} Z^k+1∣k=HX^k+1∣k
- Step3：计算残差 Z ~ k + 1 = Z k + 1 − Z ^ k + 1 ∣ k \tilde{Z}_{k+1}=Z_{k+1}-\hat{Z}_{k+1|k} Z~k+1=Zk+1−Z^k+1∣k
- Step4：计算增益 P k + 1 ∣ k = F P k ∣ k F ′ + G Q k G ′ P_{k+1|k}=FP_{k|k}F'+GQ_{k}G' Pk+1∣k=FPk∣kF′+GQkG′ S k + 1 = H P k + 1 ∣ k H ′ + R k + 1 S_{k+1}=HP_{k+1|k}H'+R_{k+1} Sk+1=HPk+1∣kH′+Rk+1 K k + 1 = P k + 1 ∣ k H ′ S k + 1 − 1 K_{k+1}=P_{k+1|k}H'S_{k+1}^{-1} Kk+1=Pk+1∣kH′Sk+1−1
- Step5：更新状态估计 X ^ k + 1 ∣ k + 1 = X ^ k + 1 ∣ k + K k + 1 Z ~ k + 1 \hat{X}_{k+1|k+1}=\hat{X}_{k+1|k}+K_{k+1}\tilde{Z}_{k+1} X^k+1∣k+1=X^k+1∣k+Kk+1Z~k+1
- Step6：更新协方差
P
k
+
1
∣
k
+
1
=
P
k
+
1
∣
k
−
K
k
+
1
H
P
k
+
1
∣
k
P_{k+1|k+1}=P_{k+1|k}-K_{k+1}HP_{k+1|k}
Pk+1∣k+1=Pk+1∣k−Kk+1HPk+1∣k
卡尔曼滤波公式很容易查到，为什么我要花大篇幅写下来呢？自然有我的用意，在这先埋个伏笔！
3 模型不匹配下性能
在仿真之前，可能从书本上或者老师告诉过你结果
这个结论是正确的。 但为什么？为什么模型不匹配会造成滤波发散？ 只有知道发散的原因，才有解决模型不匹配的思路。停下来思考下，如果没有想出发散理由和解决办法，我很高兴，因为当你看完这篇文章时候，你会收获不少，这也是我写这篇笔记的初衷。
3.1 仿真实验场景设置
我们先来看看，模型不匹配情况下卡尔曼滤波到底是什么结果吧。首先，给出一个简单的仿真场景：一辆汽车在笔直公路上行驶，一个静止雷达测距仪在汽车后方每隔一秒测下汽车位置，测距仪的测量标准差为1米。 现在汽车先是匀速运动一段时间（ 13 s 13~s 13 s），某一时刻司机察觉到后方的交警，猛加油门离开（ 加速度为 5 m / s 2 5m/s^2 5m/s2 持续 2 s 2~s 2 s）。运动轨迹如图所示
可以看到，在整个过程中汽车不只是匀速运动，若用匀速运动模型的卡尔曼滤波将会有什么结果呢？这里直接放均方根误差曲线图了（ 1000 1000 1000次蒙特卡洛仿真，滤波器使用的状态噪声方差为 1 0 − 4 m 2 / s 4 10^{-4}m^{2}/s^4 10−4m2/s4）
从上图可以看到位置和速度的RMSE曲线在模型不匹配时刻突然升高，滤波器出现了发散。OK，到这还没解决这节开头提出的问题，为什么？其实答案早已隐含在前面的内容里了。
3.2 滤波发散原因
让我们回到卡尔曼滤波过程这段，仔细分析下卡尔曼滤波过程。
- Step4：计算增益 P k + 1 ∣ k = F P k ∣ k F ′ + G Q k G ′ P_{k+1|k}=FP_{k|k}F'+GQ_{k}G' Pk+1∣k=FPk∣kF′+GQkG′ S k + 1 = H P k + 1 ∣ k H ′ + R k + 1 S_{k+1}=HP_{k+1|k}H'+R_{k+1} Sk+1=HPk+1∣kH′+Rk+1 K k + 1 = P k + 1 ∣ k H ′ S k + 1 − 1 K_{k+1}=P_{k+1|k}H'S_{k+1}^{-1} Kk+1=Pk+1∣kH′Sk+1−1
- Step6：更新协方差 P k + 1 ∣ k + 1 = P k + 1 ∣ k − K k + 1 H P k + 1 ∣ k P_{k+1|k+1}=P_{k+1|k}-K_{k+1}HP_{k+1|k} Pk+1∣k+1=Pk+1∣k−Kk+1HPk+1∣k
仔细观察这两步骤，状态协方差和增益的计算是与观测无关的。换句话说，在我得到量测数据前，我就能算出各个时刻状态协方差和增益的大小。且增益是随时间减小的。由此可以得出结论：
实际模型发生变化的这一信息包含在量测数据里，然而，量测数据在滤波过程中起到的作用是越来越小的。也就是，估计过程会更加相信一步预测（模型），万万没想到模型是错的，最终导致滤波的发散。
位置的卡尔曼增益如图所示，随着估计过程进行而逐渐降低，与量测数据的到来以及大小没关系。不管模型是否发生了改变，哎，增益就这样任性，就是玩！
3.3 仿真实验验证猜想
猜想1： 如前所说，滤波增益随着时间减小。若模型不匹配提早发生，量测数据说话的份量就会大点（增益大点）。那么模型不匹配时候，状态估计的误差应该会相较后发生模型改变情况减小。
基于上述这一观点，修改仿真实验场景，让汽车加速提早 1 s 1~s 1 s到来。也就是汽车匀速运动 12 s 12~s 12 s，加速运动 3 s 3~s 3 s。其余仿真参数不变
观察上图中蓝色曲线上X13和红色曲线上的X14数据点值，提早 1 s 1~s 1 s发生模型变化时，模型不匹配估计误差会降低，验证了我们这个猜想。
猜想2： 如果增大模型的状态噪声，使模型的不确定性增加，滤波器增益不会太偏向于模型。那么模型改变时候，估计的误差会有所降低。
量测噪声大家都知道，用来描述测量数据仪器或设备的精确度，比如尺子刻度的准确性等等。可是状态噪声是个什么东西？先来谈谈我对这个状态噪声的理解吧：状态噪声是用来衡量对模型的相信度。 如果我对模型很自信，现实中就应该严格按照所建立的模型来，那就可以把它设置成很小的数值。如果想给自己模型增添包容性，那就增大状态噪声。OK，这部分了解了，该设置实验验证了。
前面仿真实验滤波器使用的状态噪声方差为 1 0 − 4 m 2 / s 4 10^{-4}m^{2}/s^4 10−4m2/s4，现在增大状态噪声 1 0 − 1 m 2 / s 4 10^{-1}m^{2}/s^4 10−1m2/s4，其余参数设置不变
仿真图例中QS表示小的状态噪声，QL表示较大的状态噪声。可以清晰看到，使用较大的状态噪声，在模型发生改变时候，估计的误差增长得以一定的缓解。但是，在汽车做匀速运动时候，估计的效果并不佳。这点不难理解，增大状态噪声导致增益相对抬高。
人总是贪婪的，既希望在模型吻合情况下估计误差降低，又希望模型不匹配时估计不是那么糟糕，这个该怎么办呢？这个涉及到后续准备讲的内容了：自适应卡尔曼滤波
标签：仿真,状态,匹配,卡尔曼滤波,噪声,增益,模型 来源： https://blog.csdn.net/buxinlegan/article/details/118031911
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。