目标追踪中的卡尔曼滤波公式的推导总结

介绍

卡尔曼滤波主要有两个过程

• 预测：根据上一个状态对当前状态进行预测
• 更新：根据传感器的观测值，来修正当前状态的估计值

预测的过程

假设一台机器小车在$k-1$时刻的状态为${\hat{x}}_{k-1}$

假设这个状态包含两个参数

• 当前的位置参数${\hat{p}}_{k-1}$
• 当前的速度参数${\hat{v}}_{k-1}$

根据这些参数，可以推断$k$时刻的小车状态，假设小车为匀速直线运动，则$k$时刻的预测为

• $\widehat{p_k}={\hat{p}}_{k-1}+{\hat{v}}_{k-1}\cdot \Delta t$
• $\widehat{v_k}={\hat{v}}_{k-1}$

那么$k$时刻的状态就是$\widehat{x_k}=(\widehat{p_k},\widehat{v_k})$

由于状态信息中，每个变量之间可能存在一定的相关性，所以存在一个描述相关性的协方差矩阵$P_{K-1}$,这个协方差矩阵会随着状态的预测而调整

卡尔曼滤波的计算过程中，需要一个协方差矩阵 $P_k$ 是因为它表示了状态估计的不确定性或者误差。$P_k$ 的大小反映了状态估计的精度，$P_k$ 的形状反映了状态变量之间的相关性。卡尔曼滤波的目标是通过预测和更新，使得 $P_k$ 尽可能地减小，从而提高状态估计的准确性。

上述的过程可以修改为
$${\hat{x}}_k=\left|\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right|{\hat{x}}_{k-1}=F_k{\hat{x}}_{k-1}$$

协方差矩阵

已知：
$Cov(x)=\Sigma$

$Cov(Ax)=A\Sigma A^T$

则$P_k=F_k{P}_{k-1}{F}_k^T$

外部影响

除了速度和位置，外因也会对系统造成影响，比如模拟火车运动，除了列车自驾系统，列车的操作员可能会手动速度，对于这些信息，我们把它看作一个向量${\vec{u}}_k$,假设油门控制带来的加速度为$a$，则
$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$

转换为矩阵的形式为为：

$${\hat{x}}_k=F_k{\hat{x}}_{k-1}+\left|\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right|a=F_k{\hat{x}}_k+B_k{\vec{u}}_k$$

其中 $B_k$是控制矩阵，${\vec{u}}_k$是控制向量，如果外部环境异常简单，则可以忽略这部分

外部不确定性

如果存在我们不知道的力量呢？当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。

这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和“世界”相关的所有不确定性，这些不确定性视为协方差的噪音$Q_k$：

$${\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k$$
$$P_k=F_k{P}_{k-1}{F}_k^T+Q_k$$

简而言之，可以总结如下：

新的最佳估计是基于原最佳估计和已知外部影响矫正后得到的预测

新的不确定性是基于原不确定性和外部环境的不确定性得到的预测

通过测量来细化估计值

我们可能有好几个传感器，他们一起提供了有关系统状态的信息。传感器的作用不是重点，重点是它可以告诉我们关于状态的间接信息。传感器给出的是系统输出或者测量值 ${\vec{z}}_k$ ，它们通常包含一些噪声和误差，并且可能只能反映状态信息的一部分或者间接的信息。我们需要知道每个传感器测量的是什么，以及它们和状态信息之间的关系。这个关系可以用一个矩阵 $H_k$ 来表示输出或者测量函数，即

$${\vec{z}}_k=H_k{\hat{x}}_k+{\overrightarrow{ϵ}}_k$$

其中 ${\overrightarrow{ϵ}}_k$ 是一个表示测量噪声或者误差的向量。根据状态信息预测得到的输出或者测量值为 $H_k\cdot {\hat{x}}_k$ ，它表示传感器期望得到的读数。根据状态信息得到的测量值期望和协方差为

${\vec{\mu }}_{expected}=H_k{\hat{x}}_k$

${\Sigma }_{expected}=H_k{P}_k{H}_k^T+R_k$

其中 $R_k$ 是一个表示测量噪声${\overrightarrow{ϵ}}_k$的协方差矩阵。

现在我们得到了两块高斯分布，一块围绕预测输出或者测量值 ${\vec{\mu }}_{expected}$ ，另一块围绕实际输出或者测量值 ${\vec{z}}_k$ 。

如果要生成靠谱预测，模型必须调和这两个信息。也就是说，对于任何可能的输出或者测量值 $(z_1,z_2,...,z_n)$ ，这两种方法预测的状态都有可能是准确的，也都有可能是不准确的。重点是我们怎么找到这两个准确率。

最简单的方法是两者相乘，两块高斯分布相乘后，我们可以得到它们的重叠部分，这也是会出现最佳估计的区域。换个角度看，它看起来也符合高斯分布

事实证明，当你把两个高斯分布和它们各自的均值和协方差矩阵相乘时，你会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了：我们必须有一个公式来从旧的参数中获取这些新参数！

$N(x,{\mu }_0,{\sigma }_0)\ast N(x,{\mu }_1,{\sigma }_1)=N(x,{\mu }^{{}^{\prime }},{\sigma }^{{}^{\prime }})$

${\mu }^{{}^{\prime }}={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}$

${{\sigma }^{{}^{\prime }}}^2={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}$

令$k=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}$，则：

${\mu }^{{}^{\prime }}={\mu }_0+k({\mu }_1-{\mu }_0)$

${{\sigma }^{{}^{\prime }}}^2={\sigma }_0^2-k{\sigma }_0^2$

扩展到多维的内容，则如下表达：

$K={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}$

${\vec{\mu }}^{\prime }=\overrightarrow{{\mu }_0}+K(\overrightarrow{{\mu }_1}-{\vec{\mu }}_0)$

${\Sigma }^{\prime }={\Sigma }_0-K{\Sigma }_0$

其中$K$称之为卡尔曼增益

把他们俩结合起来

截至目前，我们有用矩阵$({\mu }_0,{\Sigma }_0)=(H_k{\hat{x}}_k,H_k{P}_k{H}_k^T)$的传感器读数预测分布，也有传感器读数$({\mu }_1,{\Sigma }_1)=({\vec{z}}_k,R_k)$的传感器实际读数（测量值）分布，带入上式的矩阵等式中可以得到：

$H_k{\hat{x}}_k^{{}^{\prime }}=H_k{\hat{x}}_k+K(\overrightarrow{z_k}-H_k{\hat{x}}_k)$

$H_k{P}_k^{{}^{\prime }}{H}_k^T=H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T$

$K=H_k{P}_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}$

上述式子还包含一个$H_k$,化简得到：

${\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K^{\prime }(\overrightarrow{z_k}-H_k{\hat{x}}_k)$

$P_k^{{}^{\prime }}=P_k-K^{\prime }{H}_k{P}_k$

$K^{\prime }=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}$

最后，$x_k^{{}^{\prime }}$就是我们的最佳估计，可以继续把他放入公式做下一轮预测

总结

文字总结

卡尔曼滤波是一种迭代的过程，它不断地交替进行预测和更新，以利用系统模型和传感器数据来估计系统状态。它的基本步骤如下：

• 预测：根据上一时刻的状态估计和控制输入，预测当前时刻的状态和协方差
• 更新：根据当前时刻的输出或者测量值，计算卡尔曼增益，并用它来修正预测状态和协方差，得到最佳状态估计
• 本质：卡尔曼滤波就是利用传感器读数和状态预测来融合得到一个更加准确的状态估计，然后用这个状态估计来预测下一个状态。卡尔曼滤波的核心思想是利用两个高斯分布的乘积来得到一个方差更小的高斯分布，这个方差更小的高斯分布就是最佳状态估计的分布。

卡尔曼滤波的优点是它可以有效地处理不确定性和噪声，以及融合多个传感器数据。它的缺点是它需要知道系统模型和噪声模型，并且假设状态和输出或者测量之间是线性关系。如果这些假设不成立，那么就需要使用非线性卡尔曼滤波，例如扩展卡尔曼滤波或者无迹卡尔曼滤波。

计算公式总结

根据上一个状态估计当前状态

$${\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k$$

$$P_k=F_k{P}_{k-1}{F}_k^T+Q_k$$

• ${\hat{x}}_k$ 是当前时刻的状态估计向量，包含了系统内部的变量
• $F_k$ 是状态转移矩阵，包含了系统动力学模型，描述了状态之间的关系；
• $B_k$ 是控制矩阵，包含了控制模型，描述了控制输入对状态的影响；
• ${\vec{u}}_k$ 是控制输入向量，表示外部对系统施加的作用力或者操作；
• $P_k$ 是协方差矩阵，表示状态估计的不确定性或者误差；
• $Q_k$ 是过程噪声协方差矩阵，表示外部环境的不确定性或者干扰。

根据观测值修正当前状态的估计

$${\hat{x}}_k^{{}^{\prime }}={\hat{x}}_k+K^{\prime }(\overrightarrow{z_k}-H_k{\hat{x}}_k)$$

$$P_k^{{}^{\prime }}=P_k-K^{\prime }{H}_k{P}_k$$

$$K^{\prime }=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}$$

• ${\hat{x}}_k^{{}^{\prime }}$ 是修正后的状态估计向量，是最佳状态估计；
• ${\vec{z}}_k$ 是观测值或者测量值向量，表示传感器给出的系统输出；
• $H_k$ 是输出或者测量函数矩阵，描述了状态值和测量值之间的关系；
• $K^{\prime }$ 是卡尔曼增益矩阵，表示预测输出或者测量值和实际输出或者测量值之间的权重分配；
• $P_k^{{}^{\prime }}$ 是修正后的协方差矩阵，表示最佳状态估计的不确定性或者误差；
• $R_k$ 是测量噪声协方差矩阵，表示传感器的不确定性或者误差。