卡尔曼滤波器（Kalman Filter）算法原理

卡尔曼滤波是一种“动态加权平均”的算法，它能让系统在噪声中看清真实信号，是“能自动权衡信号与噪声”的智能滤波算法，。

一、卡尔曼滤波的直观理解

在现实系统中，我们常常要从噪声观测数据中估计一个真实状态。
例如：

• GPS 信号有抖动；
• 陀螺仪数据有漂移；
• 雷达测距存在随机误差；
• 电机编码器输出有毛刺。

如果我们直接使用这些原始数据，会导致系统控制不稳定。
于是我们希望有一种方法能从嘈杂的观测中估计出真实状态。

传统的均值滤波、滑动平均滤波虽然简单，但响应慢、延迟大。
而卡尔曼滤波（Kalman Filter）则能在平滑和实时性之间找到最优平衡。

我们希望得到更平滑、准确的结果——这就是滤波器要做的事。

而卡尔曼滤波器（Kalman Filter, KF）的特别之处在于：

它利用了“数学建模 + 统计推理”，动态地在“预测值”和“测量值”之间做最优加权。

二、核心思想：动态加权平均

卡尔曼滤波是一种递推估计算法，每次迭代分为两步：

1. 预测（Predict）：根据模型预测下一时刻状态；
2. 更新（Update）：根据测量修正预测结果。

这两个过程交替进行，让系统估计值不断逼近真实值。

三、基本模型

系统状态可以抽象为：

'''katex
\begin{cases}
xk = A x{k-1} + B u_k + w_k \
z_k = H x_k + v_k
\end{cases}
'''

其中：

假设 $w_k$ 与 $v_k$ 都是零均值高斯白噪声：

'''katex
w_k \sim N(0, Q), \quad v_k \sim N(0, R)
'''

四、算法步骤详解

卡尔曼滤波的每一轮计算分为“预测”和“更新”两步：

🧩 1. 预测阶段（Predict）

预测下一时刻的状态与协方差：

'''katex
\hat{x}{k|k-1} = A\hat{x}{k-1|k-1} + B uk
'''
'''katex
P{k|k-1} = A P_{k-1|k-1} A^T + Q
'''

意思是：根据模型预测状态，同时考虑过程噪声导致的不确定性。

🧩 2. 更新阶段（Update）

得到测量值 $z_k$ 后，进行修正：

计算卡尔曼增益（Kalman Gain）：

'''katex
Kk = P{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}
'''

更新状态估计：

'''katex
\hat{x}{k|k} = \hat{x}{k|k-1} + K_k (zk - H\hat{x}{k|k-1})
'''

更新协方差：

'''katex
P_{k|k} = (I - Kk H) P{k|k-1}
'''

五、通俗理解 —— “信任谁更多？”

卡尔曼滤波的灵魂在于卡尔曼增益 $K_k$：

• 如果测量噪声大（$R$ 大），算法更信任模型预测；
• 如果模型误差大（$Q$ 大），算法更信任测量结果。

这就像一个有经验的人在做判断：

“我有预测，但我也听听传感器的意见。”
“两边都不太准？那我折中一点。”

六、一维卡尔曼滤波的简化形式

考虑一个简单场景：
小车匀速行驶，测量位置带噪声。

'''katex
xk = x{k-1} + v + w_k \
z_k = x_k + v_k
'''

简化算法如下：

# 一维卡尔曼滤波伪代码
x_hat = 0      # 初始估计
P = 1          # 初始不确定性
Q = 0.01       # 过程噪声协方差
R = 0.1        # 测量噪声协方差

while True:
    # 1. 预测
    x_hat = x_hat + v
    P = P + Q

    # 2. 更新
    K = P / (P + R)
    x_hat = x_hat + K * (z - x_hat)
    P = (1 - K) * P

## 四、算法结构小结

| 步骤 | 名称 | 公式 | 含义 |
|------|------|------|------|
| 1 | 状态预测 | $\hat{x}_{k|k-1} = A\hat{x}_{k-1|k-1} + Bu_k$ | 根据模型预测下一状态 |
| 2 | 协方差预测 | $P_{k|k-1} = AP_{k-1|k-1}A^T + Q$ | 不确定性传播 |
| 3 | 卡尔曼增益 | $K_k = P_{k|k-1}H^T(HP_{k|k-1}H^T+R)^{-1}$ | 计算信任比例 |
| 4 | 状态更新 | $\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k(z_k - H\hat{x}_{k|k-1})$ | 融合测量结果 |
| 5 | 协方差更新 | $P_{k|k} = (I-K_kH)P_{k|k-1}$ | 更新不确定性 |

---

## 五、参数解释

| 符号 | 含义 |
|------|------|
| $A$ | 状态转移矩阵（系统模型） |
| $B$ | 控制输入矩阵 |
| $H$ | 观测矩阵 |
| $Q$ | 过程噪声协方差（模型不确定性） |
| $R$ | 测量噪声协方差 |
| $P$ | 估计协方差矩阵（置信度） |

---

## 六、一维位置估计示例（最简单情况）

假设你的小车以恒速行驶：

$$
x_k = x_{k-1} + v + w_k
$$

测量系统读数为：
$$
z_k = x_k + v_k
$$

其中：
- $w_k \sim N(0, Q)$：过程噪声；
- $v_k \sim N(0, R)$：测量噪声。

卡尔曼滤波更新如下：

```pseudo
初始化：
x̂ = 初始位置
P = 初始协方差

循环：
    # 预测
    x̂ = x̂ + v
    P = P + Q

    # 更新
    K = P / (P + R)
    x̂ = x̂ + K * (z - x̂)
    P = (1 - K) * P