轻松理解卡尔曼滤波-以MOT运动估计为例

最新推荐文章于 2024-07-01 11:37:46 发布

ClarissaYL

最新推荐文章于 2024-07-01 11:37:46 发布

阅读量2.3k

点赞数 5

分类专栏：多目标跟踪文章标签：卡尔曼滤波目标追踪 MOT

原文链接：https://www.jianshu.com/p/f6ce8943560c

版权

多目标跟踪专栏收录该内容

7 篇文章 4 订阅

订阅专栏

我在学习MOT问题时碰到了卡尔曼滤波，为弄清楚它先后找了不少资料，这一篇来自简书“胡涂笔记”的讲解是我目前看过最清晰的，让我茅塞顿开。它给定的场景与“目标追踪算法”中的用法一致，且辅以清晰的数学推理和逻辑分析，强烈建议大家一读。原文链接

此前学习和实现卡尔曼滤波花费了很多时间，其实想要理解其原理并不算很复杂。只是简单套用卡尔曼滤波的公式，而没有系统理解公式里面每个变量的缘来，不去理解卡尔曼滤波器的迭代过程和原理，在实现和调试系统的时候无疑是会找不着北的。本文将指引你轻松理解卡尔曼滤波。

1. 一个简单的场景

假设我们开发了一台无人机（假设它的名字是Eva），想要用它来在城市中送快递，Eva身上有一些传感器，可以让我们知道它的速度v(比如三维空间中沿x,y,z各轴的速度大小)，同时Eva身上还有GPS系统、气压计等设备，可以获知它的位置p(比如经纬度，海拔等)，也就是说我们可以实时观测Eva的状态。
那么我们可以把Eva的某一个时刻的状态表示为一个向量：

2. 不确定性和相关性

虽然我们比较肯定Eva此时的状态，但是无论如何系统总是会存在误差的，无论是计算上，还是传感器的检测上，所以我们只能认为当前状态是当前真实状态的一个最优估计。那么我们不妨认为Eva的当前状态服从一个高斯分布，如下图所示：
当前状态服从高斯分布
高斯分布的中心 $\mu$ 就是图中的 $\hat{x}_k$ :

对于方差 $\sigma^2$ (也就是图中的椭圆的范围)，因为我们有两个变量，所以可以用一个协方差矩阵来表示:

所以Eva的真实状态可能就位于上图椭圆的范围内，位于圆心的概率最大。

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

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

写成矩阵形式就是：

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

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

4. 考虑系统内部控制

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

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

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

5. 考虑系统外部影响

但是，外界可能有很多影响因素，导致我们对Eva实施控制的时候并不总是如我们所愿，有时候会逆风，有时候则是顺风，在此我们猜测外部的不确定因素对Eva造成的系统状态误差 $w_k$ 服从高斯分布 $w_k\sim N(0,Q_k)$ ，至此我们就能得到Kalman滤波中完整的状态预测方程：

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

6. 此时应该观测到什么？

前面我们通过Eva的上一个状态，对它的当前状态做了缜密的预测，此时我们要考虑我们事先安装在Eva身上的各种传感器应该能够观测到什么呢？
Eva当前的状态和我们观测到的传感器数据应该具备特定的关系，假设这个关系通过矩阵表示为 $H_k$ ，如下图所示：

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

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

7. 考虑实际观测的结果

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

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

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

终于来到了最关键的一步：卡尔曼滤波需要做的最重要的最核心的事就是融合预测和观测的结果，充分利用两者的不确定性来得到更加准确的估计。通俗来说就是怎么从上面的两个椭圆中来得到中间淡黄色部分的高斯分布，看起来这是预测和观测高斯分布的重合部分，也就是概率比较高的部分。

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

注：两个概率密度函数的乘积是一个联合分布。求观测和预测的联合分布，作为当前系统状态的分布。
一维的高斯分布通过高斯概率密度函数来表示，在坐标轴上画出来是一个类似草帽的形状。下面给出两个高斯概率密度函数相乘的直观的结果，需要详细的推导过程可以戳此查看。
两个高斯概率密度函数的乘积

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

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

clear all
x=-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});

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

因为matlab的函数normpdf使用的参数是均值和标准差，所以代码和截图中也是使用均值和标准差表示一个高斯分布。

图中蓝色和橙色两个波形的直接乘积是黄色这个波形，而它其实可以通过紫色的波形乘上一个系数得到，也就是前面代码中的scale这个变量，计算公式在上面的已经提供。如果在计算y4(紫色的波形)的时候乘上这个系数，你会发现它的波形就和黄色的波形(y3)完全重合了。把对应行稍作修改即可：

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

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

9. 新的高斯分布

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

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

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

所以我们可以进行如下推导，来得到卡尔曼滤波对当前状态(基于预测和观测的)最优估计的计算方程：
在这里插入图片描述
好的，两边化简下，注意可以展开，于是可以得到(是的，易得)：

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