卡尔曼滤波通俗介绍

卡尔曼滤波通俗介绍

易于理解的介绍，应该是属于文字逻辑，而不是公式逻辑

参考文献

建议按照顺序阅读

【1】【易懂教程】我是如何十分钟理解与推导贝叶斯滤波(Bayes Filter)算法？

【2】如何通俗并尽可能详细地解释卡尔曼滤波？

【3】卡尔曼滤波器开发实践之一: 五大公式详解

【4】详解卡尔曼滤波器

卡尔曼滤波的作用

• 卡尔曼滤波用于优化我们感兴趣的量，当这些量无法直接测量但可以间接测量时。

• 用于估算系统状态，通过组合各种受噪音的传感器测量值

从贝叶斯滤波出发

本部分并不需要真正的了解贝叶斯滤波，只需要理解卡尔曼滤波和它的关系，更清晰的理解卡尔曼滤波

贝叶斯滤波的工作就是根据不断接收到的新信息和提供的一些已经知道的统计值，来不断更新概率，更新概率值的方法是根据概率论中的条件概率计算公式。

贝叶斯滤波公式如下：
$$P(x_t|z_t,u_t,x_{t-1})=\eta P(z_t|x_t)P(x_t|u_t,x_{t-1})$$
在这个公式中，有两个状态估计假设

• 当前状态$x_t$只与上个时刻状态$x_{t-1}$以及控制命令$u_t$有关（可以推算出$x_t$的预估值）
• 当前的观测量$z_t$只与当前状态$x_t$有关（可以推算出$x_t$的观测值）

贝叶斯滤波是一种思想，提出了如何利用观测值和预估值推算出$x_t$的预测值的可信度。

如果你对观测值，预估值，预测值还有点疑惑，没关系，我们还没有正式介绍它们。

而卡尔曼滤波是对贝叶斯滤波的一种具体实现，具体实现的方式是

• 状态量是线性变化
• 变量和噪声都是正态分布
  • 只要知道$\mu$，${\sigma }^2$，则有关变量和噪声的所有信息都知道
  • 正太分布经过线性变换依然是线性（刚好与状态线性变换假设对应）

正式进入卡尔曼滤波

还记得卡尔曼滤波的假设吗？所有变量和噪声都是正太分布。

那么显然，对于正太分布$x\sim N(\mu ,{\sigma }^2)$，$x$最有可能的值是$\mu$，当然，这个数据并不一定靠谱，因为只是最有可能，而不是绝对，那么对于用$\mu$代表$x$就有一个可信度，它就是${\sigma }^2$。
$$\begin{gathered} \mu 可能值 \\ {\sigma }^2可信度 \end{gathered}$$
这里值的一提的是，我们往往用向量、矩阵去存储数据、描述状态、代表变化过程等，所以对于$x\sim N(\mu ,{\sigma }^2)$，我们可能会有点小变化，比如${\sigma }^2$变成了协方差矩阵$\Sigma$。当然这些在概念理解时无伤大雅，只是在公式理解时会出现。
$$\begin{gathered} \overrightarrow{\mu }可能值 \\ \Sigma 可信度 \end{gathered}$$
好的，我们开始吧。

预估过程

对于一个系统，我们现在知道了它上个时刻的状态${\hat{x}}_{k-1}$和对应的可信度$P_{k-1}$
$$\begin{gathered} \overrightarrow{\mu }可能值{\hat{x}}_{k-1} \\ \Sigma 可信度P_{k-1} \end{gathered}$$

这里值的一提的是，$\hat{}$修饰的向量，表示我们已知（无论是以怎么样的方式得到的，如测量，预估等）。而其对应的可信度，往往是我们卡尔曼滤波需要调的参数

很显然的是，我们对状态的变化有一个数学模型的描述，这个数学模型能够由上一时刻的状态($x_{k-1}$)，推出（也叫预估）这一时刻的状态（$x_k$），如在最普通的卡尔曼滤波中，这个数学模型是线性的。
$${\hat{x}}_k=A{\hat{x}}_{k-1}+\overrightarrow{b}$$
当然还有很多其他模型，如对于一个运动系统，可以是匀速直线运动，也可以是匀加速运动等，对于这个数学模型的预估过程（使用数学模型，由上一时刻的状态($x_{k-1}$)，推出这一时刻的状态（$x_k$）），我们用状态转移矩阵$F_k$统一表示。
$$\begin{gathered} {\hat{x}}_{k-1}\stackrel{预估过程F_k}{\to }{\hat{x}}_t^{-}=F_k{\hat{x}}_{k-1} \\ {P}_{k-1}\stackrel{预估过程F_k}{\to }{P}_k^{-}=F_k{P}_{k-1}{F}_k^T \end{gathered}$$

有对于${\hat{x}}_t^{-}$这样在右上角有${}^{-}$的数据，表示预估值，代表着单纯由我们的数学模型推测出来的值，但并不是卡尔曼滤波真正得到的值，只是一个过程量。

对于这个预估过程很好理解，一切都是我们知道的！（牢记卡尔曼滤波的假设！），因此无论是状态向量还是可信度，他们的预估值都是完全在我们掌握之下的，因此无需添加一些量进行修正。

控制过程

对于一个系统，我们会进行一些控制使其状态发生变化，像开车时，我们一直匀速运动，那么预估过程也会是匀速模型，而我们突然踩油门，会有一个加速的控制，这些是我们数学模型没有掌握的，因此我们需要添加控制量对应的项。我们掌握了很多具体控制，如踩多少油门，这些具体控制用${\hat{B}}_k$表示控制过程，${\hat{u}}_t$表示控制量。
$$\begin{gathered} {\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}\stackrel{控制过程{\hat{B}}_k,控制量{\hat{u}}_k}{\to }{\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{\hat{u}}_k^{-} \\ {P}_k^{-}=F_k{P}_{k-1}{F}_k^T\stackrel{控制过程{\hat{B}}_k,控制量{\hat{u}}_t}{\to }{P}_k^{-}=F_k{P}_{k-1}{F}_k^T \end{gathered}$$
而**还有很多我们没有掌握的“控制”**会对汽车的运行造成影响，如风速、车胎磨损、路面情况等，这些我们没有掌握但是造成影响的“控制”，我们称其为噪声（干扰），它们对我们状态量的改变未知（因为我们对它们没有掌握！），因此会对我们修正的预估值的可信度产生影响，这个影响称为$Q_k$。注意，我们的状态量只是最可能的量，因此不一定是准确的。（牢记卡尔曼滤波的假设！）
$$\begin{gathered} {\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{\hat{u}}_k^{-}\stackrel{噪声Q_k}{\to }{\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{\hat{u}}_k^{-} \\ {P}_k^{-}=F_k{P}_{k-1}{F}_k^T\stackrel{噪声Q_k}{\to }{P}_k^{-}=F_k{P}_{k-1}{F}_k^T+Q_k \end{gathered}$$

更新过程

以上是我们完全根据上一个状态的预估，但实际上，系统的状态可能在这个时刻突然发生改变（注意与控制过程造成状态改变的区别）

• 控制过程造成的状态改变往往是平滑的，即使有噪声的影响，也不会改变整体曲线的平滑改变的趋势。
• 这里说的系统状态的改变，指的是陡峭尖锐的曲线变化。

举一个例子：原本我们正常的在路上驱车行使，刚开始是匀速行驶（这时我们可以给自己车建立一个数学模型了——线性的变换；实际上，绝大部分过程的数学模型都可设置成线性的，因为时间间隔足够小，以直代曲的思想），这时我们进入了预估过程；我们需要超车，于是踩油门，因此进入了控制过程；不幸的是在$t$时刻，我们超车失败，撞在了装了30吨货物的大卡车上，速度瞬间变成0，发动机也报废，而因为这只是瞬间发生的事件，因此我们仍然在踩油门，即预估过程和控制过程一直在继续，但是实际上我们已经完全停下来了，但是我们的滤波没有即时跟上我们的状态变化。

我们希望我们的滤波能够及时的反映一些意外，因此我们需要传感器数据对卡尔曼滤波进行修正（更新），这是更新过程。

我们有很多传感器可以帮助我们及时跟进系统的状态变化，对我们的预估值进行更新。依然是关于汽车的例子，比如我们有发动机的温度计和GPS两个传感器，对发动机的温度（$T$）和汽车速度（$v_x,v_y$）进行观测（这时候$\overrightarrow{x_t}$是三维向量$(T,v_x,v_y{)}^T$，这里上标T表示转置），我们会有一个2X3的矩阵，把

我们有m个传感器，会对n个状态值进行观测（${\hat{x}}_t$是n维向量）,那我们就会有一个$m\times n$的矩阵，表示由状态的预估值向传感器数据的预估值的转换矩阵，这个矩阵是$H_k$，而传感器的预估值为${\hat{z}}_k^{-}$，它的可信度是$R_k^{-}$。
$$\begin{gathered} {\hat{x}}_k^{-}\stackrel{由状态量到传感器数据的推算过程H_k}{\to }{\hat{z}}_k^{-}=H_k{\hat{x}}_k^{-} \\ {P}_k^{-}\stackrel{H_k}{\to }{R}_k^{-}=H_k{P}_k{H}_k^T \end{gathered}$$
而我们还有$t$时刻的传感器测量值为${\overrightarrow{z}}_k$，它的可信度是$R_k$。那么我们该怎么样用${\overrightarrow{z}}_k$，$R_k$更新${\hat{z}}_k^{-}$，$R_k^{-}$呢？

我们的基本思路是${\overrightarrow{z}}_k$相信一些，${\hat{z}}_k^{-}$也相信一些，在${\overrightarrow{z}}_k$和${\hat{z}}_k^{-}$之间取值，即取${\overrightarrow{z}}_k$和${\hat{z}}_k^{-}$的加权平均数。有了这个想法后，有一个很简单的算法：相乘！（不要忘记我们的假设：都是高斯分布！）

$N({\mu }_1,{\Sigma }_1)$与$N({\mu }_2,{\Sigma }_2)$相乘，结果还是高斯分布$N(\mu ,\Sigma )$！高斯万岁！

$\mu =\frac{{\mu }_1{\Sigma }_2+{\mu }_2{\Sigma }_1}{{\Sigma }_1+{\Sigma }_2}\Sigma =\frac{{\Sigma }_1{\Sigma }_2}{{\Sigma }_1+{\Sigma }_2}$

可以化简为

$\mu ={\mu }_1+k({\mu }_2-{\mu }_1)\Sigma ={\Sigma }_1-k{\Sigma }_1$

其中$k=\frac{{\Sigma }_1}{{\Sigma }_1+{\Sigma }_2}$，这里有一个比较重要的问题是${\Sigma }_1,{\Sigma }_2$是协方差矩阵，没有“倒数”的概念，应该是“逆”，但是这样写可以在更直观的理解加权平均数的概念，所以仅仅在理解上，我们做以下的对应：

$$({\Sigma }_1+{\Sigma }_2{)}^{-1}=\frac{1}{{\Sigma }_1+{\Sigma }_2}$$

我们相乘，可得
$$\begin{gathered} {\hat{z}}_k^{{}^{\prime }}={\hat{z}}_k^{-}+K({\overrightarrow{z}}_k-{\hat{z}}_k^{-})R_k^{{}^{\prime }}=R_k^{-}-K{R}_k^{-} \\ K=\frac{H_k{P}_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k} \end{gathered}$$

最后，我们需要推算出k时刻的最优值
$$\begin{gathered} {\hat{z}}_k^{{}^{\prime }}={\hat{z}}_k^{-}+K({\overrightarrow{z}}_k-{\hat{z}}_k^{-})\stackrel{由传感器数据到状态量的推算过程H_k^{-}}{\to }{\hat{x}}_k=H_k^{-1}{\hat{z}}_k^{{}^{\prime }}={\hat{x}}_k^{-}+K^{{}^{\prime }}({\overrightarrow{z}}_k-H_k^{-1}{\hat{x}}_k^{{}^{\prime }}) \\ {R}_k^{{}^{\prime }}=R_k^{-}-K{R}_k^{-}\stackrel{H_k^{-}}{\to }{P}_k=H_k^{-1}{R}_k^{{}^{\prime }}{H_k^T}^{-1}=P_k^{-}-K^{{}^{\prime }}{H}_k{P}_k^{-} \\ {K}^{{}^{\prime }}=\frac{P_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k} \end{gathered}$$
对于${\hat{x}}_k^{-}$
$${\hat{x}}_k=(1-K^{{}^{\prime }}{H}_k){\hat{x}}_k^{-}+K^{{}^{\prime }}{H}_k{\overrightarrow{x}}_k$$
注意:这里有一步${\overrightarrow{z}}_k=H_K{\overrightarrow{x}}_k$的变换

所以，我们可以直观的看出：卡尔曼滤波是一种权重不断自我更新的加权平均数

复习

请给下面的图填写完整吧！（实际上是我的绘图软件没法打数学公式）