4、卡尔曼滤波实战：一维位置滤波、二维位置滤波、速度与加速度建模

卡尔曼滤波，说白了就是一套「预测+修正」的数学框架。很多初学者一上来就被那五个公式吓住了，其实没那么玄乎。我当年第一次在嵌入式板子上跑卡尔曼滤波，也是对着公式发懵——但真正上手调过一两个项目后，你会发现它就是个聪明的加权平均器。

这一章，咱们直接动手。从最简单的一维位置滤波开始，逐步扩展到二维，最后加入速度和加速度模型。嗯，代码我会给，但更重要的是背后的物理直觉。

4.1 一维位置滤波：从单点定位说起

假设你只有一个GPS模块，输出的是经度或纬度的一维数值。信号有噪声，你希望得到一个平滑的位置估计。这就是最经典的「一维位置滤波」场景。

状态向量很简单：x = [位置]。状态转移矩阵 F = [1]，因为位置在短时间内变化不大。观测矩阵 H = [1]，因为我们直接观测位置。

我习惯把卡尔曼滤波拆成两步走：

1. 预测步：根据上一时刻的状态，猜当前时刻的状态。
2. 更新步：用当前观测值，修正这个猜测。

下面是一个C语言风格的一维位置滤波核心代码：

// 一维位置卡尔曼滤波
typedef struct {
    float x;    // 状态：位置
    float P;    // 状态协方差
    float Q;    // 过程噪声
    float R;    // 观测噪声
} Kalman1D;

void Kalman1D_Predict(Kalman1D *kf) {
    // 预测状态：位置不变（匀速假设）
    kf->x = kf->x;
    // 预测协方差：加上过程噪声
    kf->P = kf->P + kf->Q;
}

void Kalman1D_Update(Kalman1D *kf, float z) {
    // 卡尔曼增益
    float K = kf->P / (kf->P + kf->R);
    // 更新状态
    kf->x = kf->x + K * (z - kf->x);
    // 更新协方差
    kf->P = (1 - K) * kf->P;
}

4.2 二维位置滤波：东向+北向联合估计

实际定位中，我们通常需要同时处理东向（E）和北向（N）两个分量。二维滤波和一维的核心思想完全一样，只是状态向量变成了2x1，矩阵变成了2x2。

状态向量：x = [E, N]^T。状态转移矩阵 F = I（单位矩阵）。观测矩阵 H = I。

为什么可以直接用单位矩阵？因为我们在短时间内假设位置不变——说白了就是「静止假设」。如果目标在运动，那就需要引入速度模型了，这个后面会讲。

二维滤波的代码实现，其实就是把一维的标量运算换成矩阵运算。但在嵌入式平台上，我建议你手写2x2矩阵运算，别偷懒用通用矩阵库——省下的那点代码量，换来的可能是几倍的性能提升。

// 二维位置滤波（简化版，仅展示核心）
typedef struct {
    float x[2];      // 状态：[E, N]
    float P[2][2];   // 协方差矩阵
    float Q[2][2];   // 过程噪声矩阵
    float R[2][2];   // 观测噪声矩阵
} Kalman2D;

void Kalman2D_Update(Kalman2D *kf, float z[2]) {
    // 计算卡尔曼增益矩阵 K = P * (P + R)^(-1)
    float S00 = kf->P[0][0] + kf->R[0][0];
    float S01 = kf->P[0][1] + kf->R[0][1];
    float S10 = kf->P[1][0] + kf->R[1][0];
    float S11 = kf->P[1][1] + kf->R[1][1];
    float det = S00 * S11 - S01 * S10;
    float invS00 = S11 / det;
    float invS01 = -S01 / det;
    float invS10 = -S10 / det;
    float invS11 = S00 / det;

    float K00 = kf->P[0][0]*invS00 + kf->P[0][1]*invS10;
    float K01 = kf->P[0][0]*invS01 + kf->P[0][1]*invS11;
    float K10 = kf->P[1][0]*invS00 + kf->P[1][1]*invS10;
    float K11 = kf->P[1][0]*invS01 + kf->P[1][1]*invS11;

    // 更新状态
    float innov0 = z[0] - kf->x[0];
    float innov1 = z[1] - kf->x[1];
    kf->x[0] += K00 * innov0 + K01 * innov1;
    kf->x[1] += K10 * innov0 + K11 * innov1;

    // 更新协方差（简化版）
    // 实际项目中建议用 Joseph 形式保证对称性
    float temp[2][2];
    // ... 矩阵运算略
}

4.3 速度与加速度建模：让滤波器动起来

静止假设的滤波器，在目标移动时会有明显的滞后。怎么办？把速度和加速度也加入状态向量。

状态向量扩展为：x = [位置, 速度, 加速度]^T。状态转移矩阵变成：

F = [[1, dt, 0.5*dt^2],
     [0,  1,    dt    ],
     [0,  0,    1     ]]

这个矩阵的物理含义很直观：位置的变化由速度和加速度共同决定，速度的变化由加速度决定，加速度假设为常数（或者受过程噪声驱动）。

我个人的经验是：加速度模型不要轻易用。为什么？因为加速度的估计非常依赖观测数据的质量。如果GPS更新频率低（比如1Hz），加速度估计基本是噪声。我一般在更新频率≥10Hz时才启用加速度模型。

下面是一个带速度的一维卡尔曼滤波代码片段：

// 一维位置+速度卡尔曼滤波
typedef struct {
    float x[2];    // [位置, 速度]
    float P[2][2];
    float Q[2][2];
    float R;       // 观测噪声（标量，只观测位置）
    float dt;      // 时间步长
} Kalman1D_Vel;

void Kalman1D_Vel_Predict(Kalman1D_Vel *kf) {
    float dt = kf->dt;
    // 预测状态
    float new_x0 = kf->x[0] + dt * kf->x[1];
    float new_x1 = kf->x[1];
    kf->x[0] = new_x0;
    kf->x[1] = new_x1;

    // 预测协方差
    float F[2][2] = {{1, dt}, {0, 1}};
    // P = F * P * F^T + Q
    // ... 矩阵运算略
}

4.4 知识体系总览

下面这张图总结了本章的核心逻辑：从一维到二维，从静止到运动，卡尔曼滤波的复杂度逐步提升，但核心的「预测-更新」框架始终不变。

4.5 实战避坑指南

最后，分享几个我在项目中踩过的坑：

• 协方差发散：我曾经在调试车载定位时，滤波器跑了20分钟突然发散。查了半天，发现是浮点精度导致P矩阵失去正定性。解决方案：每步更新后强制检查P矩阵的对角线元素，如果出现负数，重置为一个小正数。
• 时间步长不一致：卡尔曼滤波假设固定的dt，但实际GPS数据可能不均匀。我建议在预测步中动态计算dt，而不是用固定值。否则，滤波器在数据间隔突然变大时会严重偏离真实值。
• Q和R的物理意义：别把Q和R当成玄学参数。Q反映你对模型的信任程度，R反映你对传感器的信任程度。一个简单的调参方法：让Q和R的比值等于「模型误差方差」与「观测误差方差」的比值。

好了，这一章的内容就到这里。从一维到二维，从静止到运动，卡尔曼滤波的每一步扩展都有明确的物理含义。你想想看，其实它就是在做一件事：用模型预测未来，用观测修正现在。掌握了这个核心思想，剩下的就是矩阵运算和参数调优了。