简介：扩展卡尔曼滤波项目C++代码是一份面向自动驾驶传感器融合场景的完整工程实现，主要解决雷达、激光雷达与GPS等异构数据在非线性系统中的状态估计问题，适合具备一定C++基础并希望深入EKF原理的开发者学习。压缩包共360个文件，以头文件和源码为核心，包含h、cpp、txt、sh、md及图片等类型，覆盖滤波器初始化、雅可比矩阵计算、状态预测与更新、协方差调整等关键模块，整体仅2.54MB，轻量易读。代码清晰划分了预测与更新两个阶段，通过泰勒展开将非线性函数局部线性化，并结合协方差矩阵量化不确定性，从而完成多传感器数据的有效融合。项目源自Udacity的CarND-Extended-Kalman-Filter任务，内含基于模拟场景的测试环境，可帮助读者快速验证算法在车辆定位中的鲁棒性。已有1670人学习浏览，通过阅读和运行这套代码，能够完整掌握EKF的推导细节与工程落地方式，并迁移到自己的自动驾驶或机器人项目中。 很多做定位、跟踪、状态估计的C++开发者，迟早都会撞上扩展卡尔曼滤波（EKF）这座山。我第一次把EKF从MATLAB原型改写成C++工程时，最大的感受是：公式推得再顺，落到代码里照样能栽跟头。网上讲原理的文章很多，但能照着写、写完能跑、跑了结果还对的例子反而不多。这篇文章，我把自己在C++里落地EKF的完整过程整理出来——从一个带噪声的雷达测距测角场景出发，拆解状态方程、观测方程和雅可比矩阵怎么算，再给出一份基于Eigen的完整可运行代码，最后聊聊Q矩阵、R矩阵的调参经验，以及三个特别容易翻车的工程细节。适合已经理解卡尔曼滤波基本概念、正准备把EKF写进真实项目里的开发者。
1. 为什么工程里真正常用的是EKF，而不是标准卡尔曼
1.1 现实世界里的观测方程，几乎没有纯线性的
标准卡尔曼滤波（KF）有三个硬前提：状态方程线性、观测方程线性、噪声高斯。第一条和第三条在工程里大多能凑合，第二条几乎做不到。以雷达或激光雷达为例，传感器返回的是距离和角度，本质是极坐标系里的两个数；而定位算法要维护的往往是直角坐标系下的位置和速度。从极坐标换算到直角坐标，要经过sqrt、atan2、sin、cos，这些运算没有一个是矩阵乘法能直接表达的。如果硬把距离和角度当成直角坐标分量塞进KF的观测矩阵H，等于拿直线去拟合一条弯曲的曲线，偏差大得离谱。
更麻烦的是，状态转移方程也可能非线性。最典型的是带航向角的运动模型，车辆的下一帧位置和当前航向角之间夹杂着cos和sin；机械臂的关节角到末端位置的映射更是充满了三角函数。所以工程里做多传感器融合、目标跟踪、组合导航，直接套用标准KF的场景反而少见，大家的默认选择都是EKF，或者它后面进化出来的UKF、粒子滤波。
1.2 泰勒展开背后的代价
EKF的思路非常朴素：用“切线代替曲线”，在当前估计点附近把非线性函数做一阶泰勒展开，求一个雅可比矩阵，然后就能继续沿用标准KF那套线性框架。这和中学数学里用切线近似函数曲线的道理一模一样——在展开点附近，切线和原函数非常接近；离展开点越远，近似误差越大。
所以EKF的性能好坏，很大程度上取决于“当前估计点离真实值到底有多近”。如果初始偏差给得太大，或者系统本身的非线性极强，一阶近似的偏差就会显现。工程上有个经验判断：如果观测函数在估计点附近曲率很小，比如雷达距离比较远、角度变化平缓，EKF完全够用；如果目标贴着雷达飞过、方位角在短时间内剧烈翻转，就得考虑UKF这类对非线性更友好的算法。写代码之前花十分钟看看状态方程和观测方程长什么样，判断一下非线性强度，能帮你避开很多后期调参的坑。
2. EKF五步迭代的工程化拆解：从状态方程到雅可比矩阵
2.1 状态模型与观测模型怎么定，才不会给自己挖坑
设计EKF的第一步不是写代码，而是把数学模型定死。状态量怎么选，决定了整个滤波器的表达能力和计算负担。对二维匀速运动目标，一个最常用的状态向量是：
x = [p_x, v_x, p_y, v_y]^Tp_x、p_y是位置，v_x、v_y是速度，这四个量足够描述平面内匀速运动的对象。如果目标在做匀加速运动，你就得把加速度加进状态向量；如果要做三维姿态估计，状态维度可能直接上到十几维。设计原则是：模型里只放你真正关心、且能从观测中激励出来的量。塞太多状态量进去，不仅计算量大，还可能出现不可观测的问题——某个状态在观测方程里根本不出现，滤波器只能靠过程噪声瞎猜，结果自然好不了。
匀速模型下，状态转移方程是线性的。预测后的位置等于当前位置加上速度乘以时间间隔，转成矩阵就是F矩阵。这里有个工程细节：dt不一定是固定值，所以F矩阵要在每次predict调用时根据实际dt重新构造，不能只在初始化时算一次。
观测模型是非线性的。雷达测距测角场景下：
r = sqrt(p_x^2 + p_y^2) θ = atan2(p_y, p_x)观测向量z_k = [r, θ]^T，噪声近似为零均值高斯，协方差为R。这里雷达距离和方位角的噪声互相独立，所以R矩阵是对角阵。整套模型的符号和维度整理如下。
| 符号 | 含义 | 维度 |
|---|---|---|
| x | 状态：位置+速度 | 4×1 |
| F | 状态转移矩阵（匀速模型） | 4×4 |
| Q | 过程噪声协方差 | 4×4 |
| z | 观测：距离+方位角 | 2×1 |
| H | 观测雅可比矩阵 | 2×4 |
| R | 观测噪声协方差 | 2×2 |
2.2 五个核心公式，以及雅可比矩阵的求法
EKF的五个核心公式和标准KF长得几乎一模一样，唯一的关键区别是把观测矩阵H换成了“当前预测状态处求值的雅可比矩阵”。预测阶段两步：
x_pred = F * x P_pred = F * P * F^T + Q更新阶段三步：
y = z - h(x_pred) // 新息 S = H * P_pred * H^T + R K = P_pred * H^T * S^{-1} x = x_pred + K * y P = (I - K * H) * P_pred这里最容易搞错的就是H。H不是常量，它是观测函数h对状态x求偏导得到的雅可比矩阵，而且必须在每一次update时根据当前的x_pred重新计算。很多人把H固定成初始值，结果滤波一会儿收敛一会儿发散，定位精度飘忽不定，排查半天才发现是这里写错了。
具体到这个雷达测距测角场景，h对状态求偏导。因为距离和方位角只跟p_x、p_y有关，速度分量不直接出现在观测中，雅可比矩阵的后两列全是0。逐项求偏导：
| 观测分量 | ∂/∂p_x | ∂/∂p_y |
|---|---|---|
| r | p_x / r | p_y / r |
| θ | -p_y / r² | p_x / r² |
其中r = sqrt(p_x² + p_y²)，最终得到：
H = [ p_x/r, 0, p_y/r, 0 ; -p_y/r², 0, p_x/r², 0 ]这组式子强烈建议在纸上手推一遍。推过一次你就会发现，雅可比其实没那么神秘，它只是“当前观测值对各个状态变量的敏感程度”而已。
3. 基于Eigen的EKF类实现：接口设计与完整代码
3.1 为什么选Eigen，接口怎么设计
C++里做矩阵运算，Eigen基本是默认选择。它是纯头文件库，下载后把include路径指过去就行，不需要单独编译链接，对嵌入式交叉编译也很友好。相比手写数组运算，Eigen的表达式模板既直观又高效，矩阵乘法、转置、求逆都是一行代码的事。
类接口设计尽量贴近使用逻辑，把矩阵运算的细节封装在内部。调用方只需要关心三件事：配置参数、按时间推进、喂观测数据。所以我设计了这几个接口：
- init()：初始化状态和协方差；
- setQ() / setR() / setP0()：配置过程噪声、测量噪声、初始协方差；
- predict(dt)：按时间间隔前向传播一步；
- update(range, bearing)：输入一帧观测，完成状态更新；
- state() / covariance()：对外读取滤波结果。
这种设计的好处是，上层调用完全不用关心状态维度、矩阵尺寸这些细节，只要蒙头调参就行。而且以后想从4维状态扩到6维、9维，只需要改模板参数和模型相关的代码，接口层面完全不动。
3.2 核心代码和模拟主程序
头文件部分，把类声明写清楚，状态维度和观测维度用模板常量定死：
// ekf.h #pragma once #include <Eigen/Dense> class EKF { public: static const int N = 4; // 状态维度：p_x, v_x, p_y, v_y static const int M = 2; // 观测维度：range, bearing using VectorN = Eigen::Matrix<double, N, 1>; using MatrixNN = Eigen::Matrix<double, N, N>; using VectorM = Eigen::Matrix<double, M, 1>; using MatrixMN = Eigen::Matrix<double, M, N>; EKF() { init(); } void init(const VectorN& x0 = VectorN::Zero()); void setQ(const MatrixNN& Q) { Q_ = Q; } void setR(const Eigen::Matrix<double, M, M>& R) { R_ = R; } void setP0(const MatrixNN& P0) { P_ = P0; } void predict(double dt); void update(double range, double bearing); VectorN state() const { return x_; } MatrixNN covariance() const { return P_; } private: VectorN x_; MatrixNN P_; MatrixNN Q_; Eigen::Matrix<double, M, M> R_; };实现文件里，predict函数根据传入的dt实时重建F矩阵，update函数完成非线性观测的雅可比计算和状态修正：
// ekf.cpp #include "ekf.h" #include <cmath> void EKF::init(const VectorN& x0) { x_ = x0; P_ = MatrixNN::Identity() * 100.0; } void EKF::predict(double dt) { MatrixNN F = MatrixNN::Identity(); F(0, 1) = dt; F(2, 3) = dt; x_ = F * x_; P_ = F * P_ * F.transpose() + Q_; } void EKF::update(double range, double bearing) { double px = x_(0); double py = x_(2); double r = std::sqrt(px * px + py * py); if (r < 1e-6) { r = 1e-6; } double theta = std::atan2(py, px); VectorM z_hat; z_hat << r, theta; MatrixMN H; H << px / r, 0.0, py / r, 0.0, -py / (r * r), 0.0, px / (r * r), 0.0; VectorM y; y << range, bearing; y -= z_hat; // 角度残差归一化到 [-pi, pi] y(1) = std::atan2(std::sin(y(1)), std::cos(y(1))); Eigen::Matrix<double, M, M> S = H * P_ * H.transpose() + R_; Eigen::Matrix<double, N, M> K = P_ * H.transpose() * S.inverse(); x_ = x_ + K * y; P_ = (MatrixNN::Identity() - K * H) * P_; }主程序里，我模拟了一个“真实目标匀速运动 + 雷达测距测角 + 高斯噪声”的闭环数据流。代码输出的CSV可以直接丢进Excel或者Python脚本里画图，和真值对比看滤波效果：
// main.cpp #include "ekf.h" #include <iostream> #include <random> int main() { EKF ekf; Eigen::Matrix<double, 4, 1> x0; x0 << 2.0, 0.0, 2.0, 0.0; ekf.init(x0); double dt = 0.1; double sigma_a = 0.3; // 目标随机加速度标准差，单位 m/s^2 Eigen::Matrix<double, 4, 4> Q<...>; // 离散白噪声加速度模型，构造省略 ekf.setQ(Q); double sigma_r = 0.1; // 距离噪声标准差，单位 m double sigma_theta = 0.02; // 角度噪声标准差，约1.15° Eigen::Matrix2d R; R << sigma_r * sigma_r, 0.0, 0.0, sigma_theta * sigma_theta; ekf.setR(R); double px = 5.0, py = 1.0; double vx = 0.5, vy = 0.2; std::default_random_engine gen(42); std::normal_distribution<double> noise_r(0.0, sigma_r); std::normal_distribution<double> noise_theta(0.0, sigma_theta); std::cout << "t,true_x,true_y,meas_r,meas_theta,est_x,est_y" << std::endl; for (int i = 0; i < 200; ++i) { px += vx * dt; py += vy * dt; double true_r = std::sqrt(px * px + py * py); double true_theta = std::atan2(py, px); double z_r = true_r + noise_r(gen); double z_theta = true_theta + noise_theta(gen); ekf.predict(dt); ekf.update(z_r, z_theta); auto x = ekf.state(); std::cout << i * dt << "," << px << "," << py << "," << z_r << "," << z_theta << "," << x(0) << "," << x(2) << std::endl; } return 0; }代码里Q的构造我故意省略了展开，而是建议直接采用“离散白噪声加速度模型”，这是工程里最常用的过程噪声建模方式，后面马上详细讲。
4. 调参数、防发散：Q矩阵、R矩阵与三个高危细节
4.1 Q和R的物理意义与经验整定方法
很多新手拿到EKF代码后最迷茫的就是：Q和R到底填多少？答案取决于你的物理系统，没有任何一组参数能通吃所有场景。
Q描述的是“过程噪声”，本质是对运动模型不完美程度的补偿。匀速模型假设目标速度恒定，但现实中的目标总有随机加减速，这部分不确定性就要靠Q来吸收。离散白噪声加速度模型是这样构造的：假设每个时间步内目标加速度是一个零均值高斯白噪声，标准差为σ_a。经过一次积分后，位置和速度的不确定性会相互耦合，得到的Q矩阵在二维场景下就是两个维度独立的分块：
Q = [ q11*σ_a, q12*σ_a, 0.0, 0.0 ; q12*σ_a, q22*σ_a, 0.0, 0.0 ; 0.0, 0.0, q11*σ_a, q12*σ_a ; 0.0, 0.0, q12*σ_a, q22*σ_a ] 其中： q11 = dt^4 / 4 q12 = dt^3 / 2 q22 = dt^2σ_a的取值很讲究。如果是行人，随机加减速更频繁，可以取0.5~1.0；如果是匀速行驶的车辆，0.1~0.3就够；如果是高机动飞行器，可能要到5.0以上。这个值直接决定滤波器的“反应速度”：Q偏小，估计曲线平滑但滞后严重；Q偏大，跟踪敏捷但抖动明显。
R描述的是“测量噪声”，最好的来源是实测标定。把传感器固定住，对准一个静止目标采集几百个点，然后算标准差。距离噪声和角度噪声通常互相独立，所以R是对角阵。这里要注意一个单位陷阱：角度噪声必须用弧度，不能用度。很多人直接把传感器手册上的±1°抄进来，结果R大了将近三百倍，滤波器对测量完全不信任，估计值几乎靠模型瞎猜。
4.2 角度环绕、除零和协方差失真：三个我踩过的坑
第一个坑是角度环绕。当目标真实方位角从179°变成-179°时，传感器直接输出-179°，而滤波器预测还在179°附近，两者相减得到-358°。滤波器一看这个新息，认为预测和测量差得离谱，于是疯狂拉偏，甚至直接发散。解法非常简单——算完残差后把角度归一化到[-π, π]：
y(1) = std::atan2(std::sin(y(1)), std::cos(y(1)));这个操作放在update里、状态修正之前，一行代码能救回整个滤波器。
第二个坑是除零。目标靠近雷达正下方时，p_x和p_y都很小，r趋于0，雅可比矩阵里-p_y/r²会直接爆炸成无穷大。真实系统中目标就算真的经过原点，也不可能精确到1e-6以内，所以一种稳妥的做法是在update开头给r一个下界保护：
if (r < 1e-6) r = 1e-6;第三个坑是协方差矩阵逐渐失去对称性。浮点运算的误差会不断累积，导致(I - KH)P_pred不再对称，最终变成非半正定矩阵，滤波器随之发散。缓解办法有两个：每次更新后强制对称化P_ = (P_ + P_.transpose()) / 2，或者直接改用Joseph形式更新协方差：
MatrixNN I_KH = MatrixNN::Identity() - K * H; P_ = I_KH * P_ * I_KH.transpose() + K * R_ * K.transpose();Joseph形式多算一次矩阵乘法，换来的是长期数值稳定，工程上我更推荐这种写法。
4.3 一张问题排查速查表
| 现象 | 可能原因 | 处理方式 |
|---|---|---|
| 估计值剧烈跳变甚至发散 | 初值P0太小、Q太小、R太大 | 增大P0，重新检查Q和R的量级 |
| 估计轨迹明显滞后真值 | Q太小，模型跟不上目标机动 | 增大σ_a |
| 估计结果抖动过大 | R太小或Q太大 | 增大R或减小Q |
| 方位角在±π附近来回跳 | 角度残差没有归一化 | 用atan2(sin,cos)归一化 |
| 输出出现NaN | 雅可比除零、S矩阵奇异 | 加r下界，考虑LLT/LDLT分解 |
| 长时间运行后发散 | 协方差失去对称正定性 | 使用Joseph形式或强制对称化 |
5. 先模拟验证，再判断要不要升级到UKF或粒子滤波
5.1 用模拟数据做闭环验证的三个观察点
拿到这套代码，第一件事千万别急着接真实传感器。先用模拟数据做闭环验证，因为模拟数据有真值，你可以直接量化滤波精度，出了问题也知道往哪个方向查。我在每次迭代中习惯盯三件事：
第一，估计轨迹是否平滑，有没有明显跳变。第二，和真值之间的RMSE随迭代是否收敛。第三，新息序列是否接近零均值白噪声。
新息就是y = z - h(x_pred)，理论上它应当是一个零均值白噪声序列，也就是说不同时刻的新息之间不应该有强相关性。如果新息均值明显偏离零，说明模型有系统误差，比如初始对准不对、传感器有固定偏差；如果新息波动异常大，多半是Q和R的比值设置不合理。把CSV跑出来之后，我建议顺手算一下新息的均值和标准差，这两个数比肉眼盯轨迹曲线更能说明问题。
5.2 什么时候该从EKF切换到UKF、PF
EKF的优势是快、简单、好调试，代价则是它用高斯分布去硬套非线性变换后的分布，本质上是有偏的。以下几个信号出现时，我会果断考虑换UKF：非线性很强，比如观测角度在短时间内剧烈变化；雅可比矩阵推导太复杂，容易出错；状态分布经过非线性变换后明显歪斜，高斯假设撑不住。
UKF的核心思路是用一组sigma点直接穿过非线性函数，再还原成高斯分布，全程不需要求雅可比矩阵，对强非线性的适应性好得多，代价是计算量大约是EKF的两到三倍，但现代处理器完全扛得住。如果噪声本身不是高斯的，比如是多峰分布或重尾分布，那就要上粒子滤波了。
我的实际选择习惯是：先把EKF跑通，把数据链路和调参流程理顺，再评估需不需要换。盲目追求高级算法只会让自己排查问题的难度成倍提升。EKF在当前绝大多数工程场景里，依然是性价比最高的起点。
最后聊聊实际操作中的体会。EKF代码本身不难，难的是建模和参数整定。我强烈建议把雅可比矩阵在纸上手推一遍再抄进代码，推的过程中你才会真正理解这个矩阵为什么长这样，后面调参时也能更快定位问题。调参时一次只动一个变量：先固定R，调Q看跟踪滞后和噪声的关系；再固定Q，调R看平滑度。改参数前记一组基线数据，改完对比RMSE，不要凭感觉。另外，真实系统接入EKF之前，一定要保证数据时间戳是准的，dt不固定会导致F矩阵每次都不一样，协方差更新会乱掉。按照这套流程走下来，EKF在大部分工程场景里都能稳定工作。
本文还有配套的精品资源，点击获取
