卡尔曼滤波（附C++代码）

是什么

任何时候对于动态系统存在不确定信息，都可使用卡尔曼滤波（Kalman Filter，下面简称为KF）对系统下一步要做什么做出有根据的猜测。

KF对于连续变化的系统是理想的，优点是占用内存小而且速度快，非常适用于实时问题和嵌入式系统。

使用KF可以干啥

举例：制造了一个可以在树林里游荡的小机器人，它需要知道自己的确切位置，这样它才能导航。

假设机器人状态表示为向量$x_k$，包含位置和速度：
$$x_k=(p,v)$$

状态可以是任何想要追踪的东西。

我们可以从GPS传感器及我们发送给机器人的指令来获取机器人的状态，但是这两个来源都存在一定的不确定性和误差。

如果利用所有可以获取到的信息，可能会给出更好的判断，这就是KF的作用。

KF如何看待我们的问题

继续使用一个仅有位置和速度的简单状态
$$x=\left[\begin{array}{l}p\\ v\end{array}\right]$$

我们不知道实际的位置和速度是多少;位置和速度的组合有很多，但其中一些比其他的可能性更大

KF假设两个变量都是随机的并且符合高斯分布，每个变量都有平均值KaTeX parse error: Undefined control sequence: \μ at position 1: \̲μ̲（随机分布的中心）和方差${\sigma }^2$（不确定性）

在上图中，位置和速度是不相关的。

下图中位置和速度是相关的，观察到特定位置的可能性与速度有关。

追踪这种关系可以给我们提供更多的信息，一个测量值告诉我们其他测量值可能是什么。这就是卡尔曼滤波器的目标，我们想要从不确定的测量中尽可能多地压缩信息!

这种相关性由协方差矩阵表示，矩阵的每个元素${\Sigma }_{ij}$表示第i个状态变量和第j个状态变量的相关程度。协方差矩阵是对称阵，由$\Sigma$表示，每个元素表示为${\Sigma }_{ij}$。

使用矩阵描述问题

要将关于状态的知识建模成高斯分布，需要知道$k$时刻的两条信息：最佳估计${\hat{\mathbf{x}}}_{\mathbf{k}}$（平均值）和它的协方差矩阵${\mathbf{P}}_{\mathbf{k}}$。

$${\hat{\mathbf{x}}}_k=\left[\begin{array}{l}\text{ position }\\ \text{ velocity }\end{array}\right]{\mathbf{P}}_k=\left[\begin{array}{ll}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]$$

随后，需要一种方式来看当前状态（$k-1$时刻）并预测下个状态（$k$时刻）

可以用矩阵${\mathbf{F}}_{\mathbf{k}}$来表示这个预测步骤

它将原始估计中的每个点移动到一个新的预测位置，如果原始估计是正确的，系统就会移动到这个位置。

使用运动学公式预测下一时刻的位置和速度
$$\begin{array}{l}{p}_k=p_{k-1}+\Delta t{v}_{k-1}\\ v_k=v_{k-1}\end{array}$$

写成矩阵形式
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}$$

现在有了预测矩阵给出下一状态，但是仍然不知如何更新协方差矩阵。

首先记住一个公式：
$$\begin{array}{rl}\mathrm{Cov}(x)& =\Sigma \\ \mathrm{Cov}(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}$$
将上面两个式子结合，可得
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T\end{array}$$

外部影响

可能还会存在一些与状态无关的变化，外界世界可能也会影响系统

例如，如果状态模拟火车的运动，火车操作员可能会推油门，导致火车加速。

将这些外部世界的影响装入一个${\mathrm{u}}_k$向量中，将其作为预测的修正项。

假如已知加速度$a$，由运动学公式：
$$\begin{array}{rl}& p_k=p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a\Delta t^2\\ & v_k=v_{k-1}+a\Delta t\end{array}$$
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{u}_k\end{array}$$
${\mathbf{B}}_k$被称为控制矩阵，${\mathrm{u}}_k$被称为运动向量，对于没有外部影响的系统可以省略这些项。

外部不确定性

例如如果我们跟踪一架四轴飞行器，它可能会受到风的冲击。如果我们追踪的是轮式机器人，轮子可能会打滑，或者地面上的颠簸会让它减速。我们无法跟踪这些事情，如果这些发生了，我们的预测可能会出错，因为我们没有考虑这些额外的力。

可以在每个预测步骤之后加上一些不确定性来对不确定性进行建模

可以说每个点${\hat{\mathbf{x}}}_{k-1}$都被移动到一个协方差为${\mathbf{Q}}_k$的高斯分布中的某处，也可以说我们将未知的外部影响当作具有协方差${\mathbf{Q}}_k$的噪声。

这将会产生一个与之前具有不同协方差和相同平均值的高斯分布。

现在预测步骤可以表示为：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{u}_k\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$

也就是说新的最佳估计是由以前的最佳估计做出的预测加上对已知外部影响的修正

而新的不确定性是从旧的不确定性中预测出来的，还有一些来自环境的额外不确定性。

现在由${\hat{\mathbf{x}}}_k$和${\mathbf{P}}_k$可以对系统将来的状态有一个模糊的估计。

如果能从传感器获取一些信息，会发生什么呢？

通过测量细化估计

使用矩阵${\mathbf{H}}_k$表示传感器。

可以用通常的方式表示出传感器读数分布：
$$\begin{array}{rl}{\mu }_{\text{expected }}& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k\\ {\mathbf{\Sigma }}_{\text{expected }}& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$
KF很擅长处理传感器噪声，换句话说传感器多少都有点不可靠，最初估计的每个状态可能都会导致一定范围的传感器读数。


不确定性的协方差用${\mathbf{R}}_k$表示，分布的平均值就是我们观察到的读数，用${\mathbf{Z}}_k$表示

所以现在有两个高斯分布，一个围绕着转换后预测值的平均值，一个围绕着真实的传感器读数。

必须试着调和上面的两个值。

什么是最可能的状态呢？对于任何可能的读数$\left(z_1,z_2\right)$，需要下面两种可能性都满足，一个是传感器读数${\mathbf{z}}_k$是错误测量的可能性；另一个是之前的估计认为$\left(z_1,z_2\right)$是该看到的读数的可能性。

由概率相关的知识，上述两个可能性都为真的概率就是将两者的概率相乘，所以将两个高斯分布相乘。

上图中重叠的部分比之前的估计都精确的多，该部分的均值就是两种估计最可能出现的情况，也能给出在现有信息的情况下对于真实状态的最佳猜测。

这就出现了另一个高斯分布

也就是说，将两个高斯分布相乘，得到的是一个新的高斯分布。

组合高斯分布

从一维的角度来看，公式为
$$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$
蓝色曲线就是红绿相乘得到的高斯曲线

$$\mathcal{N}\left(x,{\mu }_0,{\sigma }_0\right)\cdot \mathcal{N}\left(x,{\mu }_1,{\sigma }_1\right)\stackrel{?}{=}\mathcal{N}\left(x,{\mu }^{\prime },{\sigma }^{\prime }\right)$$

经过推导，可得
$$\begin{array}{rl}\mathbf{k}& =\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\\ {\mu }^{\prime }& ={\mu }_0+\mathbf{k}\left({\mu }_1-{\mu }_0\right)\\ {{\sigma }^{\prime }}^2& ={\sigma }_0^2-\mathbf{k}{\sigma }_0^2\end{array}$$
写成矩阵形式如下：
$$\begin{array}{rl}& \mathbf{K}={\Sigma }_0{\left({\Sigma }_0+{\Sigma }_1\right)}^{-1}\\ & {\mu }^{\prime }={\mu }_0+\mathbf{K}\left({\mu }_1-{\mu }_0\right)\\ & {\Sigma }^{\prime }={\Sigma }_0-\mathbf{K}{\Sigma }_0\end{array}$$

矩阵$\mathbf{K}$被称为卡尔曼增益

进行组合

将之前的两个分布：预测值$\left({\mu }_0,\Sigma \mathrm{b}\right)=\left({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\right)$和观测值$\left({\mu }_1,{\Sigma }_1\right)=\left({\mathbf{z}}_k,{\mathbf{R}}_k\right)$，代入上面的式子，可以得到重叠：
$$\begin{array}{rlrl}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k& & +\mathbf{K}\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{H}}_k{\mathbf{P}}_k^{\prime }{\mathbf{H}}_k^T& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T& & -{\mathbf{KH}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$

卡尔曼增益为
$$\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}$$

将上面的式子进行化简可得
$$\begin{array}{c}{\hat{\mathbf{x}}}_k^{\prime }={\hat{\mathbf{x}}}_k+{\mathbf{K}}^{\prime }\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{P}}_k^{\prime }={\mathbf{P}}_k-{\mathbf{K}}^{\prime }{\mathbf{H}}_k{\mathbf{P}}_k\\ {\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}\end{array}$$

上面的式子给出了更新步骤，这样${\hat{\mathbf{x}}}_k^{\prime }$就是新的最佳估计，然后就可以继续进行多次的预测和更新。

整个过程如上图。

总结

所有需要记住的式子就是：

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$
和
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+{\mathbf{K}}^{\prime }\left({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k\right)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_k-{\mathbf{K}}^{\prime }{\mathbf{H}}_k{\mathbf{P}}_k\end{array}$$
和
$${\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k\right)}^{-1}$$
这些就可以用来对任何线性系统进行建模，对于非线性系统可以使用扩展的KF

简而言之

KF的核心过程如下图：

上述两个状态逐渐迭代罢了

代码实现

#include 
#include 
#include 

using namespace std;

double new_mean, new_var;

// 更新：传感器获取到比较准确的位置信息后来更新当前的预测位置，也就是纠正预测的错误。
tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2)/(var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}

// 预测：当前状态环境下，对下一个时间段t的位置估计计算的值。
tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var = var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{
    //Measurements and measurement variance
    double measurements[5] = {5, 6, 7, 8, 9};
    double measurement_sig = 4;

    //Motions and motion variance
    double motion[5] = {1, 1, 2, 1, 1};
    double motion_sig = 2;

    //Initial state
    double mu = 0;
    double sig = 1000;

    for (int i=0; i<sizeof(measurements)/sizeof(measurements[0]); i++)
    {
        tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
        printf("update: [%f, %f]\n", mu, sig);
        tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
        printf("predict: [%f, %f]\n", mu, sig);
    }
    return 0;
}