【滤波算法】卡尔曼滤波手撕版本（推导+案例解释）

卡尔曼滤波（Kalman Filter, KF）是一种递归最小均方误差估计算法，适用于线性动态系统。它分为预测（Prediction) 和更新(Update)两个阶段。其中预测也就是所谓的先验，更新实际上就是后验。

---

先理解基础的概念，以及每一个变量的定义，理解了这些才能理解接下来的公式代表的含义：

1. Kalman 方程的理解

Kalman 滤波是一种递归最小均方误差（MMSE）估计算法，它能在噪声环境下从不完美的观测数据中估计系统状态。它常用于导航、信号处理、控制系统、目标跟踪等场景，尤其适用于线性动态系统。

1. Kalman 滤波的基本数学模型

Kalman 滤波的核心是基于状态空间模型的递归估计，它由主要由2组5个方程组成：

(1) 预测步骤 (Prediction)

预测当前时刻的状态：
$${\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{u}_k$$
预测协方差：
$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+Q_k$$

(2) 更新步骤 (Correction/Update)

计算卡尔曼增益：
$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}$$
更新状态估计：
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$$
更新协方差：
$$P_k=(I-K_k{H}_k)P_k^{-}$$

其中：

• $x_k$ 是系统的状态变量（如位置、速度等）
• $F_k$ 是状态转移矩阵，描述系统的运动
• $u_k$ 是控制输入
• $B_k$ 是控制输入矩阵
• $P_k$ 是状态估计误差的协方差矩阵
• $Q_k$ 是过程噪声协方差矩阵，可能和时间有关，但是可以简化为定值，比如下面的例子过程噪声是高斯白噪声
• $H_k$ 是观测矩阵，将状态映射到测量值
• $R_k$ 是测量噪声协方差矩阵，这个和传感器的噪声相关，一般可以看作服从高斯分布
• $K_k$ 是卡尔曼增益
• $z_k$ 是观测数据

---

我们先不着急强行理解，先看一下简单的例子

2. 具体实例：目标跟踪（1D 位置估计）

假设我们要估计一个物体的 1D 位置，目标运动服从匀速模型：

$$x_k=\left[\begin{array}{c}po{s}_k\\ ve{l}_k\end{array}\right]$$

其中：

• $po{s}_k$ 是目标的当前位置
• $ve{l}_k$ 是目标的速度

(1) 定义状态转移模型

假设时间间隔 $\Delta t$ ，状态转移矩阵$F$ 为：
$$F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

这里：

• 位置更新公式 $po{s}_k=po{s}_{k-1}+ve{l}_{k-1}\cdot \Delta t$
• 速度保持不变 $ve{l}_k=ve{l}_{k-1}$

假设没有外部控制输入，则 $B=0$。

(2) 定义观测模型

假设我们只能测量位置，这里观测量就是状态量，所以直接相等：
$$H=\left[\begin{array}{cc}1& 0\end{array}\right]$$
观测噪声$R$ 服从高斯分布。

(3) 过程噪声

假设加速度变化的标准差是 ${\sigma }_a$，那么过程噪声协方差矩阵：
$$Q=\left[\begin{array}{cc}\frac{1}{4}\Delta t^4{\sigma }_a^2& \frac{1}{2}\Delta t^3{\sigma }_a^2\\ \frac{1}{2}\Delta t^3{\sigma }_a^2& \Delta t^2{\sigma }_a^2\end{array}\right]$$

(4) 预测和更新

• 用预测方程计算 $pos$ 和 $vel$
• 结合测量值更新状态

---

3. Python 实现

import numpy as np
import matplotlib.pyplot as plt

# 初始化
dt = 1  # 采样时间间隔
F = np.array([[1, dt], [0, 1]])  # 状态转移矩阵
H = np.array([[1, 0]])  # 观测矩阵
Q = np.array([[0.01, 0.01], [0.01, 0.1]])  # 过程噪声
R = np.array([[1]])  # 观测噪声，1x1 的矩阵，服从高斯分布，均值为0， 方差为1， 没有协方差部分
P = np.eye(2)  # 误差协方差

# 初始状态
x = np.array([[0], [1]])  # 初始位置0，速度1

# 真实轨迹（加随机噪声）
np.random.seed(42)
real_positions = []
measurements = []
for i in range(50):
    x_true = i + np.random.randn() * 0.5  # 真实位置
    z = x_true + np.random.randn()  # 观测值
    real_positions.append(x_true)
    measurements.append(z)

# 进行Kalman滤波
filtered_positions = []
for z in measurements:
    # 预测
    x_pred = F @ x
    P_pred = F @ P @ F.T + Q
    
    # 计算卡尔曼增益
    K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
    
    # 更新
    x = x_pred + K @ (z - H @ x_pred)
    P = (np.eye(2) - K @ H) @ P_pred
    
    filtered_positions.append(x[0, 0])

# 画图
plt.plot(real_positions, label="真实轨迹")
plt.plot(measurements, 'o', label="测量值", alpha=0.5)
plt.plot(filtered_positions, label="Kalman滤波估计")
plt.legend()
plt.show()

---

4. 结果分析

• 原始测量值（带噪声）：观测值抖动较大。
• 真实轨迹：理想状态下的轨迹。
• Kalman 估计结果：经过滤波后，轨迹更加平滑，误差减小。

结果如下：

推导相关内容

如果想深度的理解卡尔曼滤波，最好能进行相关公式的推导，我整理了如下的内容，可以帮助大家理解一下，这5个公式怎么来的。

1. 预测步骤（Prediction Step）

预测步骤的目的是利用系统的状态转移模型，从上一时刻的状态预测当前时刻的状态，并对不确定性进行传播。

(1) 预测状态方程

$${\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{u}_k$$
物理意义：

• 由 上一时刻的估计状态 ( \hat{x}_{k-1} ) 通过系统模型 ( F_k ) 预测当前状态 ( \hat{x}_k^- )。
• 若系统受到控制输入 ( u_k )（例如加速度控制车辆），则通过 ( B_k u_k ) 对状态进行修正。

注： 这里的$\hat{x}$上的帽子代表估计的意思， $x_k^{-}$这里的右上角的负号代表先验的意思，所以${\hat{x}}_k^{-}$代表的意思就是先验估计值。

还是以匀速运动模型为例：
如果我们估计一个物体的 1D 位置和速度：
$$x_k=\left[\begin{array}{c}{\text{pos}}_k\\ {\text{vel}}_k\end{array}\right]$$
假设物体遵循 匀速运动模型，状态转移矩阵为：
$$F=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$
则：
$${\hat{x}}_k^{-}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\text{pos}}_{k-1}\\ {\text{vel}}_{k-1}\end{array}\right]=\left[\begin{array}{c}{\text{pos}}_{k-1}+{\text{vel}}_{k-1}\cdot \Delta t\\ {\text{vel}}_{k-1}\end{array}\right]$$
即：

• 位置 = 旧位置 + 速度 × 时间
• 速度保持不变（没有加速度输入）

---

(2) 预测协方差方程

$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+Q_k$$
物理意义：

• $P_k^{-}$ 代表状态估计的不确定性（协方差矩阵）。
• 误差随着时间传播，乘上 $F_k$ 是因为状态自身的演化会影响不确定性。
• 加上 $Q_k$（过程噪声），表示系统在运动过程中可能会受到外界干扰（如风、摩擦、控制误差）。

推导思路：
由状态转移：
$$x_k=F_k{x}_{k-1}+w_k$$
其中 $w_k$ 是过程噪声，其协方差为 $Q_k$，即：
$$E[w_k{w}_k^T]=Q_k$$
对误差协方差 $P_k^{-}$ 计算：
$$P_k^{-}=E[(x_k-{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}{)}^T]$$
代入 $x_k=F_k{x}_{k-1}+w_k$：
$$P_k^{-}=E[(F_k{x}_{k-1}+w_k-F_k{\hat{x}}_{k-1})(F_k{x}_{k-1}+w_k-F_k{\hat{x}}_{k-1}{)}^T]$$
展开，假设过程噪声 $w_k$ 与 $x_{k-1}$ 无关（独立），去掉交叉项:

$$E[(x_{k-1}-{\hat{x}}_{k-1})w_k^T]=0,E[w_k(x_{k-1}-{\hat{x}}_{k-1}{)}^T]=0$$
等到：
$$P_k^{-}=F_{k}E[(x_{k-1}-{\hat{x}}_{k-1})(x_{k-1}-{\hat{x}}_{k-1}{)}^T]F_k^T+E[w_k{w}_k^T]$$
即：
$$P_k^{-}=F_k{P}_{k-1}{F}_k^T+Q_k$$

---

2. 更新步骤（Update Step）

更新步骤的目的是结合测量值，修正预测值，使估计更接近真实值。

(3) 计算 Kalman 增益

$$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}$$
物理意义：

• $K_k$ 控制测量值对估计的影响程度。
• 若 $R_k$（测量噪声协方差）较大，说明测量不可靠，则 $K_k$ 会降低测量值的权重，使系统更依赖模型预测。
• 若 $P_k^{-}$（先验误差协方差）较大，说明预测不确定性高，则 $K_k$ 会增加测量值的权重。

推导思路（最小均方误差估计）：
观测方程：
$$z_k=H_k{x}_k+v_k$$
其中 $v_k\sim \mathcal{N}(0,R_k)$ 是观测噪声。

观测残差（创新量）：
$$y_k=z_k-H_k{\hat{x}}_k^{-}$$

使用卡尔曼增益 $K_k$ 进行修正：
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k{y}_k$$

误差更新：
$$e_k=x_k-{\hat{x}}_k=x_k-{\hat{x}}_k^{-}-K_k{y}_k$$

代入 $y_k=z_k-H_k{\hat{x}}_k^{-}$：
$$e_k=x_k-{\hat{x}}_k^{-}-K_k(H_k{x}_k+v_k-H_k{\hat{x}}_k^{-})$$

整理：
$$e_k=(I-K_k{H}_k)(x_k-{\hat{x}}_k^{-})-K_k{v}_k$$
目标是最小化更新后的估计误差：
$$x_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$$
设 $e_k=x_k-{\hat{x}}_k$ 为更新后的误差，最小化其均方误差：
$$E[e_k{e}_k^T]=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$$

这个就相当于计算误差协方差：
$$P_k=E[e_k{e}_k^T]$$

展开：
$$P_k=E[(I-K_k{H}_k)(x_k-{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}{)}^T(I-K_k{H}_k{)}^T+K_k{v}_k{v}_k^T{K}_k^T]$$
由于预测误差协方差$P_k^{-}=E[(x_k-{\hat{x}}_k^{-})(x_k-{\hat{x}}_k^{-}{)}^T]$，且测量噪声协方差 $R_k=E[v_k{v}_k^T]$，有：
$$P_k=(I-K_k{H}_k)P_k^{-}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T$$

令 $K_k$ 使 $P_k$ 最小, 令 $P_k$ 对 $K_k$ 求导，令其梯度为 0：

$$\frac{\partial P_k}{\partial K_k}=-H_k{P}_k^{-}(I-K_k{H}_k{)}^T+K_k{R}_k=0$$

整理得：
$$K_k(H_k{P}_k^{-}{H}_k^T+R_k)=P_k^{-}{H}_k^T$$

解得：

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

---

(4) 更新状态估计

$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$$
物理意义：

• $z_k-H_k{\hat{x}}_k^{-}$ 代表测量残差（创新），即测量值和预测值的偏差。
• $K_k$ 控制更新幅度，若测量可靠，则$K_k$ 增大，状态更倾向于测量值。

---

(5) 更新协方差矩阵

$$P_k=(I-K_k{H}_k)P_k^{-}$$
物理意义：

• 误差协方差减少，说明对状态的不确定性降低。
• $(I-K_k{H}_k)$ 作用是削弱测量维度的不确定性，即：
  • 若 $K_k$ 很大（测量可靠），则 $P_k$ 明显减小。
  • 若 $K_k$ 很小（预测更可靠），则 $P_k$ 变化不大。

推导思路：
假设新误差：
$$e_k=x_k-{\hat{x}}_k=x_k-{\hat{x}}_k^{-}-K_k(z_k-H_k{x}_k^{-})$$
带入 $z_k=H_k{x}_k+v_k$，其中 $v_k$ 是测量噪声：
$$e_k=x_k-{\hat{x}}_k^{-}-K_k(H_k{x}_k+v_k-H_k{x}_k^{-})$$
$$=(I-K_k{H}_k)(x_k-{\hat{x}}_k^{-})-K_k{v}_k$$
计算误差协方差：
$$P_k=E[e_k{e}_k^T]=(I-K_k{H}_k)P_k^{-}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T$$
可近似为：
$$P_k=(I-K_k{H}_k)P_k^{-}$$

---

总结

请记住以下5个方程， 5个方程共同作用，使得 Kalman 滤波可以高效、精准地估计系统状态。

步骤	公式	物理意义
预测状态	${\hat{x}}_k^{-}=F_k{\hat{x}}_{k-1}+B_k{u}_k$	由上一时刻推测当前状态
预测协方差	$P_k^{-}=F_k{P}_{k-1}{F}_k^T+Q_k$	传播误差
计算增益	$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}$	平衡预测和测量的权重
更新状态	${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$	融合测量数据
更新协方差	$P_k=(I-K_k{H}_k)P_k^{-}$	误差减少

Kalman 滤波的优势：

• 适用于动态系统的状态估计
• 能够处理测量噪声和过程噪声
• 计算复杂度低，适合实时应用

适用条件：

• 线性系统（非线性系统需扩展到 EKF/UKF）
• 过程噪声和观测噪声服从高斯分布

对于更复杂的应用，如非线性系统、IMU 传感器融合等，可考虑 扩展 Kalman 滤波（EKF）或无迹 Kalman 滤波（UKF）。