卡尔曼滤波(Kalman Filtering)

1、 入门

问题：一辆汽车已知初始位置、速度和加速度，且有卫星定位，如何求它精确的位置？

汽车行驶会受到风速、路况、本身等因素的影响，而卫星定位也有一定的偏差，卡尔曼滤波就是通过汽车预测的位置和卫星观测定位来结合，通过卡尔曼滤波使得数据更加真实。

1.1、适用的系统：线性高斯系统。

线性：$f=ax+b$, $z=ax+y$, $w=n^2+v^2$

线性的两个性质：(1) 叠加性：$f(a+b)=f(a)+f(b)$; (2) 齐次性：$f(a)=b-->f(ka)=kf(a)=kb$

高斯： 噪声满足正态分布

1.2、宏观意义：(滤波即加权)

理想状态：信号$\times$1+噪声$\times$0

低通滤波：低频信号为1，高频信号为0

卡尔曼滤波：估计值与观测值如何融合在一起进行修正能够得到更合理的答案。

1.3、 状态空间表达式

状态方程：$x_k=A{x}_{k-1}+B{u}_k+w_k$

观测方程：$y_k=C{x}_k+v_k$

(1) 参数分析

$w_k$$\in$$N(0,Q_k)$

$v_k\in N(0,R_k)$

其中 $Q_k和R_k$ 是超参数，需要手动调节来让模型得到优化。

2、卡尔曼滤波公式及实例分析

2.1、卡尔曼滤波公式

卡尔玛滤波的实现过程：通过使用上一次的最优结果来预测当前的值，同时使用观测值来修正当前值，从而得到最优结果。

卡尔曼滤波总结起来就5个公式：预测2个，更新3个，具体如下所示。

预测：

(1) 先验估计：$ \hat{x}t^-=F\hat{x}{t-1}+Bu_{t-1}$

(2) 先验估计协方差： $ P_t^-=F \hat{P}_{t-1} F^T+Q $

更新：

(3) 更新卡尔曼增益： $K_t=P_t^{-}{H}^T(H{P}_t^{-}{H}^T+R{)}^{-1}$

(4) 修正估计值 ${\hat{x}}_t={\hat{x}}_t^{-}+K_t(Z_t-H{\hat{x}}_t^{-1})$

(5) 更新后验估计协方差 $P_t=(I-K_{t}H)P_t^{-}$

2.2、实例分析

案例：卡尔曼滤波对小车运动轨迹的分析

该案例通过视频的方式详细的讲解了卡尔玛滤波的理论以及如何将理论运用到实际生活中，对学习卡尔曼滤波有着非常大的帮助，强烈推荐观看该视频。

**超参数的调节：**公式(3)简化后如下所示，如果模型中的修正估计值更加重视观测值，那么可以增大分子；否则，增大分母。

$$K=\frac{P_{t-1}+Q}{P_{t-1}+Q+R}$$

卡尔玛滤波方法的使用：

(1) 选择状态量，观测量

(2) 构建方程

(3) 初始化参数：Q，R，$x_0$等有关参数

(4) 代入公式迭代

(5) 调节超参数

注： 习惯取 $x_0$ 为0，P取值尽可能小，一般取1，不可为0。