卡尔曼滤波——一种基于滤波的时序状态估计方法

本文是对 How a Kalman filter works, in pictures一文学习笔记，主要是提炼核心知识，方便作者快速回忆和复习，有需要的小伙伴欢迎收藏和讨论~

1. Kalman滤波及其应用

作用： 预测一个具有不确定信息（如：噪声）的动态系统的下一步状态。
应用： 无人系统导航、定位和跟踪等；毫米波雷达定位。

2. Kalman原理

卡尔曼滤波实质： 是将预测状态量的高斯分布和观测量的高斯分布做融合，生成一个新的高斯分布，其中新的高斯分布的均值和方差是两个独立的高斯分布的相关参量的加权，这个加权就是卡尔曼增益，但是预测状态量和观测量可能维度不同，需要将他们同时转换到一个向量空间中，所以观测量前面有线性变换矩阵。

公式推导：

Step 1：模型建立

式1表达的是每个$x_k$都可以通过一个线性随机方程估计出来。任意$x_k$都是其前一时刻的值与过程噪音的线性组合（这个很难概念化）。请记住，大部分情况下该式没有控制信号$u_k$项。

式2告诉我们任何测量值$z_k$（无法确定精确与否的测量值）都是信号值与测量噪声的线性组合。这两个分量符合高斯分布。（这是可以使用Kalman滤波的前提） 进程噪声与测量噪声互相统计独立。$A$，$B$和$H$是一般形式的矩阵。但在大多数信号处理问题中，这些量仅为数值。而且虽然这些值在状态变换时会改变，大多数情况下我们都可以假设他们为定值。
$$\begin{gathered} x_k=A{x}_{k-1}+B{u}_k+w_{k-1} \\ {z}_k=H{x}_k+v_k \end{gathered}$$
如果我们十分确定我们的系统符合此模型，那么唯一剩下要做的事就是估计噪音函数$w_{k-1}$和$v_k$的平均值以及标准差。我们知道，在实际生活中没有信号满足高斯分布，但我们可以近似其为高斯分布。

Step 2：开始Kalman滤波

Kalman滤波核心公式有5个。
时间更新方程组（用于预测）
$$\begin{gathered} \widehat{x_k}=A\widehat{x_{k-1}}+B{u}_k \\ \overline{P_k}=A{P}_{k-1}{A}^T+Q \end{gathered}$$
测量更新方程组（用于修正）
$$\begin{gathered} K_k=\overline{P_k}{H}^T(H\overline{P_k}HT+R{)}^{-1} \\ \widehat{x_k}=\overline{\hat{x_k}}+K_k(z_k-H\overline{\hat{x_k}}) \\ {P}_k=(I-K_{k}H)\overline{P_k} \end{gathered}$$
建模部分已经在步骤一完成了，所以矩阵$A$，$B$和$H$已知。这些矩阵很可能是一个常数，而且大部分情况下会等于1。
最让人难受的部分就是决定$R$和$Q$的值了。$R$的值还是很容易找的，因为一般情况下我们对环境中的噪音还是能够确认的。（起码能用仪器测一下）。但是找$Q$的值就没那么直观了。为了使滤波器能够运行，我们还需要知道$x_0$和$P_0$的估计值。

Step 3：迭代滤波

在获得了滤波器运行所需的所有信息后，我们就可以估值迭代了。记住：前一状态的估值将成为当前状态的输入。

一般超过50次迭代后，就会比较好的收敛了。