3. 卡尔曼滤波实现：标准KF的C++/Python实现、矩阵运算库选择、数值稳定性问题

好，咱们进入实操环节。

前面讲了那么多理论，什么状态空间、协方差传递、增益计算……说实话，光看公式容易晕。我当年刚学KF的时候，对着那五个方程看了三天，觉得自己懂了，一写代码就崩。后来发现，真正落地的时候，坑全在实现细节里。

这一节，咱们就手把手把标准卡尔曼滤波写出来。我会分别给出C++和Python的实现，然后聊聊矩阵运算库怎么选，最后重点讲数值稳定性——这玩意儿要是没处理好，滤波器分分钟发散给你看。

3.1 标准KF的Python实现

先上Python版本。为啥先Python？因为调试方便，原型验证快。我在项目初期做算法验证时，从来不用C++，太浪费时间了。

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, P0, x0):
        """
        F: 状态转移矩阵 (n x n)
        H: 观测矩阵 (m x n)
        Q: 过程噪声协方差 (n x n)
        R: 观测噪声协方差 (m x m)
        P0: 初始状态协方差 (n x n)
        x0: 初始状态 (n,)
        """
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.P = P0
        self.x = x0
        self.n = F.shape[0]
        self.m = H.shape[0]
        
    def predict(self):
        """预测步骤"""
        # 状态预测
        self.x = self.F @ self.x
        # 协方差预测
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x
    
    def update(self, z):
        """更新步骤
        z: 观测值 (m,)
        """
        # 计算残差
        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
        # 协方差更新（Joseph形式，数值更稳定）
        I = np.eye(self.n)
        self.P = (I - K @ self.H) @ self.P @ (I - K @ self.H).T + K @ self.R @ K.T
        return self.x, K

嗯，这里有个细节我要强调一下。你看协方差更新那行，我用了Joseph形式，而不是简单的 P = (I - K*H)*P。为什么？

我在一个惯性导航项目里吃过亏。当时用标准形式，跑了两个小时，P矩阵突然变成负定矩阵，滤波器直接炸了。后来查资料才发现，标准形式在数值上不能保证P始终对称正定，而Joseph形式可以。说白了，就是多花点计算量，买个保险。

3.2 标准KF的C++实现

Python版本验证通过后，就要移植到C++了。嵌入式环境里，Python跑不动，必须上C++。

#include <Eigen/Dense>
#include <iostream>

class KalmanFilter {
private:
    Eigen::MatrixXd F, H, Q, R, P;
    Eigen::VectorXd x;
    int n, m;
    
public:
    KalmanFilter(const Eigen::MatrixXd& F, 
                 const Eigen::MatrixXd& H,
                 const Eigen::MatrixXd& Q, 
                 const Eigen::MatrixXd& R,
                 const Eigen::MatrixXd& P0, 
                 const Eigen::VectorXd& x0)
        : F(F), H(H), Q(Q), R(R), P(P0), x(x0) {
        n = F.rows();
        m = H.rows();
    }
    
    Eigen::VectorXd predict() {
        x = F * x;
        P = F * P * F.transpose() + Q;
        return x;
    }
    
    Eigen::VectorXd update(const Eigen::VectorXd& z) {
        Eigen::VectorXd y = z - H * x;
        Eigen::MatrixXd S = H * P * H.transpose() + R;
        Eigen::MatrixXd K = P * H.transpose() * S.inverse();
        
        x = x + K * y;
        
        // Joseph形式协方差更新
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n);
        P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();
        
        return x;
    }
};

你看，逻辑和Python版本一模一样。但C++版本里有个关键选择——我用了Eigen库。为什么不用别的？下面细说。

3.3 矩阵运算库选择

做组合导航，矩阵运算是家常便饭。你不可能手写矩阵乘法求逆，那会累死。所以选一个好用的矩阵库至关重要。

我这些年用过不少，说说我的感受：

库名 适用场景 优点 缺点
Eigen C++项目，嵌入式/桌面 纯头文件，无依赖，速度快，API优雅 编译慢，调试信息不友好
Armadillo 科研原型，MATLAB迁移 语法像MATLAB，易上手 依赖LAPACK，嵌入式不好用
NumPy Python原型验证 生态好，调试方便 不适合实时系统
自家手写 资源极度受限的MCU 可控，体积小 开发量大，容易出bug

我个人习惯：原型用NumPy，产品用Eigen。除非你的MCU只有几十KB的RAM，否则别手写矩阵库。我曾经在一个项目里手写过，结果矩阵求逆的数值精度出了问题，查了三天才发现是算法实现有bug。后来换了Eigen，问题瞬间解决。

我的建议： 如果你用C++做嵌入式组合导航，直接上Eigen。它支持动态矩阵和固定大小矩阵，固定大小矩阵在编译时分配内存，没有动态分配的开销，非常适合实时系统。

3.4 数值稳定性问题

好，终于到了最头疼的部分。数值稳定性，说白了就是你的滤波器能不能长时间稳定运行。

你想想看，卡尔曼滤波的核心是协方差矩阵P。理论上它应该是对称正定的。但在计算机里，浮点数运算有舍入误差，迭代几百步之后，P可能就不再对称了，甚至变成负定。一旦P负定，卡尔曼增益就乱算，状态估计直接飞掉。

我遇到过最惨的一次：无人机组合导航系统，飞行了40分钟，一切正常。第41分钟，滤波器突然发散，姿态角直接跳了90度。幸好飞控有保护逻辑，切回了纯惯导，不然就炸机了。事后分析，就是P矩阵在长时间运行后失去了正定性。

怎么解决？几个实用技巧：

  • 强制对称化： 每次更新后，执行 P = (P + P^T) / 2。这能保证P对称，但不保证正定。
  • Joseph形式： 前面代码里已经用了。它比标准形式更稳定。
  • 平方根滤波： 用P的Cholesky分解因子来传递，而不是直接传递P。这能从根本上保证正定性。代价是实现复杂，计算量大。
  • 限制P的奇异值： 如果P的特征值太小或太大，就截断到合理范围。
注意： 千万不要在嵌入式平台上直接调用 np.linalg.inv() 或 S.inverse() 来求逆。应该用 solve() 或 ldlt() 分解来解线性方程组，而不是显式求逆。显式求逆的数值误差更大，而且慢。

举个例子，在C++里，应该这样写：

// 不好的做法：显式求逆
Eigen::MatrixXd K = P * H.transpose() * S.inverse();

// 好的做法：用求解器
Eigen::MatrixXd K = P * H.transpose() * S.ldlt().solve(Eigen::MatrixXd::Identity(m, m));

嗯，这里用 ldlt() 是因为S是对称正定矩阵，LDLT分解比LU分解更稳定，而且快。

3.5 一个完整的数值稳定性检查清单

我每次写完卡尔曼滤波代码，都会做下面这些检查。你可以直接拿去用：

  1. 检查P是否对称： max(abs(P - P^T)) < 1e-12
  2. 检查P是否正定： 所有特征值 > 0，或者Cholesky分解成功
  3. 检查残差是否合理： 残差的均值应该接近0，方差应该接近S
  4. 检查卡尔曼增益： 增益矩阵的元素不应该出现NaN或Inf
  5. 长时间运行测试： 至少跑10万步，观察P的轨迹是否稳定

说实话，第5条最重要。很多滤波器在仿真里跑1000步没问题，跑10万步就露馅了。我现在的习惯是，任何滤波算法都要做24小时的连续运行测试，通过才能上线。

核心总结： 标准卡尔曼滤波的实现并不难，难的是让它稳定运行。选对矩阵库（Eigen/NumPy），用Joseph形式更新协方差，避免显式求逆，再加上强制对称化和长时间测试，基本就能保证工程可用。
标准卡尔曼滤波实现核心逻辑 系统模型 F, H, Q, R 初始值 x0, P0 预测步骤 x = F·x P = F·P·Fᵀ + Q 更新步骤 y = z - H·x S = H·P·Hᵀ + R K = P·Hᵀ·S⁻¹ x = x + K·y 数值稳定性 ✓ Joseph形式更新P ✓ 避免显式求逆 ✓ 强制对称化 ✓ 长时间测试 状态估计 x

这张图把整个流程串起来了。左边是输入，中间是预测和更新两个核心步骤，右边是数值稳定性的关键措施。你写代码的时候，就照着这个框架来，基本不会漏东西。

好了，标准卡尔曼滤波的实现就讲到这里。代码有了，库选好了，稳定性也注意了，接下来就可以放心地把它用到组合导航里了。

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