卡尔曼滤波详解（1）

目录

1. 核心思想

2. 五个公式的解读

2.1 预测部分

2.2 更新部分

3. 公式的实际应用

4. 调参方法

---

1. 核心思想

首先，卡尔曼滤波器可以用来估计系统的状态，这个状态是时间序列上的，利用上一时刻的状态可以预测当前时刻的状态，利用当前时刻的观测可以更新和修正当前时刻的预测。这么说可能有点绕，看下图。

绿色的x表示系统的状态，y表示对系统状态的观测，蓝色的x表示修正后的状态。卡尔曼滤波的核心思想，就是用利用蓝色进行预测，得到下一时刻绿色的。在利用下一时刻的观测和预测得到的x2得到下一时刻修正后的状态，即蓝色的。

由上面的过程可以发现，卡尔曼滤波由一个明显的特点，即当前时刻的状态，只和上一时刻的状态有关，这叫做马尔可夫性。对于实际应用来说，内存中只需要存储上一时刻的状态，占用内存小，速度快，对于实时问题有很好的响应。

2. 五个公式的解读

下面来直接解读五个公式

2.1 预测部分

首先是预测部分，预测就是根据当前上一时刻的融合状态，推测当前时刻的状态，而这个状态不能完全代表当前时刻的最终状态，所以看到预测部分的两个公式都是带了“-”号的。

：表示t时刻系统的状态

F：表示状态转移矩阵，即如何从变换到，通常和理论公式有关。

u: t 时刻外界对系统的作用 （现在不理解没关系，实际应用中会具象化）

B：控制矩阵，将外界的作用转化为对状态的影响 

P: 系统的不确定度，这个非常的重要，以协方差矩阵的形式展现，也可以表征状态量之间的关系，随着卡尔曼过程的迭代，这个P应该逐渐减小。

Q：预测过程增加进系统的不确定度，可以视作从上一时刻得到当前时刻状态量的可靠程度，也以协方差的形式呈现，有时也将外部的影响放入Q中一起考虑。

对于预测部分，第一个公式也有在最后加上一个噪声的，如果有些噪声可以被表示清楚，写进公式可以增加预测结果的可信程度，Q也就随之减小（我个人的理解）。

第二个公式关于系统不确定度P，一定要认识到其实以协方差矩阵的形式展现的，能够表征系统各状态量之间的关系。同时最重要的是P是表征整个系统的不确定度的，所以随着迭代的进行，P也是要更新的，这就是有第二个公式的原因。而这个公式的来源就是将上一时刻的状态预测为当前时刻，用协方差的数学特性也变换到当前时刻，再加上这么变换的误差噪声Q。

2.2 更新部分

更新部分的3个公式，卡尔曼增益K是一个过程量，最终会得到修正后的状态量x及系统的不确定度P。这其中

H：是变换矩阵，表示从观测值到状态量之间的关系，因为有些状态量是无法直接观测的

Z：是观测值

R:  是观测的噪声，也是以协方差的形式表示的

K：卡尔曼增益，这个增益其实就是为了表达简便，认为定义的一个公式。

更新部分的三个公式来源，实际上是由两个正态分布的公式相乘的结果得到的。根据预测部分的结果，我们得到了预测的当前时刻的状态量，这个状态量其实服从高斯分布。同时，在当前时刻，我们有当前时刻的观测量，这个量也服从高斯分布。更新部分的主要操作，就是将这两个高斯分布的量结合，得到一个方差更小的高斯分布。图示如下（这个图是来自两个高斯分布乘积的理论推导_两个高斯分布相乘-CSDN博客

讲两个高斯分布如何融合起来，讲的很好）

而这三个公式，实际上就是两个高斯分布相乘得到新高斯分布的公式套用：

简化一下表达，定义K为：

则公式可以写为：

再对应到上面的更新部分的三个公式，是不是如出一辙。

3. 公式的实际应用

现在我们假设一个物体匀加速直线运动，状态量X是[x, y, vx, vy],由于是匀加速直线运动，我们很容易得到前一时刻各个状态量和当前时刻各个状态量之间的关系，y方向以此类推。

写成矩阵的形式为：

x       =         F                              x           +          B           u

可以对照预测部分的公式看下实际对应的矩阵是什么

现在我们假设观测用的传感器是激光雷达，从激光雷达中能直接读到距离，即x,y，无法直接读到速度。我们可以设置初始的系统不确定度为

这里因为无法直接得到速度，所以速度项的不确定度要高，而位置项的不确定度低。u自胡总随着迭代过程的进行，整个系统的不确定度会降低。当然这里的初值设定还可以继续深入讨论。

再看，由于观测值是2维的，状态量是4维，所以需要变换矩阵H将状态量变化为2维，则

以上的过程，在给定Q和R之后，就可以进行迭代计算了。

4. 调参方法

由上面的应用实例可以看出，整个过程可以调整的参数，有初始的系统不确定度P，过程噪声Q，测量噪声R。而参数调整关心的几个问题是，收敛的速度，结果的准确程度或者平滑程度（更相信观测还是预测）。有几个比较定性的结论：

Q越大，表示更不相信预测，结果波动会较大，滤波效果差。

R越大，表示更不相信观测，结果会越平滑，滤波效果好，卡尔曼滤波输出收敛的越慢。

卡尔曼增益的收敛值为 Q/(Q+R)，比如其值为0.5，那么卡尔曼增益会向0.5收敛（比如预测的位置为10m，观测的位置为15m，则修正后的优化位置为：10+0.5*（15-10）=12.5m。增益就是观测和预测差值的加权，增益越大，越相信观测值。

P0/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢。

后来还看到了一些有趣的调参方法，比如：

自适应滤波，在Q和R的基础上，加入随时间变化的因子。

离线学习，一般的卡尔曼滤波是利用过去和现在的测量数据来估计将来的状态。也可以将时间序列上的数据全部采集，即已经获得所有（包括未来）测量数据的情况下，估计过去时间下的系统状态。