60、C++项目实战：卡尔曼滤波器实现

卡尔曼滤波器，这个名字听起来挺唬人的。我第一次接触它是在做无人机姿态解算的时候，当时看着一堆矩阵公式，说实话有点懵。但后来我发现，它本质上就是一个“预测+修正”的循环。你想想看，我们生活中做决策不也是这样吗？先根据经验猜一下，再根据实际观测调整一下。

今天我们就用C++把这个经典算法实现出来。我会带着你一步步拆解，保证你学完就能用在自己的项目里。

卡尔曼滤波的核心思想

说白了，卡尔曼滤波就是解决一个问题：如何从有噪声的测量值中，估计出系统的真实状态。

举个例子，你用一个温度传感器测室温。传感器有噪声，测出来可能是25.1°C，下一秒又变成24.8°C。但你知道室温不会跳来跳去。卡尔曼滤波就能帮你“平滑”这些数据，给出一个更靠谱的估计值。

它的工作流程分两步：

  • 预测（Predict）：根据上一时刻的状态，预测当前时刻的状态。
  • 更新（Update）：用当前时刻的测量值，修正预测值。

这两步不断循环，滤波器就能“活”起来。

核心公式（离散线性系统）：

预测阶段：

  • 状态预测：x̂k|k-1 = F · x̂k-1|k-1 + B · uk
  • 协方差预测：Pk|k-1 = F · Pk-1|k-1 · FT + Q

更新阶段：

  • 卡尔曼增益：Kk = Pk|k-1 · HT · (H · Pk|k-1 · HT + R)-1
  • 状态更新：x̂k|k = x̂k|k-1 + Kk · (zk - H · x̂k|k-1)
  • 协方差更新：Pk|k = (I - Kk · H) · Pk|k-1

别被这些符号吓到。我刚开始看的时候也头大。但实际写代码时，你会发现每个变量都有明确的物理意义。

SVG：卡尔曼滤波循环流程图

卡尔曼滤波循环流程 初始状态 x̂₀, P₀ 预测阶段 (Predict) x̂k|k-1 = F·x̂k-1 + B·uk Pk|k-1 = F·Pk-1·FT + Q 更新阶段 (Update) Kk = Pk|k-1·HT·(H·Pk|k-1·HT+R)-1 x̂k = x̂k|k-1 + Kk·(zk - H·x̂k|k-1) 循环迭代

C++实现：一个通用的卡尔曼滤波器类

我习惯把卡尔曼滤波器封装成一个模板类，这样既能处理一维数据，也能处理多维数据。下面这个实现，我用了Eigen库来处理矩阵运算——别自己造轮子，Eigen已经非常成熟了。

#include <Eigen/Dense>
#include <stdexcept>

template <typename T, int StateDim, int MeasDim>
class KalmanFilter {
public:
    // 状态向量 x
    Eigen::Matrix<T, StateDim, 1> x;
    // 协方差矩阵 P
    Eigen::Matrix<T, StateDim, StateDim> P;
    // 状态转移矩阵 F
    Eigen::Matrix<T, StateDim, StateDim> F;
    // 控制输入矩阵 B（可选）
    Eigen::Matrix<T, StateDim, 1> B;
    // 测量矩阵 H
    Eigen::Matrix<T, MeasDim, StateDim> H;
    // 过程噪声协方差 Q
    Eigen::Matrix<T, StateDim, StateDim> Q;
    // 测量噪声协方差 R
    Eigen::Matrix<T, MeasDim, MeasDim> R;

    KalmanFilter() {
        // 初始化状态和协方差
        x.setZero();
        P.setIdentity();
        F.setIdentity();
        B.setZero();
        H.setIdentity();
        Q.setIdentity() * 0.01;
        R.setIdentity() * 0.1;
    }

    // 预测步骤
    void predict(const Eigen::Matrix<T, StateDim, 1>& u = Eigen::Matrix<T, StateDim, 1>::Zero()) {
        x = F * x + B.array() * u.array();  // 注意：这里简化了控制输入的处理
        P = F * P * F.transpose() + Q;
    }

    // 更新步骤
    void update(const Eigen::Matrix<T, MeasDim, 1>& z) {
        Eigen::Matrix<T, MeasDim, 1> y = z - H * x;  // 测量残差
        Eigen::Matrix<T, StateDim, MeasDim> S = H * P * H.transpose() + R;
        Eigen::Matrix<T, StateDim, MeasDim> K = P * H.transpose() * S.inverse();  // 卡尔曼增益
        x = x + K * y;
        P = (Eigen::Matrix<T, StateDim, StateDim>::Identity() - K * H) * P;
    }

    // 获取当前状态估计
    const Eigen::Matrix<T, StateDim, 1>& getState() const {
        return x;
    }
};

我的经验：刚开始调参时，Q和R的取值很关键。Q越大，表示你越相信测量值；R越大，表示你越相信模型预测。我一般先设R比Q大一个数量级，再根据实际效果微调。

实战：一维温度估计

我们用一个最简单的例子来验证这个滤波器。假设我们要估计一个恒温环境的温度，传感器有高斯白噪声。

#include <iostream>
#include <random>

int main() {
    // 一维卡尔曼滤波器：状态维度1，测量维度1
    KalmanFilter<double, 1, 1> kf;

    // 设置参数
    kf.F(0,0) = 1.0;      // 状态转移：温度不变
    kf.H(0,0) = 1.0;      // 测量矩阵：直接测量温度
    kf.Q(0,0) = 0.01;     // 过程噪声：很小，因为温度变化慢
    kf.R(0,0) = 0.25;     // 测量噪声：传感器精度约±0.5°C
    kf.P(0,0) = 1.0;      // 初始协方差

    // 模拟真实温度25°C，带噪声的测量值
    std::default_random_engine gen;
    std::normal_distribution<double> noise(0.0, 0.5);  // 标准差0.5

    double true_temp = 25.0;
    std::cout << "真实温度: " << true_temp << "°C\n\n";

    for (int i = 0; i < 20; ++i) {
        double measurement = true_temp + noise(gen);

        // 预测
        kf.predict();
        // 更新
        kf.update(Eigen::Matrix<double, 1, 1>(measurement));

        double estimated = kf.getState()(0,0);
        std::cout << "步数 " << i+1
                  << " | 测量值: " << measurement
                  << " | 估计值: " << estimated
                  << " | 误差: " << (estimated - true_temp)
                  << std::endl;
    }

    return 0;
}

运行这段代码，你会发现估计值比原始测量值平滑得多。刚开始几步误差大一点，但很快滤波器就“收敛”了。

我曾经踩过的坑：有一次我把F矩阵设错了，导致预测完全偏离。检查了半天才发现是矩阵维度不匹配。所以，一定要确认你的矩阵维度。Eigen在Debug模式下会检查维度，Release模式下不会，出错了很难排查。

参数调优指南

卡尔曼滤波器的效果，很大程度上取决于参数设置。下面是我总结的一些经验：

参数 含义 调大效果 调小效果
Q（过程噪声） 模型的不确定性 更相信测量值，响应快但噪声大 更相信模型，响应慢但平滑
R（测量噪声） 传感器的不确定性 更相信模型，平滑但滞后 更相信测量值，响应快但易受噪声影响
P₀（初始协方差） 初始状态的不确定性 初始收敛快 初始收敛慢

我的调参习惯：先固定R（根据传感器手册），然后调整Q。如果系统响应太慢，就增大Q；如果噪声太大，就减小Q。P₀一般设为单位矩阵乘以一个较大的数，比如100，让滤波器快速收敛。

扩展：非线性系统的EKF

实际项目中，很多系统是非线性的。比如，用角度和角速度估计姿态。这时候就需要扩展卡尔曼滤波（EKF）。

EKF的核心思想是：对非线性函数做一阶泰勒展开，近似成线性系统。说白了，就是用切线代替曲线。

实现上，只需要修改预测和更新步骤中的雅可比矩阵：

// EKF的预测步骤（示意）
void predictEKF(const Eigen::VectorXd& u) {
    // 非线性状态转移函数 f
    x = f(x, u);
    // 计算雅可比矩阵 F_jacobian
    Eigen::MatrixXd F_jacobian = computeJacobianF(x, u);
    P = F_jacobian * P * F_jacobian.transpose() + Q;
}

// EKF的更新步骤（示意）
void updateEKF(const Eigen::VectorXd& z) {
    // 非线性测量函数 h
    Eigen::VectorXd y = z - h(x);
    // 计算雅可比矩阵 H_jacobian
    Eigen::MatrixXd H_jacobian = computeJacobianH(x);
    Eigen::MatrixXd S = H_jacobian * P * H_jacobian.transpose() + R;
    Eigen::MatrixXd K = P * H_jacobian.transpose() * S.inverse();
    x = x + K * y;
    P = (Eigen::MatrixXd::Identity(x.size()) - K * H_jacobian) * P;
}

注意：EKF对初始状态和噪声参数比较敏感。如果初始状态偏差太大，或者非线性程度太高，EKF可能会发散。我遇到过几次这种情况，后来改用无迹卡尔曼滤波（UKF）才解决。

总结

卡尔曼滤波是一个很实用的工具。从温度估计到目标跟踪，从导航定位到金融预测，到处都有它的身影。

今天我们用C++实现了一个通用的卡尔曼滤波器，并且用一维温度估计的例子验证了它的效果。你完全可以把这段代码拿过去，改改参数，用到自己的项目里。

记住几个关键点：矩阵维度要对、Q和R要调好、初始状态别太离谱。做到这三点，你的滤波器基本就能跑起来了。

好了，今天的分享就到这里。如果你在实际使用中遇到什么问题，欢迎随时交流。

公众号：蓝海资料掘金营，微信deep3321

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