4、线性卡尔曼滤波（LKF）实现：一维与多维卡尔曼滤波的Python实现，以无人机高度估计为例。
好，咱们今天来点硬核的。前面讲了那么多理论，什么状态空间、协方差矩阵、贝叶斯推断……说实话，光看公式容易晕。我当年刚接触卡尔曼滤波时，也是对着那五个公式发愣了好久。
后来我发现，真正让我理解卡尔曼滤波的，不是数学推导，而是亲手写一遍代码。看着数据从噪声中一点点收敛到真实值，那种感觉，嗯，很踏实。
所以这一章，咱们就动手。以无人机高度估计为例，从最简单的一维情况开始，再到多维。代码我会逐行解释，你跟着敲一遍，保证通透。
4.1 一维卡尔曼滤波：无人机高度估计
先想一个最简单的场景。你的无人机悬停在空中，高度大概是10米。但传感器有噪声，每次测量值都在10米上下跳来跳去。你该怎么估计真实高度？
一维卡尔曼滤波就是干这个的。说白了，它只有两个变量：状态（高度）和方差（不确定度）。
4.1.1 问题建模
我们定义：
- 真实高度：\( x = 10 \) 米（我们不知道，但算法要估计它）
- 测量值：\( z_k = x + v_k \)，其中 \( v_k \) 是高斯噪声，标准差 \( \sigma_z = 0.5 \) 米
- 系统模型：假设无人机静止，所以状态转移是恒等变换（\( A = 1 \)）
- 控制输入：无（\( u = 0 \)）
我个人习惯，在写代码前先把物理模型理清楚。你想想看，如果模型都错了，滤波再漂亮也没用。
4.1.2 一维卡尔曼滤波的五个核心公式
这里我直接给出离散形式，咱们对着代码看：
- 预测状态：\( \hat{x}_{k|k-1} = A \hat{x}_{k-1|k-1} + B u_k \)
- 预测协方差：\( P_{k|k-1} = A P_{k-1|k-1} A^T + Q \)
- 卡尔曼增益：\( K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} \)
- 更新状态：\( \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) \)
- 更新协方差：\( P_{k|k} = (I - K_k H) P_{k|k-1} \)
别怕，一维情况下这些矩阵都退化成标量，简单得很。
4.1.3 Python实现：一维高度估计
import numpy as np
import matplotlib.pyplot as plt
# 真实值
true_height = 10.0
# 测量噪声标准差
sigma_z = 0.5
# 过程噪声标准差（我们假设无人机有轻微扰动）
sigma_q = 0.1
# 初始化
x_hat = 0.0 # 初始状态估计（完全不知道高度）
P = 1.0 # 初始估计方差（很大，表示不确定）
A = 1.0 # 状态转移矩阵
H = 1.0 # 观测矩阵
Q = sigma_q**2 # 过程噪声协方差
R = sigma_z**2 # 测量噪声协方差
# 模拟数据
np.random.seed(42)
n_steps = 50
measurements = true_height + np.random.normal(0, sigma_z, n_steps)
# 存储结果
estimates = []
variances = []
# 卡尔曼滤波循环
for z in measurements:
# 1. 预测
x_pred = A * x_hat
P_pred = A * P * A + Q
# 2. 更新
K = P_pred * H / (H * P_pred * H + R)
x_hat = x_pred + K * (z - H * x_pred)
P = (1 - K * H) * P_pred
estimates.append(x_hat)
variances.append(P)
# 绘图
plt.figure(figsize=(10, 6))
plt.plot(measurements, 'ro', markersize=3, label='测量值（带噪声）')
plt.plot(estimates, 'b-', linewidth=2, label='卡尔曼滤波估计')
plt.axhline(y=true_height, color='g', linestyle='--', label='真实高度')
plt.xlabel('时间步')
plt.ylabel('高度 (m)')
plt.legend()
plt.title('一维卡尔曼滤波：无人机高度估计')
plt.grid(True)
plt.show()关键观察：
- 初始时估计值从0开始，但很快收敛到10附近
- 方差P从1逐渐减小，说明我们对估计越来越有信心
- 卡尔曼增益K一开始很大（相信测量），后来变小（相信模型）
我的经验：我在做四旋翼飞控时，发现初始方差P设得太大，收敛会慢；设得太小，容易过拟合噪声。一般取测量噪声方差的2-5倍作为初始值，效果不错。
4.2 多维卡尔曼滤波：加入速度估计
一维的情况太理想了。现实中，无人机高度会变化，比如爬升或下降。这时候只估计高度就不够了，还得估计速度。
为什么？因为如果你只测高度，当无人机匀速上升时，测量值会一直偏大，滤波会滞后。我踩过这个坑——有一次无人机在风场中飘移，高度估计一直跟不上，差点撞树。
4.2.1 状态向量扩展
我们把状态从标量变成向量：
\[ \mathbf{x} = \begin{bmatrix} h \\ v \end{bmatrix} \]
其中 \( h \) 是高度，\( v \) 是垂直速度。
状态转移矩阵变成：
\[ A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \]
这个矩阵的含义是：新高度 = 旧高度 + 速度 × 时间间隔。速度不变（匀速模型）。
4.2.2 Python实现：二维高度-速度估计
import numpy as np
import matplotlib.pyplot as plt
# 参数
dt = 0.1 # 时间步长，秒
true_height = 10.0
true_velocity = 2.0 # 匀速上升，m/s
# 噪声参数
sigma_z = 0.5 # 测量噪声标准差（高度）
sigma_q_h = 0.1 # 过程噪声：高度扰动
sigma_q_v = 0.05 # 过程噪声：速度扰动
# 初始化
x_hat = np.array([[0.0], [0.0]]) # [高度, 速度]
P = np.array([[1.0, 0.0],
[0.0, 1.0]]) # 初始协方差
# 系统矩阵
A = np.array([[1.0, dt],
[0.0, 1.0]])
H = np.array([[1.0, 0.0]]) # 只测量高度
Q = np.array([[sigma_q_h**2, 0.0],
[0.0, sigma_q_v**2]])
R = np.array([[sigma_z**2]])
# 模拟数据
np.random.seed(42)
n_steps = 100
true_states = []
measurements = []
for k in range(n_steps):
true_h = true_height + true_velocity * k * dt
true_states.append(true_h)
z = true_h + np.random.normal(0, sigma_z)
measurements.append(z)
# 卡尔曼滤波
estimates = []
for z in measurements:
# 预测
x_pred = A @ x_hat
P_pred = A @ P @ A.T + Q
# 更新
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_hat = x_pred + K @ (np.array([[z]]) - H @ x_pred)
P = (np.eye(2) - K @ H) @ P_pred
estimates.append(x_hat[0, 0])
# 绘图
plt.figure(figsize=(10, 6))
plt.plot(measurements, 'ro', markersize=3, label='测量值')
plt.plot(estimates, 'b-', linewidth=2, label='卡尔曼滤波估计')
plt.plot(true_states, 'g--', linewidth=2, label='真实高度')
plt.xlabel('时间步')
plt.ylabel('高度 (m)')
plt.legend()
plt.title('二维卡尔曼滤波：高度与速度联合估计')
plt.grid(True)
plt.show()注意：多维情况下，矩阵求逆是性能瓶颈。实际嵌入式系统中，我一般用Cholesky分解代替直接求逆，能快3-5倍。另外，注意检查矩阵是否奇异——我曾经因为Q矩阵设得太小，导致P_pred奇异，程序直接崩了。
4.3 一维 vs 多维：什么时候用哪个？
| 场景 | 推荐维度 | 原因 |
|---|---|---|
| 悬停定高 | 一维（仅高度） | 速度接近0，模型简单，计算快 |
| 匀速爬升/下降 | 二维（高度+速度） | 需要估计速度来消除滞后 |
| 加速机动 | 三维（高度+速度+加速度） | 需要加速度来跟踪变化 |
| GPS+气压计融合 | 四维以上 | 多传感器，需要更多状态量 |
我个人建议：能用低维就别用高维。维度越高，计算量越大，而且调参越困难。我见过有人把状态扩展到10维，结果滤波器发散得一塌糊涂。
4.4 调参经验谈
卡尔曼滤波有三个关键参数：Q（过程噪声）、R（测量噪声）、P0（初始协方差）。
- R：这个最好确定。拿传感器静态测量几分钟，算一下方差就行。
- Q：这个最难调。我一般先设一个很小的值，然后看滤波响应。如果响应太慢（滞后），就增大Q；如果太抖（跟随噪声），就减小Q。
- P0：设大一点没关系，反正会收敛。我习惯设成R的10倍。
避坑指南：我曾经在调试一个无人机高度估计时，发现滤波结果总是偏小。查了半天，原来是Q设得太小，导致滤波器过于相信模型，忽略了测量。后来把Q增大了一个数量级，问题立刻解决。
4.5 小结
这一章我们亲手实现了卡尔曼滤波。从一维到多维，从理论到代码。你想想看，其实卡尔曼滤波没那么神秘——它就是一个聪明的加权平均器，根据当前的不确定度，动态调整对测量和模型的信任程度。
下一章，我们会把卡尔曼滤波扩展到非线性系统，也就是扩展卡尔曼滤波（EKF）。到时候，无人机的高度估计会加入加速度计和气压计的数据融合，更有意思。
嗯，今天就到这里。代码建议你多跑几遍，改改参数，看看效果变化。动手是最好的学习方式。