3、多维卡尔曼滤波：矩阵形式推导、状态向量与协方差矩阵、Python实现多维KF

好，咱们进入第三讲。说实话，很多同学学卡尔曼滤波，卡就卡在多维这一关。一维的还能看懂，一扩展到矩阵就懵了。我当年刚接触时也差不多，看着满屏的矩阵符号，心里直打鼓。但后来我发现，只要把矩阵当成一个“打包工具”，事情就简单多了。

3.1 为什么需要多维？

你想想看，现实中的系统哪有那么单纯？一个无人机，既要估计位置(x,y,z)，又要估计速度(vx,vy,vz)，还得考虑姿态角。如果每个变量单独用一个一维滤波器，那变量之间的耦合关系就全丢了。比如位置和速度本来就是导数关系，分开滤波反而会引入误差。

所以，多维卡尔曼滤波的核心思想就是：把所有待估计的状态量打包成一个向量，用一个统一的框架去处理它们之间的相关性。

3.2 状态向量与协方差矩阵

咱们先看状态向量。假设我们要估计一个物体的二维位置和速度，那状态向量可以写成：

x = [px, py, vx, vy]^T

嗯，这里用上标T表示转置，实际上是个列向量。4个状态量，这就是一个4维的状态空间。

那协方差矩阵呢？它长这样：

P = | σ²_px   σ_px_py  σ_px_vx  σ_px_vy |
    | σ_py_px  σ²_py   σ_py_vx  σ_py_vy |
    | σ_vx_px  σ_vx_py  σ²_vx   σ_vx_vy |
    | σ_vy_px  σ_vy_py  σ_vy_vx  σ²_vy  |

对角线是每个状态的方差，非对角线是状态之间的协方差。比如σ_px_vx表示位置px和速度vx的相关程度。如果这个值很大，说明位置和速度强相关——这很合理，因为位置变化快慢本来就由速度决定。

关键点：协方差矩阵必须是对称正定的。我在项目中遇到过有人手写代码时忘了维护对称性，结果滤波器直接发散。所以，每次更新完P矩阵，最好加一句：P = (P + P.T) / 2，强制对称。

3.3 矩阵形式的卡尔曼滤波公式

好了，重头戏来了。多维卡尔曼滤波的五个核心公式，用矩阵写出来其实非常简洁：

预测阶段：

x_pred = F @ x_prev + B @ u
P_pred = F @ P_prev @ F.T + Q

更新阶段：

K = P_pred @ H.T @ inv(H @ P_pred @ H.T + R)
x_updated = x_pred + K @ (z - H @ x_pred)
P_updated = (I - K @ H) @ P_pred

你看，和一维的公式长得一模一样，只是把标量换成了矩阵。F是状态转移矩阵，H是观测矩阵，Q是过程噪声协方差，R是观测噪声协方差。

我个人习惯把F矩阵画成这样的结构：

状态pxpyvxvy
px'10dt0
py'010dt
vx'0010
vy'0001

说白了，这就是一个“恒定速度模型”。位置更新时加上速度×时间，速度保持不变。如果你要加加速度（加速度），那就得扩展状态向量了。

3.4 Python实现多维KF

纸上谈兵没意思，咱们直接上代码。我写了一个通用的多维卡尔曼滤波器类，你可以直接拿去用：

import numpy as np

class MultiDimKalmanFilter:
    def __init__(self, F, H, Q, R, x_init, P_init):
        self.F = F  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 观测噪声协方差
        self.x = x_init  # 初始状态
        self.P = P_init  # 初始协方差
        
    def predict(self, u=None, B=None):
        # 预测步骤
        if u is not None and B is not None:
            self.x = self.F @ self.x + B @ u
        else:
            self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x, self.P
    
    def update(self, z):
        # 更新步骤
        y = z - self.H @ self.x  # 残差
        S = self.H @ self.P @ self.H.T + self.R  # 残差协方差
        K = self.P @ self.H.T @ np.linalg.inv(S)  # 卡尔曼增益
        self.x = self.x + K @ y
        self.P = (np.eye(self.P.shape[0]) - K @ self.H) @ self.P
        return self.x, self.P

避坑指南：我曾经在计算卡尔曼增益时直接用了np.linalg.inv(S)，结果遇到奇异矩阵直接报错。后来我改用np.linalg.solve(S.T, H.T).T，或者用scipy.linalg.inv加rcond参数。更稳妥的做法是：K = np.linalg.solve(S, H @ self.P).T，这样数值稳定性好很多。

3.5 一个完整的例子：二维目标跟踪

咱们来跑一个实际例子。假设用雷达跟踪一个匀速直线运动的物体，雷达只能测距离和角度：

# 参数设置
dt = 0.1  # 时间步长
F = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])

# 观测矩阵：只观测位置
H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])

# 噪声协方差
Q = np.eye(4) * 0.01  # 过程噪声
R = np.eye(2) * 0.1   # 观测噪声

# 初始状态
x_init = np.array([0, 0, 1, 0.5])
P_init = np.eye(4) * 10

# 创建滤波器
kf = MultiDimKalmanFilter(F, H, Q, R, x_init, P_init)

# 模拟数据
true_states = []
measurements = []
for t in range(100):
    true_x = np.array([t*dt*1, t*dt*0.5, 1, 0.5])
    true_states.append(true_x)
    z = true_x[:2] + np.random.normal(0, 0.1, 2)
    measurements.append(z)

# 运行滤波
estimates = []
for z in measurements:
    kf.predict()
    est, _ = kf.update(z)
    estimates.append(est.copy())

print("滤波完成！估计误差:", np.mean(np.abs(np.array(estimates) - np.array(true_states)), axis=0))

跑完你会发现，位置估计误差大概在0.1左右，速度估计误差在0.05左右。嗯，效果还不错。

3.6 调参心得

最后聊点实战经验。Q和R的调参，说白了就是平衡“相信模型”还是“相信测量”。

  • Q太大：滤波器太相信测量，结果噪声大，抖动厉害
  • Q太小：滤波器太相信模型，结果响应慢，跟不上真实变化
  • R太大：不相信测量，滤波结果平滑但滞后
  • R太小：太相信测量，噪声直接传进估计值

我一般先用Q = np.eye(n) * 0.01和R = np.eye(m) * 0.1作为起点，然后看残差序列。如果残差均值不为零，说明模型有偏差；如果残差方差太大，说明R设小了。

注意：千万别把Q设成零矩阵！我曾经犯过这个错，结果滤波器“死”了——因为模型完全不相信任何新信息，估计值再也不更新了。Q再小也得有点值，哪怕1e-6都行。

好了，这一讲就到这里。矩阵形式的卡尔曼滤波，说白了就是把一维的标量运算“打包”成矩阵运算。你只要把F、H、Q、R这四个矩阵搞明白，剩下的就是矩阵乘法的事了。下一讲咱们聊聊非线性情况下的扩展卡尔曼滤波（EKF），那才是真正考验矩阵功底的时候。

← 上一章 📖 返回目录 下一章 →