卡尔曼滤波器

什么是卡尔曼滤波器（Kalman Filter）

虽然名字里带了“滤波”，但其实卡尔曼滤波器是一种状态优化估算算法（optimal estimation algorithm）。

为什么使用KF

• 当我们有很多种数据来源时，使用KF进行多数据融合

  比如汽车：陀螺仪积分得到位移、里程计、GPS，那么通过KF就可以结合这3个数据，得到最大概率位置

• 当需要测量无法测量物理量时，使用KF估计状态

  比如火箭发动机内温：可以测量火箭发动机外壳温度，再利用导热定律求解内温；也可以通过燃料的使用量和燃料的燃烧热计算出内温。这都是间接计算内温，那么真正的内温是多少？还是使用KF得出最终温度（这好像还是多数据融合）

KF原理

状态观测器

可以将下图理解为：y是火箭发动机外壳温（可直接测量数据），x为火箭发动机内温（不可测量物理量），u为火箭燃料流量（系统输入值）

假设有一个线性系统(使用状态方程表示)

$$
\dot x = Ax+Bu \\
y=Cx+Du
$$

所以，我们对该系统建立一个数学上相同的预测模型(可以理解为运行在控制器里)

$$
\dot {\hat x} = A\hat x+Bu \\
\hat y=C\hat x+Du
$$

那么如图所示，由于采用误差反馈的方式，则预测模型变为：

$$
\dot {\hat x} = A\hat x+Bu+K(y-\hat{y}) \\
\hat y=C\hat x+Du
$$

将原系统和该模型联立（相减）可求：

$$
e=x-\hat{x} \\
\begin{cases}
\dot x-\dot {\hat x}=A(x-\hat x)-K(y-\hat{y}) \\
y-\hat{y}=C \cdot e
\end{cases} \\ \Rightarrow
\dot e=(A-KC)e \\
\therefore e = \exp[(A-KC)t]\cdot e(0)
$$

那么通过调整K的值，就能调整误差收敛的速度，也就是模型预测值接近实际值的速度

最佳状态估计器（KF）

上面讨论的状态观察器中，调整K就能调整误差收敛速度，那么K如何选取呢？通过数据的概率统计优劣，也就是数据的方差，来选择更加可信的数据，也就是K（数据选择比）

对于第一个状态观察模型，是没有考虑概率误差（噪声）的，而现实生活中往往存在噪声（大自然最常见的：高斯白噪声）！如果没有噪声，似乎K的选择只要加速逼近速度就好了，那么K越大不是就越好了？

类上，此处建立了一个离散线性状态方程，且是伴随噪声的系统：

$$
x_k = A\cdot x_{k-1}+B\cdot u_k+w_k \\
y_k = C\cdot x_k + v_k
$$

可以很放心的把噪声假设为高斯白噪声：

$$
w_k \sim N(0,Q) \\
v_k \sim N(0,R)
$$

那么同样的，可以对该系统建立预测模型（数理知识真是太重要了）

$$
\hat x_k = A\cdot \hat x_{k-1}+ B\cdot u_k \\
\hat y_k = C\cdot \hat x_k
$$

也同样再引入误差反馈

$$
e_k = y_k - \hat y_k = y_k - C \cdot (A \cdot \hat x_{k-1}+ B\cdot u_k) \\
\hat x_k = A \cdot \hat x_{k-1}+ B \cdot u_k + K_k \cdot e_k
$$
则有
$$
{\hat x}^-_k= A \cdot \hat{x} _{k-1}+ B \cdot u_k \\
\hat x_k = \hat x^-_k + K_k(y_k - C \hat x^-_k)
$$

将概率特性（协方差矩阵）带入可得：

$$
P_k = (I-K_kC)P^- _k \\
P^-_k = AP_{k-1}A^T +Q
$$

增益

$$
K_k=\frac{P^-_k C^T}{CP^-_k C^T + R} \\
$$

增益理解

• 当输出方程方差无穷小时

  $$
  \lim_{R\to 0} K_k =\frac{1}{C} \\
  \hat{x}_k = y_k
  $$

  也就是说当输出方程方差趋近0时，显然测量值更加可靠，那么KF的输出值就应该更加依赖测量值

• 当迭代方程方差无穷小时

  $$
  \lim_{P^-_k \to 0} K_k =0 \\
  \hat{x}_k = \hat{x}_k^-
  $$

  也就是状态迭代方程误差趋近于0，显然预测模型计算的结果更加可靠，那么KF的输出就应该更加依赖预测值

KF计算方法

上面的论述已经非常详细给出了KF的原理，此处给出工程中（比如在微控制器上）实际计算KF的迭代顺序：

预测部分

• 根据预测模型计算预测的（先验）结果值

  $$
  \hat{x} _k^-= A\cdot \hat{x} _{k-1}+ B\cdot u_k
  $$

• 计算出预测结果值的协方差矩阵

  $$
  P^- _k = AP _{k-1}A^T +Q
  $$

更新部分

• 根据统计特性（方差=可信度）计算K值

  $$
  K_k=\frac{P^-_k C^T}{CP^-_k C^T + R}
  $$

• 根据确定的K值，计算（后验）结果值

  $$
  \hat{x}_k = \hat{x}_k^- + K_k(y_k-C\hat{x}_k^-)
  $$

• 计算（后验）协方差矩阵

  $$
  P_k = (I-K_kC)P^-_k
  $$

不停重复交替，迭代计算以上两个部分，就是KF的工作原理了

非线性KF

上面对KF的讨论中，使用的都是状态方程，而显然状态方程是表示线性系统的！对于非线性系统，KF就是另一个故事了，但是我们可以观察KF方程形式，其中被非线性系统影响到的系统矩阵A、B、C，相当于导数

考虑泰勒展开来近似曲线，同理可以使用Jacobian矩阵来替代这里的A、B、C矩阵

扩展卡尔曼滤波器(Extended Kalman Filter)

有非线性系统：
$$
x_k = f(x_{k-1},u_k)+w_k \\
y_k = g(x_k)+v_k
$$

该系统的Jacobian：

$$
F=\frac{\partial f}{\partial x}|_{\hat{x}_{k-1},u_k} \\
G=\frac{\partial g}{\partial x}|_{\hat{x}}
$$

做线性近似（一阶泰勒）
$$
\Delta x_k \simeq F\Delta x_{k-1} + w_k \\
\Delta y_k \simeq G\Delta x_k +v_k
$$

所以，一阶泰勒近似的缺点，也就是EKF的缺点：不连续点处无效、极度非线性区域无效…

无迹卡尔曼滤波器（Unscented Kalman Filter）

选取符合原统计特性的数个样点，将样点通过非线性系统，得到的新统计特性作为计算时使用的统计特性

粒子滤波器（Particle Filter）

样点非常多，以符合你想要的所有的概率统计特性，然后就是类似于UKF的工作

参考资料

• 知乎: 如何通俗并尽可能详细地解释卡尔曼滤波？