卡尔曼滤波在模型不匹配下的性能分析

1 问题由来

应用卡尔曼滤波(Kalman Filter)的一个基础前提是：准确建立模型

可实际哪能如人所愿，若使用的模型与实际不符，卡尔曼滤波器将会有什么样的性能表现呢？以此问题为切入点，深入了解卡尔曼滤波内部机制。以便后续研究卡尔曼滤波在故障检测，机动目标跟踪中的应用。

2 卡尔曼滤波

2.1 建立方程

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。正如前所说，卡尔曼滤波离不开模型的建立。首先，对系统状态变化建立状态方程$$X_{k+1}=F{X}_k+G{v}_k$$这里$F$为状态转移矩阵，$G$为状态噪声增益矩阵，$v_k$为过程噪声。状态方程里的状态$X_i$是待估计量，我们能得到的是观测数据。观测方程如下$$Z_{k+1}=H{X}_{k+1}+w_{k+1}$$其中，$H$为观测矩阵，$w_{k+1}$为观测噪声。这里假设$v_k$和$w_k$是互不相关的高斯噪声。各自的协方差矩阵为$Q_k$和$R_k$，我们要做的是给定观测序列，对系统状态进行估计。

2.2 滤波过程

具体的卡尔曼滤波推导步骤详见另一篇文章，这里不再赘述。只列出主要步骤：

• Step1：一步状态预测$${\hat{X}}_{k+1|k}=F{\hat{X}}_{k|k}$$
• Step2：一步观测预测$${\hat{Z}}_{k+1|k}=H{\hat{X}}_{k+1|k}$$
• Step3：计算残差$${\tilde{Z}}_{k+1}=Z_{k+1}-{\hat{Z}}_{k+1|k}$$
• Step4：计算增益$$P_{k+1|k}=F{P}_{k|k}{F}^{\prime }+G{Q}_k{G}^{\prime }$$$$S_{k+1}=H{P}_{k+1|k}{H}^{\prime }+R_{k+1}$$$$K_{k+1}=P_{k+1|k}{H}^{\prime }{S}_{k+1}^{-1}$$
• Step5：更新状态估计$${\hat{X}}_{k+1|k+1}={\hat{X}}_{k+1|k}+K_{k+1}{\tilde{Z}}_{k+1}$$
• Step6：更新协方差$$P_{k+1|k+1}=P_{k+1|k}-K_{k+1}H{P}_{k+1|k}$$
  卡尔曼滤波公式很容易查到，为什么我要花大篇幅写下来呢？自然有我的用意，在这先埋个伏笔！

3 模型不匹配下性能

在仿真之前，可能从书本上或者老师告诉过你结果

“哦，卡尔曼滤波在模型不吻合情况下会出现滤波发散”
这个结论是正确的。 但为什么？为什么模型不匹配会造成滤波发散？ 停下来思考下，如果没有想出发散理由和解决办法，我很高兴，因为这也是我写这篇笔记的初衷。

3.1 仿真实验场景设置

我们先来看看，模型不匹配情况下卡尔曼滤波到底是什么结果吧。首先，给出一个简单的仿真场景：一辆汽车在笔直公路上行驶，一个静止雷达测距仪在汽车后方每隔一秒测下汽车位置，测距仪的测量标准差为1米。 现在汽车先是匀速运动一段时间（$13s$），某一时刻司机察觉到后方的交警，猛加油门离开（ 加速度为 $5m/s^2$ 持续$2s$）。运动轨迹如图所示

可以看到，在整个过程中汽车不只是匀速运动，若用匀速运动模型的卡尔曼滤波将会有什么结果呢？这里直接放均方根误差曲线图了（$1000$次蒙特卡洛仿真，滤波器使用的状态噪声方差为$10^{-4}{m}^2/s^4$）

从上图可以看到位置和速度的RMSE曲线在模型不匹配时刻突然升高，滤波器出现了发散。OK，到这还没解决这节开头提出的问题，为什么？其实答案早已隐含在前面的内容里了。

3.2 滤波发散原因

让我们回到卡尔曼滤波过程这段，仔细分析下卡尔曼滤波过程。

• Step4：计算增益$$P_{k+1|k}=F{P}_{k|k}{F}^{\prime }+G{Q}_k{G}^{\prime }$$$$S_{k+1}=H{P}_{k+1|k}{H}^{\prime }+R_{k+1}$$$$K_{k+1}=P_{k+1|k}{H}^{\prime }{S}_{k+1}^{-1}$$
• Step6：更新协方差$$P_{k+1|k+1}=P_{k+1|k}-K_{k+1}H{P}_{k+1|k}$$

仔细观察这两步骤，状态协方差和增益的计算是与观测无关的。换句话说，在我得到量测数据前，我就能算出各个时刻状态协方差和增益的大小。且增益是随时间减小的。由此可以得出结论：

实际模型发生变化的这一信息包含在量测数据里，然而，量测数据在滤波过程中起到的作用是越来越小的。也就是，估计过程会更加相信一步预测（模型），最糟糕的是模型不匹配。错误的预测值搭配离线的增益，最终导致滤波的发散。

位置分量的卡尔曼增益如图所示，随着估计过程进行而逐渐降低，与量测数据的到来以及大小没关系。不管模型是否发生了改变，哎，增益就这样任性，就是玩！

3.3 仿真实验验证猜想

猜想1： 如前所说，滤波增益随着时间减小。若模型不匹配提早发生，量测数据说话的份量就会大点（增益大点）。那么模型不匹配时候，状态估计的误差应该会相较后发生模型改变情况减小。

基于上述这一观点，修改仿真实验场景，让汽车加速提早$1s$到来。也就是汽车匀速运动$12s$，加速运动$3s$。其余仿真参数不变

观察上图中蓝色曲线上X13和红色曲线上的X14数据点值，提早$1s$发生模型变化时，模型不匹配估计误差会降低，验证了我们这个猜想。

---

猜想2： 如果增大滤波时候使用的状态噪声，使模型的不确定性增加，滤波器增益不会太偏向于模型。那么模型改变时候，估计的误差会有所降低。

量测噪声大家都知道，用来描述测量数据仪器或设备的精确度，比如尺子刻度的准确性等等。可是状态噪声是个什么东西？先来谈谈我对状态噪声的理解吧：状态噪声是用来衡量对模型的相信度。 如果我对模型很自信，现实中就应该严格按照所建立的模型来，那就可以把它设置成很小的数值。如果想给自己模型增添包容性，那就增大状态噪声。OK，这部分了解了，该设置实验验证了。

前面仿真实验滤波器使用的状态噪声方差为$10^{-4}{m}^2/s^4$，现在增大状态噪声$10^{-1}{m}^2/s^4$，其余参数设置不变

仿真图例中QS表示小的状态噪声，QL表示较大的状态噪声。可以清晰看到，使用较大的状态噪声，在模型发生改变时候，估计的误差增长得以一定的缓解。但是，在汽车做匀速运动时候，估计的效果并不佳。这点不难理解，增大状态噪声导致增益相对抬高。

4. 动态滤波过程

前面详细分析卡尔曼滤波在模型不匹配时的表现。这一部分，将用更形象的概率密度图的形式，动态展示这一过程。

选择用概率密度的形式，是因为个人认为，从概率密度函数的角度能更好的理解卡尔曼估计过程。为什么这样说，卡尔曼滤波是线性系统高斯分布下的最优估计。回顾卡尔曼滤波过程，估计可以分为两大部分。离线计算协方差（分布的方差）和增益，在线更新状态估计（分布的均值）。均值和方差可由概率密度函数体现，高斯分布由一二阶矩表示，所以从概率密度函数角度能更好理解卡尔曼滤波过程。

4.1 仿真实验

在此仿真实验设置，一辆汽车在笔直公路上行驶，一个静止雷达测距仪在汽车后方每隔一秒测下汽车位置，测距仪的测量标准差为1米。汽车以$5m/s^2$的加速度行驶。现用匀速运动模型对其进行滤波，进行$1000$次蒙特卡洛仿真，观察每一时刻下状态预测的概率密度函数

因为滤波模型是不准确的，状态预测与量测的概率密度函数距离会越来越大。如果使用大的状态噪声进行滤波，这种差距会有所减小。

对比两种参数设置下的滤波结果

在高状态噪声情况下，滤波结果的概率密度函数更接近量测，但仍是有偏估计。
​