轻松理解卡尔曼滤波

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

1. 一个简单的场景

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

2. 不确定性和相关性

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

$当前状态服从高斯分布$

高斯分布的中心 ${\mu}$就是图中的 ${\mathbf{\hat{x}}_k}$:

对于方差 ${\sigma^2}$(也就是图中的椭圆的范围)，因为我们有两个变量，所以可以用一个协方差矩阵 ${P_k}$来表示(如果对协方差矩阵还不了解，戳此了解):
${\mathbf{P}_k = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix}}$
所以Eva的真实状态可能就位于上图椭圆的范围内，位于圆心的概率最大。

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

Ok，接下来我们需要通过Eva当前的状态，运用一些物理学的知识来预测它的下一个状态，通过简单的物理学知识，通过k-1时刻的位置和速度，可以推测下一个时刻的状态为：
${\begin{aligned} \color{red}{p_k} &= \color{blue}{p_{k-1}} + \Delta t &\color{blue}{v_{k-1}} \\ \color{red}{v_k} &= &\color{blue}{v_{k-1}} \end{aligned}}$
写成矩阵形式就是：
${\begin{aligned} \color{red} {\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{blue}{\mathbf{\hat{x}}_{k-1}} = \mathbf{F}_k \color{blue}{\mathbf{\hat{x}}_{k-1}} \end{aligned}}$
此处的 ${F_k}$就是状态转移矩阵。
Eva的系统误差通过协方差矩阵来表示，根据协方差矩阵的性质：
${\begin{aligned} Cov(x) &= \Sigma\\ Cov(\color{red}{\mathbf{A}}x) &= \color{red}{\mathbf{A}} \Sigma \color{red}{\mathbf{A}}^T \end{aligned}}$
那么我们所预测的Eva下一个时刻的状态误差为：
${\begin{aligned} \color{red}{\mathbf{P}_k} &= \mathbf{F_k} \color{blue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{aligned}}$

4. 考虑系统内部控制

为了能让Eva到达任何地方，毫无疑问我们需要对它进行控制，比如加速和减速，假设某个时刻我们施加给Eva的加速度是 ${a}$，那么下一时刻的位置和速度则应该为：
${\begin{aligned} \color{red} {p_k} &= \color{blue} {p_{k-1}} + {\Delta t} &\color{blue} {v_{k-1}} + &\frac{1} {2} \color{green} {a} {\Delta t}^2 \\ \color{red} {v_k} &= &\color{royalblue} {v_{k-1}} + & \color{green} {a} {\Delta t} \end{aligned}}$

因此我们的状态预测方程更新为：
${\begin{aligned} \color{red} {\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{blue} {\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{green} {a} \\ &= \mathbf{F}_k \color{blue} {\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{green} {\mathbf{u}_k} \end{aligned}}$

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

扫描二维码关注公众号，回复： 6529791 查看本文章

5. 考虑系统外部影响

但是，外界可能有很多影响因素，导致我们对Eva实施控制的时候并不总是如我们所愿，有时候会逆风，有时候则是顺风，在此我们猜测外部的不确定因素对Eva造成的系统状态误差 ${\color{cyan}{\mathbf{w}_k}}$服从高斯分布 ${\color{cyan}{\mathbf{w}_k} \sim N(0 ,\color{cyan}{\mathbf{Q}_k})}$，至此我们就能得到Kalman滤波中完整的状态预测方程：
${\begin{aligned} \color{red}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{blue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{green}{\mathbf{u}_k} + \color{cyan}{\mathbf{w}_k} \\ \color{red}{\mathbf{P}_k} &= \mathbf{F_k} \color{blue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{cyan}{\mathbf{Q}_k} \end{aligned}}$

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

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

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

$当前状态和观测值之间的关系$

在此前对Eva所做的预测状态下，我们应该观测到传感器的观测值为：
${\begin{aligned} \mathbf{\mu}_{\text{expected}} &= \mathbf{H}_k \color{red} {\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{red}{\mathbf{P}_k} \mathbf{H}_k^T \end{aligned}}$

因此我们就完成了对观测值的预测，预测其结果服从如下高斯分布：
${(\color{red}{\vec{\mu}_0}, \color{#FF007F}{\mathbf{\Sigma}_0}) = (\color{red}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{#FF007F}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})}$

7. 考虑实际观测的结果

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

$预测观测值和实际观测值$

上图中的 ${\color{#ACE1AF}{\mathbf{z}_k}}$表示实际观测的结果，但是观测的结果肯定也是不准确的，所以我们认为其观测噪声 ${\color{green}{\mathbf{v}_k}}$是一个均值为0，协方差矩阵为 ${\color{green}{\mathbf{R}_k}}$的高斯分布，即：
${\color{green}{\mathbf{v}_k} \sim N(0 ,\color{green}{\mathbf{R}_k})}$
其实也就是说我们对Eva的观测值服从高斯分布，Eva真实的情况应该存在以 ${\color{#ACE1AF}{\mathbf{z}_k}}$为椭圆心的椭圆内，即观测结果服从高斯分布：
${(\color{#ACE1AF}{\vec{\mu}_1}, \color{green}{\mathbf{\Sigma}_1}) = (\color{#ACE1AF}{\mathbf{z}_k}, \color{green}{\mathbf{R}_k})}$
终于来到了最关键的一步：卡尔曼滤波需要做的最重要的最核心的事就是融合预测和观测的结果，充分利用两者的不确定性来得到更加准确的估计。通俗来说就是怎么从上面的两个椭圆中来得到中间淡黄色部分的高斯分布，看起来这是预测和观测高斯分布的重合部分，也就是概率比较高的部分。

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

一维的高斯分布通过高斯概率密度函数来表示，在坐标轴上画出来是一个类似草帽的形状。下面给出两个高斯概率密度函数相乘的直观的结果，需要详细的推导过程可以戳此查看。

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

对比标准的高斯概率密度函数，相乘的结果是一个乘了特定系数的新的高斯概率密度函数(这个系数在后面的演示代码中会计算)，并且我们可以求解得到这个新的高斯分布的均值和方差分别为：
${\begin{aligned} \color{blue}{\mu}' &= \mu_0 + \frac{\sigma_0^2 (\mu_1 - \mu_0)} {\sigma_0^2 + \sigma_1^2} \\ \color{blue}{\sigma}'^2 &= \sigma_0^2 - \frac{\sigma_0^4} {\sigma_0^2 + \sigma_1^2} \end{aligned}}$

通过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. 新的高斯分布

那么我们把关注点放在这个乘积中这个新的高斯概率密度函数，其实它就描述了一个新的高斯分布，这正是卡尔曼滤波想要的最优估计。在新的均值和方差计算公式中，我们令：
${\color{magenta}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2}}$
那么可以得到：
${\begin{aligned} \color{blue}{\mu}' &= \mu_0 + \color{magenta}{\mathbf{k}} (\mu_1 - \mu_0)\\ \color{blue}{\sigma}'^2 &= \sigma_0^2 - \color{magenta}{\mathbf{k}} \sigma_0^2 \end{aligned}}$
将它们写成矩阵形式就是：
${\begin{aligned} \color{magenta}{\mathbf{K}} &= \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} \\ \color{blue}{\vec{\mu}'} &= \vec{\mu_0} + \color{magenta}{\mathbf{K}} (\vec{\mu_1} - \vec{\mu_0})\\ \color{blue}{\Sigma'} &= \Sigma_0 - \color{magenta}{\mathbf{K}} \Sigma_0 \end{aligned}}$
前面我们已经得到了预测结果和观测结果服从的两个高斯分布，如下：
${\begin{aligned} (\color{red}{\vec{\mu}_0}, \color{#FE6F5E}{\mathbf{\Sigma}_0}) &= (\color{red}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{#FE6F5E}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T}) \\ (\color{#ACE1AF}{\vec{\mu}_1}, \color{green}{\mathbf{\Sigma}_1}) &= (\color{#ACE1AF}{\mathbf{z}_k}, \color{green}{\mathbf{R}_k}) \end{aligned}}$
所以我们可以进行如下推导，来得到卡尔曼滤波对当前状态(基于预测和观测的)最优估计的计算方程：

好的，两边化简下，注意 ${\color{magenta}{\mathbf{K}}}$可以展开，于是可以得到(是的，易得)：
${\begin{aligned} \color{blue} {\mathbf{\hat{x}}_k'} &= \color{red} {\mathbf{\hat{x}}_k} & + & \color{magenta}{\mathbf{K}'} ( \color{#ACE1AF}{\mathbf{z}_k} - \color{red}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{blue}{\mathbf{P}_k'} &= \color{#FE6F5E}{\mathbf{P}_k} & - & \color{magenta}{\mathbf{K}'} \color{#FE6F5E}{\mathbf{H}_k \mathbf{P}_k} \end{aligned}}$
此处的 ${\color{magenta}{\mathbf{K}'}}$就是传说中的卡尔曼增益：
${\color{magenta}{\mathbf{K}'} = \color{#FE6F5E}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{#FE6F5E}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{green}{\mathbf{R}_k})^{-1}}$

10. 实际实现时的计算步骤

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

预测阶段
${\begin{aligned} \mathbf{x} = (\mathbf{F} * \mathbf{x}) + (\mathbf{B} * \mathbf{u}) \\ \mathbf{P} = (\mathbf{F} * \mathbf{P} * \mathbf{F}^T) + \mathbf{Q} \end{aligned}}$
修正阶段
${\begin{aligned} \mathbf{y} &= \mathbf{z} - (\mathbf{H} * \mathbf{x}) \\ \mathbf{S} &= (\mathbf{H} * \mathbf{P} * \mathbf{H}^T) + \mathbf{R} \\ \mathbf{K} &= \mathbf{P} * \mathbf{H}^T * \mathbf{S}^{-1} \\ \mathbf{x} &= \mathbf{x} + (\mathbf{K} * \mathbf{y}) \\ \mathbf{P} &= (\mathbf{I} - (\mathbf{K} * \mathbf{H})) * \mathbf{P} \end{aligned}}$

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

最重要的是，我们要时刻关注不断迭代的系统变量，分别是系统的状态： ${\mathbf{x}}$，其误差协方差矩阵： ${\mathbf{P}}$，和卡尔曼增益： ${\mathbf{K}}$。
在实际应用时，对 ${\mathbf{Q}}$和 ${\mathbf{R}}$的选择要依据实际情况来定，可以不断调试来寻找一个最优解，也可以是可变的，只要最终效果能够更好。理解卡尔曼滤波的整个迭代过程，相信大家在实践和调试的时候也会得心应手。