轻松理解卡尔曼滤波

作者：很酷的程序员，小米科技，高级算法工程师。

卡尔曼滤波是非常经典的预测追踪算法，能够在系统存在噪声和干扰的情况下进行系统状态的最优估计，广泛使用在导航、制导、控制相关的领域。本文将指引读者轻松地，一步步地深入理解卡尔曼滤波。

笔者此前学习和实现卡尔曼滤波花费了很多时间，其实想要理解其原理并不算很复杂。

只是简单套用卡尔曼滤波的公式，而没有系统理解公式里面每个变量的缘来，不去理解卡尔曼滤波器的迭代过程和原理，在实现和调试系统的时候无疑是会找不着北的。

本文是一篇翻译，只是笔者为了加深理解，重新组织了一些内容，并没有完全按照原文翻译。

原文链接：

https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

一个简单的场景

假设我们开发了一台无人机（假设它的名字是Eva），想要用它来在城市中送快递，Eva身上有一些传感器。

可以让我们知道它的速度v(比如三维空间中沿x,y,z各轴的速度大小)，同时Eva身上还有GPS系统、气压计等设备，可以获知它的位置p(比如经纬度，海拔等)，也就是说我们可以实时观测Eva的状态。

那么我们可以把Eva的某一个时刻的状态表示为一个向量：

不确定性和相关性

虽然我们比较肯定Eva此时的状态，但是无论如何系统总是会存在误差的，无论是计算上，还是传感器的检测上，所以我们只能认为当前状态是当前真实状态的一个最优估计。

那么我们不妨认为Eva的当前状态服从一个高斯分布，如下图所示：

预测下一个位置的系统状态和系统误差

Ok，接下来我们需要通过Eva当前的状态，运用一些物理学的知识来预测它的下一个状态，通过简单的物理学知识，通过k-1时刻的位置和速度，可以推测下一个时刻的状态为：

写成矩阵形式就是：

此处的Fk就是状态转移矩阵。Eva的系统误差通过协方差矩阵Pk来表示，根据协方差矩阵的性质：

那么我们所预测的Eva下一个时刻的状态误差为：

考虑系统内部控制

为了能让Eva到达任何地方，毫无疑问我们需要对它进行控制，比如加速和减速，假设某个时刻我们施加给Eva的加速度是a，那么下一时刻的位置和速度则应该为：

因此我们的状态预测方程更新为：

Ok，新方程中的Bk我们称为状态控制矩阵，而uk称为状态控制向量，含义很明显，前者表明的是加速减速如何改变Eva的状态，而后者则表明控制的力度大小和方向。

 考虑系统外部影响

但是，外界可能有很多影响因素，导致我们对Eva实施控制的时候并不总是如我们所愿，有时候会逆风，有时候则是顺风。

在此我们猜测外部的不确定因素对Eva造成的系统状态误差wk服从均值为0高斯分布wk∼N(0,Qk)，至此我们就能得到Kalman滤波中完整的状态预测方程：

因为wk为0，所以有的文章可能会忽略不写，但是如果明确知道均值不为零的是时候，就需要注意了，这要看实际应用时候的场景，理解了它的原理，就能对各部分的变化有深入体会。

  此时应该观测到什么？

前面我们通过Eva的上一个状态，对它的当前状态做了缜密的预测，此时我们要考虑我们事先安装在Eva身上的各种传感器应该能够观测到什么？

Eva当前的状态和我们观测到的传感器数据应该具备特定的关系，假设这个关系通过矩阵表示为Hk，如下图所示：

在此前对Eva所做的预测状态下，我们应该观测到传感器的观测值为：

因此我们就完成了对观测值的预测，预测其结果服从如下高斯分布：

考虑实际观测的结果

好的，我们不仅推测了Eva当前的状态，还推测了我们应该观测到的传感器数据，但是现实和理想之间必然是存在差距的，我们预测的观测结果和实际的观测结果可能如下图所示：

上图中的zk表示实际观测的结果，但是观测的结果肯定也是不准确的，所以我们认为其观测噪声vk是一个均值为0，协方差矩阵为Rk的高斯分布，即：

其实也就是说我们对Eva的观测值服从高斯分布，Eva真实的情况应该存在以zk为椭圆心的椭圆内，即观测结果服从高斯分布：

终于来到了最关键的一步：卡尔曼滤波需要做的最重要的最核心的事就是融合预测和观测的结果，充分利用两者的不确定性来得到更加准确的估计。

通俗来说就是怎么从上面的两个椭圆中来得到中间淡黄色部分的高斯分布，看起来这是预测和观测高斯分布的重合部分，也就是概率比较高的部分。

两个高斯概率密度函数的乘积

一维的高斯分布通过高斯概率密度函数来表示，在坐标轴上画出来是一个类似草帽的形状。

下面给出两个高斯概率密度函数相乘的直观的结果。

对比标准的高斯概率密度函数，相乘的结果是一个乘了特定系数的新的高斯概率密度函数(这个系数在后面的演示代码中会计算)，并且我们可以求解得到这个新的高斯分布的均值和方差分别为：

通过matlab我们可以计算两个高斯概率密度函数的乘积，以及通过上述公式计算得到的新的高斯概率密度函数，以下是相关的代码和运行截图：

clear allx=-1:0.01:2.5;mu0 = 0.3;mu1 = 0.8;sigma0 = 0.2;sigma1 = 0.5;sigma0_sq = 0.04;sigma1_sq = 0.25;y1=normpdf(x,mu0,sigma0);y2=normpdf(x,mu1,sigma1);y3=y1.*y2;k = sigma0_sq / (sigma0_sq + sigma1_sq);mu = (mu0*sigma1_sq + mu1*sigma0_sq) / (sigma0_sq + sigma1_sq);sigma = sqrt((sigma0_sq * sigma1_sq) / (sigma0_sq + sigma1_sq));scale = (1.0 / (sqrt(2*pi*(sigma0_sq + sigma1_sq)))) * exp(-1.0 * ((mu0-mu1)^2/(2.0 * (sigma0_sq + sigma1_sq))));y4 = normpdf(x,mu,sigma);% * scale;figure;plot(x,y1,x,y2,x,y3, x, y4,'MarkerSize',20,'LineWidth',5);grid;tip1 = sprintf('(\\mu_0,\\sigma_0) = (%.4f, %.4f)', mu0, sigma0);tip2 = sprintf('(\\mu_1,\\sigma_1) = (%.4f, %.4f)', mu1, sigma1);tip3 = '(\mu_0,\sigma_0)*(\mu_1,\sigma_1)';tip4 = sprintf('(\\mu^\\prime,\\sigma^\\prime) = (%.4f, %.4f)', mu, sigma);legend({tip1, tip2, tip3, tip4});

运行截图：

图中蓝色和橙色两个波形的直接乘积是黄色这个波形，而它其实可以通过紫色的波形乘上一个系数得到，也就是前面代码中的scale这个变量，计算公式在上面的已经提供。

如果在计算y4(紫色的波形)的时候乘上这个系数，你会发现它的波形就和黄色的波形(y3)完全重合了。把对应行稍作修改即可：

y4 = normpdf(x,mu,sigma) * scale;

大家可以自行复制代码进行实验。

新的高斯分布

那么我们把关注点放在这个乘积中这个新的高斯概率密度函数，其实它就描述了一个新的高斯分布，这正是卡尔曼滤波想要的最优估计。在新的均值和方差计算公式中，我们令：

那么可以得到：

将它们写成矩阵形式就是：

前面我们已经得到了预测结果和观测结果服从的两个高斯分布，如下：

所以我们可以进行如下推导，来得到卡尔曼滤波对当前状态(基于预测和观测的)最优估计的计算方程：

好的，两边化简下，注意K可以展开，于是可以得到(是的，易得)：

此处的K′就是传说中的卡尔曼增益：

实际实现时的计算步骤

在实际使用卡尔曼滤波的时候，计算的步骤一般为(这里把下标去掉了，因为在实现的时候，即使下标不一样，我们用的其实就是一个变量，注意和前面的方程进行比对)：

上面的y是测量余量(measurement residual)，S是测量余量协方差矩阵。

最重要的是，我们要时刻关注不断迭代的系统变量，分别是系统的状态：x，其误差协方差矩阵：P，和卡尔曼增益：K。

在实际应用时，对Q和R的选择要依据实际情况来定，可以不断调试来寻找一个最优解，也可以是可变的，只要最终效果能够更好。

理解卡尔曼滤波的整个迭代过程，相信大家在实践和调试的时候也会得心应手。

本文仅做学术分享，如有侵权，请联系删文。

分享

收藏

点赞

在看