图解卡尔曼滤波

卡尔曼滤波的基本过程如上图所示，下面我们来理解一下这个公式中的各个字母分别表示的是什么意思？ ${\hat{\mathbf{x}}}_{\mathbf{k}}$:表示k时刻，带估计的状态变量的最优估计，比如机器人的位置和速度最优估计构成的矢量。 ${\mathbf{P}}_{\mathbf{k}}$:表示k时刻，带估计量的协方差矩阵，比如位置和速度，本质上来说他们是随机变量，也就是说有会存在一个分布，一般来说他们是服从高斯分布的，两两组合可以生成一个协方差矩阵。 卡尔曼滤波假设状态所有的变量都是随机的且都服从高斯分布，每个变量都有其对应的均值以及方差（它代表了不确定性）。

${\mathbf{F}}_{\mathbf{k}}$:表示k-1时刻到k时刻的状态转移矩阵。 以速度和位置的关系为例，我们可以写出以下的表达式： $$\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}$$ 这样我们可以得到k-1时刻和k时刻的状态变量协方差矩阵的转移关系如下： $$\begin{array}{l}{\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}$$

对于机器人控制，以无人机为例，我们通过油门来进行控制，本质上来说是就是给力，根据牛顿第二定律，我们可以知道力是产生加速度的原因，因此我们可以拓展运动方程（状态方程）为： $$\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]a\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{\mathbf{u}}_k\end{array}$$ 这其中， ${\mathbf{u}}_{\mathbf{k}}$：表示控制量，也就是加速度或者力 ${\mathbf{B}}_{\mathbf{k}}$：被称为控制矩阵，表示了控制量和状态变量之间的转换关系

以上讨论在理想的情况下，现实世界中，不可避免的会有一些外界的干扰，比如风、打滑等等，这样会引入一个外界的不确定性，反映到数学上就是会带来一个外界协方差矩阵${\mathbf{Q}}_{\mathbf{k}}$，来对最佳估计进行校正。这样，我们就可以得到预测阶段完整的状态转移方程，如下： $$\begin{array}{l}{\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}$$

换句话说： 新的最佳估计是根据先前的最佳估计做出的预测，再加上对已知外部影响的校正。 新的不确定度是根据先前的不确定度做出的预测，再加上来自环境额外的不确定度。

对于传感器的测量，他们的读数和状态的估计也存在一定的关系，我们用简单的线性关系对其进行建模，可以假设一个$\mathbf{H}$矩阵，使得传感器测量值和状态估计之间满足一下表达关系： $$\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}$$ 注意此处的$\mu$可以认为是虚拟的抽象传感器，对应状态转移方程得到的反解，同样的$\Sigma$也是这个假想的虚拟传感器的协方差矩阵。

卡尔曼滤波器的伟大之处就在于它能够处理传感器噪声。换句话说，传感器本身的测量是不准确的，且原始估计中的每个状态都可能导致一定范围的传感器读数，而卡尔曼滤波能够在这些不确定性存在的情况下找到最优的状态。

根据传感器的读数，我们会猜测系统正处于某个特定状态。但是由于不确定性的存在，某些状态比其他状态更可能产生我们看到的读数。 我们引入新的变量如下： ${\mathbf{R}}_{\mathbf{k}}$：表示这种不确定性（如传感器噪声）的协方差 ${\mathbf{z}}_{\mathbf{k}}$：表示读数的分布均值，也就是我们观察到传感器的读数

这样一来，我们有了两个高斯分布：一个围绕通过状态转移预测的平均值，另一个围绕实际传感器读数。

因此，我们需要将基于预测状态（粉红色）的推测读数与基于实际观察到的传感器读数（绿色）进行融合。

那么融合后最有可能的新状态是什么？对于任何可能的读数，我们都有两个相关的概率：（1）我们的传感器读数是的测量值的概率，以及（2）先前估计值的概率认为是我们应该看到的读数。

如果我们有两个概率，并且想知道两个概率都为真的机会，则将它们相乘。因此，我们对两个高斯分布进行了相乘处理： 两个概率分布相乘得到的就是上图中的重叠部分。而且重叠部分的概率分布会比我们之前的任何一个估计值/读数都精确得多，这个分布的均值就是两种估计最有可能配置（得到的状态）。

事实证明，两个独立的高斯分布相乘之后会得到一个新的具有其均值和协方差矩阵的高斯分布！ 对于以上两个以为的高斯分布，分别满足以下分布表达式： $$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}$$ 我们可以得到新的高斯分布的均值和方差如下所示： $$\begin{gathered} {\mu }^{\prime }={\mu }_0+\frac{{\sigma }_0^2\left({\mu }_1-{\mu }_0\right)}{{\sigma }_0^2+{\sigma }_1^2} \\ {\sigma }^{{\prime }^2}={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2} \end{gathered}$$ 令$\mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$，我们可以简化上式为：$$\begin{array}{rl}{\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}$$ 我们再令：$$\mathbf{K}={\Sigma }_0{\left({\Sigma }_0+{\Sigma }_1\right)}^{-1}$$ 这可$\mathbf{K}$就是大名鼎鼎的“卡尔曼增益”（Kalman gain）,我们进一步可以得到： $$\begin{array}{l}{\mu }^{\prime }={\mu }_0+\mathbf{K}\left({\mu }_1-{\mu }_0\right)\\ {\Sigma }^{\prime }={\Sigma }_0-\mathbf{K}{\Sigma }_0\end{array}$$ 其中$\Sigma$表示新高斯分布的协方差矩阵，$\mu$是每个维度(如速度、位置等)的均值。

这样我们得到了两个高斯分布，一个是根据预测观测得到的虚拟传感器的分布$\left({\mu }_0,{\Sigma }_0\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}{rl}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k+\mathbf{K}(z_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{H}}_k{\mathbf{P}}_k^{\prime }{\mathbf{H}}_k^T& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T-\mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$ 我们可以得到卡尔曼增益可以表示为： $$\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_{\mathbf{k}}{)}^{-1}$$ 我们将${\mathbf{H}}_k$消除，令 $${\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_{\mathbf{k}}{)}^{-1}$$ 可以得到最终的化简形式的更新方程如下： $$\begin{array}{l}{\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}$$

以上公式推导仅供参考，具体使用的时候，有用的公式是以下几个： $$\begin{gathered} \begin{array}{l}{\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} \\ {\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({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_{\mathbf{k}}{)}^{-1} \end{gathered}$$

具体来说，我们在使用的时候，要搞搞清楚输入输出即可： ${\hat{\mathbf{x}}}_k$：k时刻最佳预估；${\mathbf{F}}_k$：k时刻状态转移矩阵；${\mathbf{B}}_k$：k时刻的控制矩阵；${\mathbf{u}}_k$：k时刻的控制量；${\mathbf{P}}_k$为k时刻最佳估计的协方差矩阵；${\mathbf{Q}}_k$为外部不确定度带来的协方差增量；${\mathbf{H}}_k$为“估计量-传感器”线性模型矩阵；${\mathbf{K}}^{\prime }$修正后的卡尔曼增益；${\mathbf{z}}_k$为k时刻传感器的读数；${\mathbf{R}}_{\mathbf{k}}$为k时刻传感器读数之间的协方差矩阵。

参考文献：How a Kalman filter works, in pictures