卡尔曼滤波从原理到C++实现完整指南

卡尔曼滤波（Kalman Filter）从原理到 C++ 实现——完整指南

摘要： 本文从零开始讲解卡尔曼滤波的数学原理、五个核心公式的直觉理解、矩阵维度推导，并给出三种不同依赖级别的 C++ 实现（纯手写、Eigen、OpenCV），最后介绍多目标跟踪系统中的工程应用。
适用读者：有一定编程基础的开发者（了解线性代数基础） | 难度：中级 | 预计阅读时间：30 分钟

---

一、前言

卡尔曼滤波（Kalman Filter）是现代控制与信号处理领域最经典的算法之一。从阿波罗登月到自动驾驶，从无人机导航到机器人定位，它无处不在。很多初学者看到公式就头疼，其实卡尔曼滤波的核心思想非常朴素：先猜一个值，再根据测量结果去修正它。

本文将涵盖以下内容：

• 卡尔曼滤波的核心思想与五个公式
• 所有矩阵的维度推导（不需要死记硬背）
• 1D / 2D / 3D 场景的建模方法
• 三种 C++ 实现：纯手写矩阵、Eigen 库、OpenCV 库
• 工程实践中的调参技巧与多目标跟踪系统设计

📝 说明： 本文基于 RoboMaster 雷达多目标跟踪项目的实际开发经验整理，但内容完全拔高成通用方法论，适用于任何需要状态估计的场景。

---

二、卡尔曼滤波是什么？

2.1 一句话理解

卡尔曼滤波的本质就是一个不断进行 “预测 → 修正” 循环的算法：

1. 预测（Predict）：基于历史数据和系统模型，推算当前时刻的状态。
2. 修正（Update）：用当前时刻的传感器测量值，对预测值进行调整。
3. 循环迭代：每一步都利用新的测量信息，让估计越来越准。

与均值滤波、高斯滤波等传统方法相比，卡尔曼滤波的优势在于：能预测未来趋势、能区分信号与噪声、能处理动态变化的系统。

2.2 贝叶斯视角

从概率论的角度看，卡尔曼滤波是贝叶斯滤波在 线性高斯假设 下的解析解：

先验（预测） → 似然（观测） → 后验（更新）

每一次更新，都是在用贝叶斯公式把"预测"和"观测"做最优融合。如果你对贝叶斯推理还不熟悉，只需记住：卡尔曼增益就是那个"该信谁多一点"的权重。

---

三、五个核心公式——闭环的"预测-修正"系统

3.1 预测阶段（时间更新）

公式	名称	含义
${\hat{x}}_k^{-}=F{\hat{x}}_{k-1}+B{u}_k$	预测状态方程	用上一时刻的最优状态推算当前状态
$P_k^{-}=F{P}_{k-1}{F}^T+Q$	预测误差协方差	推算当前预测的不确定性

3.2 修正阶段（测量更新）

公式	名称	含义
$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$	卡尔曼增益	计算预测与测量之间的信任权重
${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$	状态更新	用测量修正预测，得到最优估计
$P_k=(I-K_{k}H)P_k^{-}$	协方差更新	更新修正后的不确定性

3.3 通俗例子：用汽车运动来理解

假设一辆汽车：

• 上一时刻位置 = 5 米，速度 = 2 米/秒
• 预测：1 秒后的位置 = 5 + 2 = 7 米
• 测量：传感器读数为 6.5 米（有噪声）
• 修正：不完全信预测（7 米），也不全信测量（6.5 米），而是用卡尔曼增益 $K_k$ 做折中

如果测量值很可靠（误差小），取较大的 $K_k$（比如 0.75）：

$$\text{最终估计位置}=7+0.75\times (6.5-7)=6.625\text{ 米}$$

💡 提示： 卡尔曼增益 $K_k$ 的关键直觉——$P_k^{-}$ 大（预测不可信）→ $K_k$ 变大 → 更信测量；$R$ 大（测量噪声大）→ $K_k$ 变小 → 更信预测。它本质上是"预测误差占总误差的比例"。

---

四、关键矩阵详解——理解 A、H、B、Q、R

4.1 状态变量：我们在估计什么？

状态变量 ${\hat{x}}_k$ 描述系统在 $k$ 时刻的关键信息。选择取决于具体需求：

场景	状态向量	说明
仅位置	$[x]$	最简单，1 维
位置+速度	$[x,v_x]$	常速度模型（CV）
位置+速度+加速度	$[x,v_x,a_x]$	常加速度模型（CA）
2D 跟踪	$[x,v_x,y,v_y]$	平面运动

4.2 状态转移矩阵 F

F 把上一时刻的状态映射到当前时刻。以 2D 常速度模型为例：

$$F=\left[\begin{array}{cccc}1& dt& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& dt\\ 0& 0& 0& 1\end{array}\right]$$

直觉：$x_k=x_{k-1}+v_x\cdot dt$，速度不变。

4.3 观测矩阵 H

H 从完整状态中"提取"出传感器能测量的部分。如果传感器只测位置：

$$H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$

💡 提示： H 矩阵的作用就是"把状态变成能跟观测比的东西"。如果传感器还能测速度，H 就会多出行。

4.4 过程噪声协方差 Q

Q 反映运动模型的不确定性。我们通常不直接设置 Q，而是根据离散白噪声加速度模型推导：

$$Q=G\cdot q\cdot G^T$$

其中 $q$ 是加速度噪声功率谱密度，$G$ 是噪声输入矩阵。直觉：Q 越大，滤波器越"灵活"，更信观测。

4.5 测量噪声协方差 R

R 描述传感器测量的精度，通常由传感器 datasheet 直接设定：

$$R=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_r^2\end{array}\right]$$

直觉：R 越大，滤波器越"迟钝"，更信模型。

生活化类比：

• $P_k^{-}$ 就是你出门前猜今天温度是 25°C，感觉自己可能估错了 ±2°C（预测误差）
• $R$ 就是你拿出手机看实时温度，手机自身测量误差 ±1°C（测量误差）
• 你最终心里综合出来的"最可能温度"，就是卡尔曼滤波修正后的结果

---

五、矩阵维度推导——确定 n 和 m 就够了

所有矩阵的维度都源于一个核心逻辑：矩阵乘法的规则——“中间维度必须匹配”。

只要确定两个数字：

• n = 状态向量维度（你要估计几个量）
• m = 观测向量维度（传感器每次测到几个量）

矩阵	作用	输入	输出	维度
F	状态转移	旧状态(n)	新状态(n)	n × n
H	状态→观测	状态(n)	观测(m)	m × n
Q	过程噪声	状态(n)	状态(n)	n × n
R	测量噪声	观测(m)	观测(m)	m × m
P	状态不确定性	状态(n)	状态(n)	n × n
K	卡尔曼增益	新息(m)	状态修正(n)	n × m

5.1 一维跟踪（1D）

状态：$[x,v_x]$，观测：$[x_{meas}]$

• n = 2，m = 1
• F: 2×2，H: 1×2，Q: 2×2，R: 1×1，K: 2×1

$$F=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right],H=\left[\begin{array}{cc}1& 0\end{array}\right]$$

5.2 二维跟踪（2D）

状态：$[x,v_x,y,v_y]$，观测：$[x,y]$

• n = 4，m = 2
• F: 4×4，H: 2×4，Q: 4×4，R: 2×2，K: 4×2

5.3 三维跟踪（3D）

状态：$[x,v_x,y,v_y,z,v_z]$，观测：$[x,y,z]$

• n = 6，m = 3
• F: 6×6，H: 3×6，Q: 6×6，R: 3×3，K: 6×3

💡 提示： 无论多少维，F 总是 n×n，H 总是 m×n，Q 总是 n×n，R 总是 m×m，K 总是 n×m。你只要在心里记住 n 和 m，所有矩阵尺寸自动锁死。

---

六、C++ 实现——三种方案任你选

6.1 方案一：纯 C++ 手写矩阵（零依赖）

适合嵌入式环境或不想引入任何第三方库的场景。核心思路是用结构体封装固定大小的矩阵，手动实现矩阵乘法。

#include <iostream>
#include <cmath>

// ---- 矩阵结构体定义 ----
struct Mat4x4 {
    float m[4][4] = {0};
    Mat4x4 identity() {
        Mat4x4 I;
        for (int i = 0; i < 4; i++) I.m[i][i] = 1.0f;
        return I;
    }
};
struct Mat4x2 { float m[4][2] = {0}; };
struct Mat2x4 { float m[2][4] = {0}; };
struct Mat2x2 { float m[2][2] = {0}; };
struct Vec4 { float v[4] = {0}; };
struct Vec2 { float v[2] = {0}; };

/**
 * @brief 纯 C++ 实现的 2D 卡尔曼滤波器（零依赖）
 * 状态：[x, vx, y, vy]，观测：[x, y]，常速度模型
 */
class SimpleKalmanFilter {
public:
    Vec4 state;       // 状态向量
    Mat4x4 cov;       // 状态协方差 P
    Mat4x4 F;         // 状态转移矩阵
    Mat2x4 H;         // 观测矩阵
    Mat4x4 Q;         // 过程噪声协方差
    Mat2x2 R;         // 测量噪声协方差
    float dt = 0.1f;
    float sigma_q = 50.0f;   // 过程噪声强度
    float sigma_r = 0.1f;    // 测量噪声方差

    SimpleKalmanFilter(float init_x, float init_y) {
        state.v[0] = init_x; state.v[2] = init_y;
        cov = Mat4x4().identity();
        // H: 只提取位置分量
        H.m[0][0] = 1.0f; H.m[1][2] = 1.0f;
        // R: 测量噪声
        R.m[0][0] = sigma_r; R.m[1][1] = sigma_r;
    }

    void setDt(float new_dt) {
        dt = new_dt;
        // 更新 F 矩阵（常速度模型）
        F = Mat4x4();  // 清零
        F.m[0][0] = 1.0f; F.m[0][1] = dt;
        F.m[1][1] = 1.0f;
        F.m[2][2] = 1.0f; F.m[2][3] = dt;
        F.m[3][3] = 1.0f;
        // 更新 Q 矩阵（离散白噪声加速度模型）
        float dt2 = dt * dt, dt3 = dt2 * dt;
        Q = Mat4x4();
        Q.m[0][0] = sigma_q * dt3 / 3.0f;
        Q.m[0][1] = Q.m[1][0] = sigma_q * dt2 / 2.0f;
        Q.m[1][1] = sigma_q * dt;
        Q.m[2][2] = sigma_q * dt3 / 3.0f;
        Q.m[2][3] = Q.m[3][2] = sigma_q * dt2 / 2.0f;
        Q.m[3][3] = sigma_q * dt;
    }

    /** 预测：x⁻ = F*x, P⁻ = F*P*Fᵀ + Q */
    void predict() {
        Vec4 x_pred;
        for (int i = 0; i < 4; i++) {
            x_pred.v[i] = 0;
            for (int j = 0; j < 4; j++)
                x_pred.v[i] += F.m[i][j] * state.v[j];
        }
        state = x_pred;

        Mat4x4 FPFt;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++) {
                float sum = 0;
                for (int k = 0; k < 4; k++)
                    for (int l = 0; l < 4; l++)
                        sum += F.m[i][k] * cov.m[k][l] * F.m[j][l];
                FPFt.m[i][j] = sum;
            }
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                cov.m[i][j] = FPFt.m[i][j] + Q.m[i][j];
    }

    /** 更新：K = P*Hᵀ*S⁻¹, x = x + K*y, P = (I-KH)*P */
    void update(float meas_x, float meas_y) {
        Vec2 z = {{meas_x, meas_y}};
        // 新息 y = z - H*x
        Vec2 y_res;
        y_res.v[0] = z.v[0] - H.m[0][0]*state.v[0] - H.m[0][1]*state.v[1]
                     - H.m[0][2]*state.v[2] - H.m[0][3]*state.v[3];
        y_res.v[1] = z.v[1] - H.m[1][0]*state.v[0] - H.m[1][1]*state.v[1]
                     - H.m[1][2]*state.v[2] - H.m[1][3]*state.v[3];
        // S = H*P*Hᵀ + R
        float HP[2][4];
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 4; j++) {
                HP[i][j] = 0;
                for (int k = 0; k < 4; k++)
                    HP[i][j] += H.m[i][k] * cov.m[k][j];
            }
        Mat2x2 S;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++) {
                S.m[i][j] = R.m[i][j];
                for (int k = 0; k < 4; k++)
                    S.m[i][j] += HP[i][k] * H.m[j][k];
            }
        // S⁻¹（2x2 手动求逆）
        float det = S.m[0][0]*S.m[1][1] - S.m[0][1]*S.m[1][0];
        Mat2x2 S_inv;
        S_inv.m[0][0] =  S.m[1][1]/det; S_inv.m[0][1] = -S.m[0][1]/det;
        S_inv.m[1][0] = -S.m[1][0]/det; S_inv.m[1][1] =  S.m[0][0]/det;
        // K = P*Hᵀ*S⁻¹
        float PHT[4][2];
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 2; j++) {
                PHT[i][j] = 0;
                for (int k = 0; k < 4; k++)
                    PHT[i][j] += cov.m[i][k] * H.m[j][k];
            }
        Mat4x2 K;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 2; j++) {
                K.m[i][j] = 0;
                for (int k = 0; k < 2; k++)
                    K.m[i][j] += PHT[i][k] * S_inv.m[k][j];
            }
        // 状态更新
        for (int i = 0; i < 4; i++)
            state.v[i] += K.m[i][0]*y_res.v[0] + K.m[i][1]*y_res.v[1];
        // 协方差更新
        float KH[4][4] = {0};
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                for (int k = 0; k < 2; k++)
                    KH[i][j] += K.m[i][k] * H.m[k][j];
        Mat4x4 I = Mat4x4().identity(), new_cov;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++) {
                new_cov.m[i][j] = 0;
                for (int k = 0; k < 4; k++)
                    new_cov.m[i][j] += (I.m[i][k] - KH[i][k]) * cov.m[k][j];
            }
        cov = new_cov;
    }

    float getX() { return state.v[0]; }
    float getY() { return state.v[2]; }
};

⚠️ 注意： 手写矩阵乘法容易出错，且四重循环性能不佳。在资源允许的情况下，强烈推荐使用 Eigen 或 OpenCV。

---

6.2 方案二：基于 Eigen 实现（推荐）

Eigen 是纯头文件的线性代数库，代码与数学公式几乎一一对应，可读性极高。

#include <Eigen/Dense>

/**
 * @brief 基于 Eigen 的 2D 卡尔曼滤波器
 * 状态：[x, vx, y, vy]，观测：[x, y]，常速度模型
 */
class KalmanFilter2D {
public:
    Eigen::Vector4f x;                        // 状态向量
    Eigen::Matrix4f P, F, Q;                  // 协方差、转移矩阵、过程噪声
    Eigen::Matrix<float, 2, 4> H;             // 观测矩阵
    Eigen::Matrix2f R;                        // 测量噪声
    float dt = 0.1f, sigma_q = 50.0f, sigma_r = 0.1f;

    KalmanFilter2D(float init_x, float init_y) {
        x << init_x, 0.0f, init_y, 0.0f;
        P = Eigen::Matrix4f::Identity();
        H << 1, 0, 0, 0,
             0, 0, 1, 0;
        R << sigma_r, 0,
             0, sigma_r;
        updateMatrices();
    }

    void setDt(float new_dt) { dt = new_dt; updateMatrices(); }

    void predict() {
        x = F * x;
        P = F * P * F.transpose() + Q;
    }

    void update(float meas_x, float meas_y) {
        Eigen::Vector2f z(meas_x, meas_y);
        Eigen::Vector2f y = z - H * x;
        Eigen::Matrix2f S = H * P * H.transpose() + R;
        Eigen::Matrix<float, 4, 2> K = P * H.transpose() * S.inverse();
        x = x + K * y;
        P = (Eigen::Matrix4f::Identity() - K * H) * P;
    }

    float getX() const { return x(0); }
    float getY() const { return x(2); }

private:
    void updateMatrices() {
        F << 1.0f, dt,   0.0f, 0.0f,
             0.0f, 1.0f, 0.0f, 0.0f,
             0.0f, 0.0f, 1.0f, dt,
             0.0f, 0.0f, 0.0f, 1.0f;
        float dt2 = dt * dt, dt3 = dt2 * dt;
        Q << sigma_q*dt3/3, sigma_q*dt2/2, 0, 0,
             sigma_q*dt2/2, sigma_q*dt,    0, 0,
             0, 0, sigma_q*dt3/3, sigma_q*dt2/2,
             0, 0, sigma_q*dt2/2, sigma_q*dt;
    }
};

使用示例：

KalmanFilter2D kf(5.2f, 3.1f);

while (true) {
    float dt = 计算实际时间差;
    kf.setDt(dt);
    kf.predict();

    if (有新观测) {
        kf.update(obs_x, obs_y);
    }

    float smooth_x = kf.getX();
    float smooth_y = kf.getY();
}

✅ 推荐： Eigen 版本与手写版本相比，预测步骤从四重循环变成一行 F * P * F.transpose() + Q，与数学公式完全对应。以后想改状态维度，修改矩阵维度后所有乘法自动适应。

---

6.3 方案三：基于 OpenCV 实现

如果你的项目已经在用 OpenCV，可以直接用 cv::KalmanFilter，无需额外依赖。

#include <opencv2/opencv.hpp>

/**
 * @brief 基于 OpenCV 的 2D 卡尔曼滤波器
 * 状态：[x, vx, y, vy]，观测：[x, y]，常速度模型
 */
class OpenCVKalmanFilter2D {
private:
    cv::KalmanFilter KF;
    float dt = 0.1f;
    float sigma_q_x = 50.0f, sigma_q_y = 50.0f;
    float sigma_r_x = 0.1f,  sigma_r_y = 0.1f;

public:
    OpenCVKalmanFilter2D(float init_x, float init_y) {
        KF.init(4, 2, 0, CV_32F);

        // 初始状态
        cv::Mat state(4, 1, CV_32F);
        state.at<float>(0) = init_x;
        state.at<float>(2) = init_y;
        KF.statePost = state;

        // 状态转移矩阵 F
        setTransitionMatrix(dt);

        // 观测矩阵 H
        KF.measurementMatrix = (cv::Mat_<float>(2, 4) <<
            1, 0, 0, 0,
            0, 0, 1, 0);

        // 过程噪声 Q 和测量噪声 R
        setProcessNoiseCov(dt);
        KF.measurementNoiseCov = (cv::Mat_<float>(2, 2) <<
            sigma_r_x, 0, 0, sigma_r_y);

        cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1.0f));
    }

    void setDt(float new_dt) {
        if (new_dt <= 0) return;
        dt = new_dt;
        setTransitionMatrix(dt);
        setProcessNoiseCov(dt);
    }

    void predict() { KF.predict(); }

    void update(float meas_x, float meas_y) {
        cv::Mat measurement = (cv::Mat_<float>(2, 1) << meas_x, meas_y);
        KF.correct(measurement);
    }

    float getX() const { return KF.statePost.at<float>(0); }
    float getY() const { return KF.statePost.at<float>(2); }

private:
    void setTransitionMatrix(float d) {
        KF.transitionMatrix = (cv::Mat_<float>(4, 4) <<
            1, d, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, d,
            0, 0, 0, 1);
    }

    void setProcessNoiseCov(float d) {
        float d2 = d * d, d3 = d2 * d;
        cv::Mat Q = cv::Mat::zeros(4, 4, CV_32F);
        Q.at<float>(0,0) = sigma_q_x * d3 / 3;
        Q.at<float>(0,1) = Q.at<float>(1,0) = sigma_q_x * d2 / 2;
        Q.at<float>(1,1) = sigma_q_x * d;
        Q.at<float>(2,2) = sigma_q_y * d3 / 3;
        Q.at<float>(2,3) = Q.at<float>(3,2) = sigma_q_y * d2 / 2;
        Q.at<float>(3,3) = sigma_q_y * d;
        KF.processNoiseCov = Q;
    }
};

---

6.4 三种方案对比

特性	纯 C++ 手写	Eigen	OpenCV
外部依赖	无	仅头文件	OpenCV
代码可读性	⭐⭐	⭐⭐⭐⭐⭐	⭐⭐⭐⭐
与数学公式对应	❌ 需展开循环	✅ 一一对应	⭐⭐⭐ 部分对应
修改维度难度	需重写大量代码	改模板参数即可	调 init 参数即可
性能	一般	优秀（SIMD加速）	良好
适用场景	嵌入式/无依赖	通用推荐	已有OpenCV的项目

✅ 推荐： 大多数情况下选 Eigen。它的表达力与 NumPy 接近，且是纯头文件，#include <Eigen/Dense> 即可使用。

---

七、工程应用——多目标跟踪系统设计

掌握了单目标卡尔曼滤波后，如何将其扩展到多目标跟踪？这里介绍核心的系统架构设计思路。

7.1 系统架构概览

一个完整的多目标跟踪系统通常包含以下模块：

7.2 多目标管理器设计

核心思路：维护一个滤波器列表，每个目标对应一个独立的 KF 实例。

#include <vector>
#include <Eigen/Dense>

class TrackerManager {
public:
    struct Track {
        KalmanFilter2D kf;
        int lost_count = 0;
        Track(float x, float y) : kf(x, y) {}
    };
    std::vector<Track> tracks;
    const int MAX_LOST = 15;        // 最大允许丢失帧数
    const float GATE_THRESHOLD = 5.991;  // 马氏距离门控阈值（卡方95%，2自由度）

    void predict_all() {
        for (auto& t : tracks) t.kf.predict();
    }

    void update_with_observations(const std::vector<Eigen::Vector2f>& obs_list) {
        // 1. 计算马氏距离矩阵
        Eigen::MatrixXd cost(tracks.size(), obs_list.size());
        for (int i = 0; i < (int)tracks.size(); i++)
            for (int j = 0; j < (int)obs_list.size(); j++)
                cost(i, j) = mahalanobis_distance(tracks[i].kf, obs_list[j]);

        // 2. 匈牙利匹配 + 门控 + 更新（伪代码）
        // auto assignment = hungarian_algorithm(cost);
        // 对匹配成功的滤波器调用 update，未匹配的 lost_count++

        // 3. 未关联的观测 → 初始化新滤波器
        // 4. 删除丢失过久的滤波器
        for (int i = tracks.size() - 1; i >= 0; i--) {
            if (tracks[i].lost_count > MAX_LOST)
                tracks.erase(tracks.begin() + i);
        }
    }

private:
    /** 计算马氏距离：衡量观测与预测的"统计距离" */
    double mahalanobis_distance(KalmanFilter2D& kf, const Eigen::Vector2f& z) {
        Eigen::Vector2f y = z - kf.H * kf.x;
        Eigen::Matrix2f S = kf.H * kf.P * kf.H.transpose() + kf.R;
        return (y.transpose() * S.inverse() * y).value();
    }
};

7.3 数据关联策略

策略	原理	适用场景
最近邻（NN）	选距离最近的	目标少、稀疏场景
匈牙利算法	全局最优匹配	多目标密集场景
概率数据关联（PDA）	加权所有可能关联	杂波较多的场景

💡 提示： 马氏距离比欧氏距离更适合做门控，因为它考虑了预测的不确定性（协方差矩阵 P），在不确定度大的方向上自动放宽门限。

---

八、调参实战——从"能跑"到"好用"

8.1 关键参数速查表

参数	物理含义	调参方向
Q（过程噪声）	模型信任度	Q↑ → 滤波器更灵活，更信观测；Q↓ → 预测更平滑
R（测量噪声）	传感器精度	R↑ → 滤波器更迟钝，更信模型；R↓ → 更信观测
初始 P	初始不确定性	P↑ → 收敛更快但初期不稳定
门控阈值	匹配容忍度	通常取卡方分布 95% 分位数

8.2 调参流程

1. 先设 R：根据传感器 datasheet 或统计静止目标的方差设定。
2. 再调 Q：从较小值开始（如加速度噪声 0.1），观察轨迹。如果目标机动时误差大 → 增大 Q；如果估计抖动 → 减小 Q。
3. 最后调门控：太小会漏匹配，太大会误匹配。

⚠️ 注意： 协方差计算建议使用 Joseph 形式 P = (I-KH)P(I-KH)ᵀ + KRKᵀ 代替标准形式，保证协方差矩阵的对称正定性，避免数值发散。

8.3 数值稳定性技巧

• 协方差限幅：长时间运行时，P 矩阵的对角线元素可能过大或过小。定期检查并限制在合理范围。
• Joseph 形式：标准形式 P = (I - KH)P 在浮点运算中可能丢失对称性，Joseph 形式更稳定。
• 动态时间步长：每次预测前根据实际经过的时间更新 dt，而非使用固定值。

---

九、进阶：处理非线性系统

当系统的状态转移或观测模型是非线性时，标准卡尔曼滤波不再适用，需要使用其扩展版本：

方法	原理	优点	缺点
EKF（扩展卡尔曼）	对非线性函数求雅可比矩阵做线性化	计算量小，工程常用	需要手动推导雅可比，强非线性不稳定
UKF（无迹卡尔曼）	用确定的 Sigma 点采样传递均值和方差	无需求导，强非线性更稳定	计算量略大于 EKF
粒子滤波	蒙特卡洛采样	任意非线性/非高斯	计算量大，维度诅咒

📝 说明： 对于大多数目标跟踪场景（包括雷达、视觉），如果运动模型不涉及剧烈机动，线性 KF 配合合理的 Q 矩阵就能取得很好的效果。只有在观测模型严重非线性时（如雷达的距离-角度观测），才需要考虑 EKF/UKF。

---

十、总结

本文要点回顾：

1. 核心思想：卡尔曼滤波 = "预测 + 修正"的贝叶斯最优递推，五个公式形成闭环。
2. 矩阵维度：只需记住 n（状态维度）和 m（观测维度），所有矩阵尺寸自动确定。
3. 三种实现：纯 C++ 适合嵌入式，Eigen 是通用首选，OpenCV 适合已有项目的快速集成。
4. 工程应用：多目标跟踪 = KF 实例管理 + 数据关联（最近邻/匈牙利） + 轨迹生命周期管理。
5. 调参核心：Q 决定"信模型还是信观测"，R 由传感器精度决定，门控用卡方分位数。

实际应用场景：

• RoboMaster 赛事中的雷达多目标跟踪（激光雷达聚类 + 视觉识别 + 卡尔曼滤波融合）
• 自动驾驶中的障碍物跟踪与轨迹预测
• 无人机/飞行器的惯性导航与 GPS 融合
• 工业机器人末端执行器的运动平滑
• 金融时间序列的趋势估计

📚 延伸阅读：

• Kalman and Bayesian Filters in Python (Roger Labbe) — 最好的卡尔曼滤波交互式教程
• Wikipedia: Kalman Filter — 经典参考
• Eigen 官方文档 — 线性代数库文档
• OpenCV cv::KalmanFilter 文档 — OpenCV 内置 KF

---

参考资料

• 卡尔曼滤波五个公式通俗解释 - CSDN
• 卡尔曼滤波深入解析：从零开始彻底搞懂 - CSDN
• Understanding the Basis of the Kalman Filter (MATLAB Tech Talk)
• Roger Labbe, Kalman and Bayesian Filters in Python, 2024
• P.D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems