卡尔曼滤波器详解 + 公式推导 + 手动实现 + cv2 实现

一、基础概念

卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。

卡尔曼滤波适用于 线性高斯系统，即该系统必须是线性的，而且噪声服从正态分布。更详细一些，噪声通常被建模为一个均值为 0、方差为常数的正态分布，也就是高斯分布。该正态分布的横坐标是随机变量的取值，纵坐标是对应取值的概率密度。

协方差相关知识可以参考这篇文章

1. 公式

卡尔曼滤波的过程分为 predict 和 update 两部分

1. predict 通过之前的状态量和分布来预测当前时刻的状态 $$\begin{array}{rl}{x}_t^{-}& =F{\hat{x}}_{t-1}+B{u}_{t-1}+W_t\\ P_t^{-}& =F{P}_{t-1}{F}^T+Q_t\end{array}$$
2. update 步骤利用观测值 和 predict 得出的预测值更新当前时刻的状态量以及协方差等参数。$$\begin{array}{rl}{K}_t& =\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}\\ {\hat{x}}_t& =x_t^{-}+K_t(z_t-H{x}_t^{-})\\ P_t& =(I-K_{t}H)P_t^{-}\end{array}$$

2. 推导

1. Predict 公式推导：

   • 对于公式（1）：$x_t^{-}$ 表示 $t$ 时刻的状态预测值（先验估计），$F$ 是转换矩阵（状态转移矩阵），$u_t$ 是 $t$ 时刻的控制输入向量，$B$ 是控制矩阵，$W_t\sim N(0,Q)$ 是过程噪声（系统噪声）。这个公式可以理解为从前一时刻的状态根据一个输入推测当前状态的过程，举个例子，假设 $x$ 是一个物体移动的路程，$u$ 是速度，那么该公式可以写为：$$x_t^{-}=x_{t-1}+\Delta t\cdot u_{t-1}+W_t$$此时 $F=1$（或者单位矩阵），$B=\Delta t$，但是放在现实世界，输入和计算过程不可避免的存在误差（噪声），所以别忘了加上 $W$。需要注意的是，$x$ 是由滤波器自动推理出的值，不是输入，输入是 $u_t$，但是需要设置一个初始值 $x_0$。

   • 对于公式（2）：$P_t^{-}$ 是先验估计协方差，$Q_t$ 是过程噪声的方差。这一个公式是根据之前的协方差推理出当前时刻的协方差预测值，推导如下：$$cov({\hat{x}}_{t-1},{\hat{x}}_{t-1})=P_{t-1}$$ 把公式（1）带入：$$\begin{array}{rl}{P}_t^{-}& =cov(x_t^{-},x_t^{-})\\ & =cov(F{\hat{x}}_{t-1}+B{u}_{t-1}+W_t,F{\hat{x}}_{t-1}+B{u}_{t-1}+W_t)\end{array}$$
     要注意的是，此时 $F,B$ 是固定值，常量，$u_{t-1}$ 也是每一次手动输入的，也是常量，变量只有 ${\hat{x}}_{t-1},W_t$ 所以上式可以变成：$$\begin{array}{rl}& =Fcov({\hat{x}}_{t-1},{\hat{x}}_{t-1})F^T+cov(W_t,W_t)\\ & =F{P}_{t-1}{F}^T+Q_t\end{array}$$

2. Update 公式推导：$K$ 是卡尔曼增益，相当于一个权重，$H$ 是观测矩阵（测量值权值矩阵），$z=H{x}_t+V_t$ 是测量值，$x_t$ 是真实状态，$V_t\sim N(0,R)$ 是测量噪声。真实状态 $x_t$ 状态是无法获得的，我们只能通过卡尔曼滤波去近似，换句话说，卡尔曼滤波的目标就是使 ${\hat{x}}_t$ 尽可能接近 $x_t$。
   根据公式 $z_t=H{x}_t+V_t$，我们可以认为 $x_t$ 的一个来源是：$$x_t^{\prime }=H^{-1}{z}_t$$这里没有加上 $V_t$ 是因为我们把 $z_t$ 当作一个随机向量处理，它服从正态分布 $N({\bar{z}}_t,R_t)$，其中 ${\bar{z}}_t$ 表示当前时刻的测量值。同时根据公式（1），我们知道 $x_t$ 的另一个来源：$$x_t^{\prime \prime }=F{x}_{t-1}+B{u}_t+W_t$$通过之前的公式我们还可以知道：$$\begin{array}{rl}{x}_t^{\prime }& \sim N(H_t^{-1}{\bar{z}}_t,H_t^{-1}{R}_t(H_t^{-1}{)}^T)\\ x_t^{\prime \prime }& \sim N(x_t^{-},P_t^{-})\end{array}$$这两个状态的来源我们认为是独立的，现在我们要把这两个来源统一起来，也就是计算它的联合概率分布（高斯分布的乘积依旧是高斯分布），则新的高斯分布的均值和方差如下：$$\begin{array}{rl}\mu & =\frac{{\mu }_2{\sigma }_1^2+{\mu }_1{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\\ {\sigma }^2& =\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}\end{array}$$将两个来源的均值方差带入如下：$$\begin{array}{rl}{\hat{x}}_t=\mu & =x_t^{-}+\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}({\bar{z}}_t-H_t{x}_t^{-})\\ & =x_t^{-}+K_t(z_t-H{x}_t^{-})\end{array}$$$$P_t={\sigma }^2=P_t^{-}-K_t{H}_t{P}_t^{-}$$这里一定要注意我们把先验估计值 ${\hat{x}}_t$ 看作是正态分布均值的思想。

二、卡尔曼滤波的应用

卡尔曼滤波的使用步骤：

1. 确定状态量 $x$、观测量 $z$、输入 $u$
2. 构建方程，确定 $F,B$
3. 初始化参数 ${\hat{x}}_0,P_0,Q,R$
4. 带入公式迭代
5. 调节超参

如何调节超参
卡尔曼滤波的超参数有 $Q,R,{\hat{x}}_0,P_0$，我们分开来看：

• 调节 $Q,R$：结合公式 $P_t^{-}=F{P}_{t-1}{F}^T+Q_t$ 和 $K_t=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$，我们可以粗略认为 $K\propto \frac{P_{t-1}+Q}{P_{t-1}+Q+R}$。由此可以分析出：$R$ 越大，$K$ 越小；$Q$ 越大，$K$ 越大。再结合公式 ${\hat{x}}_t=x_t^{-}+K_t(z_t-H{x}_t^{-})$，我们认为 $K$ 越大我们越信任观测值，也就是说，当我们使用的仪器很精密，观测噪声很小时，我们越信任观测值，此时应该调小 $R$，反之则调大，另外，当我们的运动模型越准确时，我们的预测值会越精确，此时我们越信任预测值，则应该调小 $Q$
• 调节 ${\hat{x}}_0,P_0$：在卡尔曼滤波的过程中，这两个值会逐渐趋于稳定，一般来说，$x_0=0$，$P_0$ 会取一个比较小的值，但是不能为 0，通常情况下取 1

三、手动实现 + cv2 实现

现有如下场景：我们通过毫米波雷达观测到了很多点云数据，但是由于毫米波雷达在 $y$ 轴上不精准的问题，导致点云抖动幅度很大，此时我们需要通过卡尔曼滤波来让抖动尽量平滑，尽量预测出实际的 $y$ 轴坐标。由于只需要预测 $y$ 轴坐标，所以只需要一个简单的一维卡尔曼滤波。

1. 确定状态量 $x$、观测量 $z$、输入 $u$： 状态量即为要预测的值，也就是 $y$ 轴坐标，观测量就是我们观测到的 $y$ 轴坐标，输入量没有（如果能获取到抖动的瞬时速度等数据，则这些数据就可以作为输入）
2. 构建方程，确定 $F,B$： $x_t^{-}={\hat{x}}_{t-1}$ 由于没有输入，而且 $F=1$，所以整个公式都变得简单了起来。而且由于 $z_t=x_t+V_t$，所以 $H=1$
3. 初始化参数 ${\hat{x}}_0,P_0,Q,R$： 经过调参，最终的值确定为 $x_0=0,P_0=1,Q=0.1,R=8$

代码如下：

def kalman(self, z, x_last=0, p_last=1, Q=0.1, R=8):
    x_mid = x_last
    p_mid = p_last + Q
    k = p_mid / (p_mid + R)
    x_now = x_mid + k * (z - x_mid)
    p_now = (1 - k) * p_mid
    return x_now, p_now

调用 cv2 实现时需要初始化的参数如下：

############################# 初始化 #############################
kf = cv2.KalmanFilter(4, 2, 1)  # 状态变量维度为 4，观测变量维度为 2, 控制输入向量维度为 1
kf.transitionMatrix = np.array([[1, 0, 1, 0],
                                [0, 1, 0, 1],
                                [0, 0, 1, 0],
                                [0, 0, 0, 1]], np.float32)  # 状态转移矩阵 F
kf.controlNoiseCov = np.array([[1], np.float32) * 0.01  # 控制输入矩阵 B
kf.measurementMatrix = np.array([[1, 0, 0, 0],
                                 [0, 1, 0, 0]], np.float32)  # 测量矩阵 H
kf.processNoiseCov = np.array([[1, 0, 0, 0],
                               [0, 1, 0, 0],
                               [0, 0, 1, 0],
                               [0, 0, 0, 1]], np.float32) * 0.001  # 过程噪声协方差矩阵 Q
kf.measurementNoiseCov = np.array([[1, 0],
                                   [0, 1]], np.float32) * 0.01  # 测量噪声协方差矩阵 R
# 初始化 x_0 和 P_0
kf.statePost = np.zeros((4, 1), np.float32)
kf.errorCovPost = np.ones((4, 4), np.float32)

############################# 迭代 #############################
kf.correct(z, u)  # update 操作，z 是观测值，u 是控制输入向量
xt = kf.predict()  # 预测下一个值 x_t