卡尔曼滤波（Kalman Filter）

卡尔曼滤波（Kalman Filter）

1.背景

现实世界中存在很多不确定性，主要体现在

(1)不存在完美的数学模型
(2)系统的扰动不可控并且很难建模
(3)测量传感器存在误差

卡尔曼滤波算法作为一个在存在不确定性的情况下对系统状态进行估计和预测的强大工具，被广泛应用于诸如目标跟踪、导航和控制等场景。

2.数据融合的例子

首先看一个数据融合的例子，如下图所示，其中红色的高斯分布 $N\left({\mu }_f,{\sigma }_f\right)$ 代表预测的小车的位置，蓝色的高斯分布 $N\left({\mu }_g,{\sigma }_g\right)$ 代表测量的小车的位置。直观上讲，真实的小车位置更有可能出现在两个高斯分布相重合的地方，即数据融合后绿色的高斯分布 $N\left({\mu }_{fg},{\sigma }_{fg}\right)$ 所在的位置。

绿色的高斯分布可通过红色的高斯分布与蓝色的高斯分布进行乘积得到。其中

$${\mu }_{fg}=\frac{{\mu }_f{\sigma }_g^2+{\mu }_g{\sigma }_f^2}{{\sigma }_f^2+{\sigma }_g^2}={\mu }_f+\frac{{\sigma }_f^2}{{\sigma }_f^2+{\sigma }_g^2}\left({\mu }_g-{\mu }_f\right)\text{\hspace{1em}(1)}$$

$${\sigma }_{fg}^2=\frac{{\sigma }_f^2{\sigma }_g^2}{{\sigma }_f^2+{\sigma }_g^2}={\sigma }_f^2-\frac{{\sigma }_f^4}{{\sigma }_f^2+{\sigma }_g^2}\text{\hspace{1em}(2)}$$

可以看出，${\mu }_g<{\mu }_{fg}<{\mu }_f$，${\sigma }_{fg}^2<{\sigma }_f^2$ 且 ${\sigma }_{fg}^2<{\sigma }_g^2$。这相当于在 ${\mu }_g$ 和 ${\mu }_f$ 之间取了一个权衡值，更小的方差意味着更准确的估计。如果令

$$k=\frac{{\sigma }_f^2}{{\sigma }_f^2+{\sigma }_g^2}\text{\hspace{1em}(3)}$$

则公式(1)和(2)可重写为如下形式
$${\mu }_{fg}={\mu }_f+k\left({\mu }_g-{\mu }_f\right)\text{\hspace{1em}(4)}$$

$${\sigma }_{fg}^2=(1-k){\sigma }_f^2\text{\hspace{1em}(5)}$$

以上是一维的情况，如果是多维空间，公式(3)、(4)和(5)可进一步扩展为如下的矩阵形式

$$K={\Sigma }_f{\left({\Sigma }_f+{\Sigma }_g\right)}^{-1}\text{\hspace{1em}(6)}$$

$${\mathbf{\mu }}_{fg}={\mathbf{\mu }}_f+K\left({\mathbf{\mu }}_g-{\mathbf{\mu }}_f\right)\text{\hspace{1em}(7)}$$

$${\Sigma }_{fg}=(I-K){\Sigma }_f\text{\hspace{1em}(8)}$$

其中，$K$ 就是卡尔曼增益(Kalman Gain)。

3.卡尔曼滤波的基本模型

假设离散线性动态系统的模型如下

$${\mathbf{x}}_k=A{\mathbf{x}}_{k-1}+B{\mathbf{u}}_k+{\mathbf{\omega }}_{k-1}\text{\hspace{1em}(9)}$$

$${\mathbf{z}}_k=H{\mathbf{x}}_k+{\mathbf{\nu }}_k\text{\hspace{1em}(10)}$$

其中
$\mathbf{\omega }\sim N\left(0,Q\right)$ 代表过程噪声（process noise）；
$\mathbf{\nu }\sim N\left(0,R\right)$ 代表测量噪声（measurement noise）；
${\mathbf{x}}_k$ 为系统状态向量，是状态的真实值，实际中是一个未知量，在上一节的例子中，可以看作是小车的真实位置，卡尔曼滤波算法的目的就是要获得尽可能准确的 ${\mathbf{x}}_k$ 估计值；
${\mathbf{u}}_k$ 为系统输入，实际中为已知量；
${\mathbf{z}}_k$ 为系统状态向量的观测，在上一节的例子中，可以看作是测量小车位置的传感器读数。
$H$ 为状态观测矩阵，其作用可以是从现实中的物理量比如位置变成传感器输出的值，也可以作为选择矩阵，因为传感器并不一定对所有状态变量的测量都能够满足，有些状态变量无法输出。

4. 卡尔曼滤波的公式推导

4.1. 预测的分布（预测阶段）

为了得到估计的状态信息的高斯分布，需要知道 $k$ 时刻的两个信息：

(1)先验状态估计值（prior state estimate） ${\hat{\mathbf{x}}}_k^{-}$ （可看作对 ${\mathbf{x}}_k$ 估计的均值），可由状态预测方程得到

$${\hat{\mathbf{x}}}_k^{-}=A{\hat{\mathbf{x}}}_{k-1}+B{\mathbf{u}}_k\text{\hspace{1em}(11)}$$

其中 ${\hat{\mathbf{x}}}_{k-1}$ 为 $k-1$ 时刻的后验状态估计值（posteriori state estimate）。

(2)真实值与预测值之间的协方差矩阵 $P_k^{-}=E\left({\mathbf{e}}_k^{-}{\mathbf{e}}_k^{-\text{T}}\right)$ ，其中 ${\mathbf{e}}_k^{-}={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}$ 为先验状态误差向量。

$$\begin{array}{rl}{\mathbf{e}}_k^{-}& ={\mathbf{x}}_k-{\hat{\mathbf{x}}}_k^{-}\\ & =A{\mathbf{x}}_{k-1}+B{\mathbf{u}}_k+{\mathbf{\omega }}_{k-1}-\left(A{\hat{\mathbf{x}}}_{k-1}+B{\mathbf{u}}_k\right)\\ & =A\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{\omega }}_{k-1}\\ & =A{\mathbf{e}}_{k-1}+{\mathbf{\omega }}_{k-1}\end{array}\text{\hspace{1em}(12)}$$

$$\begin{array}{rl}{P}_k^{-}& =E\left({\mathbf{e}}_k^{-}{\mathbf{e}}_k^{-\text{T}}\right)\\ & =E\left(\left(A{\mathbf{e}}_{k-1}+{\mathbf{\omega }}_{k-1}\right){\left(A{\mathbf{e}}_{k-1}+{\mathbf{\omega }}_{k-1}\right)}^{\text{T}}\right)\\ & =E\left(A{\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\text{T}}{A}^{\text{T}}+A{\mathbf{e}}_{k-1}{\mathbf{\omega }}_{k-1}^{\text{T}}+{\mathbf{\omega }}_{k-1}{\mathbf{e}}_{k-1}^{\text{T}}{A}^{\text{T}}+{\mathbf{\omega }}_{k-1}{\mathbf{\omega }}_{k-1}^{\text{T}}\right)\\ & =AE\left({\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\text{T}}\right)A^{\text{T}}+E\left({\mathbf{\omega }}_{k-1}{\mathbf{\omega }}_{k-1}^{\text{T}}\right)\\ & =A{P}_{k-1}{A}^{\text{T}}+Q\end{array}\text{\hspace{1em}(13)}$$

其中 $P_{k-1}=E\left({\mathbf{e}}_{k-1}{\mathbf{e}}_{k-1}^{\text{T}}\right)$ 为 $k-1$ 时刻真实值与最优估计值之间的协方差矩阵，${\mathbf{e}}_{k-1}={\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}$ 为 $k-1$ 时刻后验状态误差向量。

因此，$k$ 时刻估计的状态信息的高斯分布为 $N\left({\hat{\mathbf{x}}}_k^{-},P_k^{-}\right)$，将其通过状态观测矩阵 $H$ 进行转换可得 $N\left(H{\hat{\mathbf{x}}}_k^{-},H{P}_k^{-}{H}^{\text{T}}\right)$

4.2. 测量的分布

根据公式(10)可知，$k$ 时刻的测量值对应的高斯分布为 $N\left({\mathbf{z}}_k,R\right)$。

4.3. 预测分布和测量分布进行数据融合（校正阶段）

记 $k$ 时刻的后验状态估计值为 ${\hat{\mathbf{x}}}_k$，真实值与最优估计值之间的协方差为 $P_k$，将其通过状态观测矩阵 $H$ 进行转换可得 $N\left(H{\hat{\mathbf{x}}}_k,H{P}_k{H}^{\text{T}}\right)$。对预测的分布 $N\left(H{\hat{\mathbf{x}}}_k^{-},H{P}_k^{-}{H}^{\text{T}}\right)$ 和测量的分布 $N\left({\mathbf{z}}_k,R\right)$ 按照公式(6)、(7)和(8)进行数据融合可得

$$G_k=H{P}_k^{-}{H}^{\text{T}}{\left(H{P}_k^{-}{H}^{\text{T}}+R\right)}^{-1}\text{\hspace{1em}(14)}$$

$$H{\hat{\mathbf{x}}}_k=H{\hat{\mathbf{x}}}_k^{-}+G_k\left({\mathbf{z}}_k-H{\hat{\mathbf{x}}}_k^{-}\right)\text{\hspace{1em}(15)}$$

$$H{P}_k{H}^{\text{T}}=(I-G_k)H{P}_k^{-}{H}^{\text{T}}\text{\hspace{1em}(16)}$$

令$G_k=H{K}_k$，则

$$K_k=P_k^{-}{H}^{\text{T}}{\left(H{P}_k^{-}{H}^{\text{T}}+R\right)}^{-1}\text{\hspace{1em}(17)}$$

$${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k\left({\mathbf{z}}_k-H{\hat{\mathbf{x}}}_k^{-}\right)\text{\hspace{1em}(18)}$$

$$P_k=(I-K_{k}H)P_k^{-}\text{\hspace{1em}(19)}$$

5. 算法步骤总结

初始化 ${\hat{\mathbf{x}}}_0$ 和 $P_0$。
预测：
(1)先验状态估计值 ${\hat{\mathbf{x}}}_k^{-}=A{\hat{\mathbf{x}}}_{k-1}+B{\mathbf{u}}_k$
(2)先验误差协方差 $P_k^{-}=A{P}_{k-1}{A}^{\text{T}}+Q$
校正：
(3)卡尔曼增益 $K_k=P_k^{-}{H}^{\text{T}}{\left(H{P}_k^{-}{H}^{\text{T}}+R\right)}^{-1}$
(4)后验状态估计值 ${\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{-}+K_k\left({\mathbf{z}}_k-H{\hat{\mathbf{x}}}_k^{-}\right)$
(5)更新误差协方差 $P_k=(I-K_{k}H)P_k^{-}$