拓冰网站定制 
robot_pose_ekf 大概是 ROS 里做里程计与 IMU 融合时最常用的现成节点室内小车、室外巡检车、双轮差速底盘很多人拿到手第一件事就是把它跑起来。我最初也这么干配置一个 launch 文件订阅 odom 和 imu输出一段融合后的位姿看起来一切正常。但用着用着就发现它像个黑盒状态结构写死了、噪声参数全局固定、想加一路视觉里程计或者把 IMU 零偏估进去根本无从下手。这篇文章就是我后续做的一件完整事情的记录——从零写一个自己的 EKF并拿它替换掉 robot_pose_ekf 中的 EKF 滤波逻辑。如果你正在做机器人定位想真正吃透 EKF 的工程实现或者对原版滤波不满意想做二次开发这篇文章应该能帮你省下不少时间。我会从原版的问题开始讲逐步拆解 EKF 的数学模型、完整 C 实现、ROS 节点接入方式最后聊一聊调参和实际调试中我踩过的坑。所有代码都基于 Eigen不需要额外安装第三方滤波库。1. robot_pose_ekf 到底在做什么以及它的问题出在哪1.1 一句话说透它的消息流robot_pose_ekf 做的事情用一个句话概括订阅多个传感器的位姿或速度消息用 EKF 做多传感器融合输出一个带协方差的融合位姿和 TF。默认支持的输入有四类odomnav_msgs/Odometry、imusensor_msgs/Imu、vo视觉里程计nav_msgs/Odometry和 gpssensor_msgs/NavSatFix。消息进入节点后内部的 FilterBase 会根据传感器类型决定这帧数据是作为运动模型的输入还是作为观测模型的输入。大多数情况下odom 的 Twist 当作预测输入IMU 的姿态当作观测输入。每收到一帧数据就按“先做预测、再做更新”的流程处理最终发布odom_combined这个话题和对应的 TF。这个设计本身是合理的用里程计的速度作为运动先验用 IMU 的姿态修正横滚和俯仰漂移室内室外都能跑。真正的问题出现在你想改动它的时候。1.2 原版 EKF 的状态结构原版维护的状态是一个 7 维向量位置 3 维x, y, z加上姿态四元数 4 维qx, qy, qz, qw。注意不是 6 维的 roll/pitch/yaw而是带单位约束的四元数。这个选择的目的是避免在欧拉角奇异点附近线性化出错工程上是正确的。协方差矩阵 P 是 7×7。运动模型是“上一时刻状态 控制量积分”观测模型根据传感器类型有不同的h(x)和H矩阵。源码里这套逻辑封装在filter_base和ekf两个类中FilterBase 定义接口EKF 实现具体滤波过程。如果你只是改某个观测方程还能勉强打补丁但你要增加状态量、增加传感器类型或者把过程噪声改成在线自适应就得动不少底层代码。我最初只是想加一个轮速比例系数估计结果发现原版的设计里根本没有地方放这个系数硬塞进去会破坏四元数的约束关系。1.3 为什么值得写一个自己的 EKF我决定自己实现原因有三个。第一是状态扩展受限。原版状态固定 7 维IMU bias、轮速比例系数、陀螺尺度因子这类扩展状态根本塞不进去。第二是噪声模型太刚性。它使用固定的 Q 和 R实际中机器人打滑、IMU 受震动噪声水平是时变的固定的噪声矩阵要么让滤波过激要么让滤波发懒。第三是调试不透明。原版打印日志很少当估计结果不对的时候很难判断是运动模型有问题还是观测模型有问题还是参数没调好。自己实现后我可以随时打印中间量看新息序列直接定位是预测还是更新出了问题。这个能力在工程调试中价值很大。2. EKF 核心原理与工程取舍2.1 为什么这里必须用 EKF标准卡尔曼滤波 KF 要求系统是线性的状态转移和观测都必须是矩阵乘法。但机器人定位里的运动模型哪怕最简单的“位置加四元数姿态”也不是线性的。四元数更新里有旋转乘积位置更新可能涉及旋转矩阵乘速度这些都是非线性函数。EKF 的思路就是在每个时刻把非线性函数在当前状态附近做一阶泰勒展开用雅可比矩阵代替原函数然后继续沿用 KF 的公式计算预测和更新。数学上不展开核心公式就是下面两套预测x_pred f(x_prev, u) P_pred F · P_prev · F^T Q更新K P_pred · H^T · (H · P_pred · H^T R)^(-1) x_new x_pred K · (z - h(x_pred)) P_new (I - K·H) · P_pred这里的 F 是 f 对 x 的雅可比H 是 h 对 x 的雅可比Q 是过程噪声协方差R 是观测噪声协方差。所有工程细节最后都落在三件事上F 怎么求、H 怎么求、Q 和 R 怎么给。2.2 运动模型的工程版本我使用的运动模型是把里程计的 Twist 作为控制量u [vx, vy, vz, wx, wy, wz]做位置和姿态更新。位置更新写成p_new p R(q) · v_body · dt其中 R(q) 是当前姿态对应的旋转矩阵v_body 是里程计输出的机体坐标系线速度。这里有一个特别容易踩坑的点很多 ROS 底盘返回的nav_msgs/Odometry里的 twist 是世界坐标系下的速度不是机体坐标系下的速度。你需要先确认你到底用的底盘给的是哪种定义。如果是世界系速度位置更新就是p_new p v·dt简单很多如果是机体系速度就必须乘上 R(q)。这个搞错了融合出来的轨迹方向全是乱的而且光看一两秒很难发现问题跑久了才会暴露。姿态更新采用四元数的一阶积分q_new q ⊗ [1, 0.5·wx·dt, 0.5·wy·dt, 0.5·wz·dt]积分完必须做一次 normalize。如果跳过这一步跑几十秒后协方差就会出现异常甚至整个状态的数值越走越偏。这个细节我会在第 3 章的代码里体现。2.3 雅可比矩阵手动推导还是数值求解F 的解析雅可比可以推但对“位置加四元数”这种状态来说四元数部分的链式法则很容易出错。我的建议是第一步先用数值雅可比把整体流程跑通确认滤波不发散之后再决定要不要换解析形式。数值法在 7 维状态下完全够用而且省去大量手推导数的精力和出错概率。数值雅可比的实现很直接对状态向量的每个分量施加一个小扰动计算状态转移函数的差分比。步长一般取 1e-6 到 1e-5。基本代码如下Eigen::MatrixXd computeF(const Eigen::VectorXd x, const Eigen::VectorXd u, double dt) { Eigen::MatrixXd F Eigen::MatrixXd::Zero(x.size(), x.size()); double eps 1e-6; for (int i 0; i x.size(); i) { Eigen::VectorXd xp x; Eigen::VectorXd xm x; xp(i) eps; xm(i) - eps; Eigen::VectorXd fp stateTransition(xp, u, dt); Eigen::VectorXd fm stateTransition(xm, u, dt); F.col(i) (fp - fm) / (2.0 * eps); } return F; }这个写法最大的优点是通用。以后你改运动模型、加状态量F 会自动跟着变不用重新推导一遍。缺点是每步预测要多算 14 次状态转移但对 7 维状态来说开销完全可以忽略一次循环在微秒级不会影响实时性。H 矩阵同理。观测方程如果是四元数分量H 是恒定稀疏矩阵如果是自定义的传感器模型则同样用数值法求。这个我在第 3 章会展开。2.4 观测模型选型欧拉角还是四元数这是原版和许多教材都不太展开讲的问题。IMU 给的是sensor_msgs/Imu其中的orientation是四元数。如果你把四元数转成欧拉角再作为观测工程上会撞上两个问题。第一个问题是欧拉角在 roll、pitch、yaw 之间的非线性耦合。第二个问题是奇异点当 pitch 接近 ±90° 时roll 和 yaw 会缠绕在一起观测方程 h(x) 的雅可比瞬间变得不可靠直接导致滤波跳动。我只推荐两种做法。第一种是直接取四元数的虚部 qx、qy、qz 作为观测向量观测方程h(x) [qx; qy; qz]此时的 H 矩阵就是[0_{3×3} | I_{3×3}]固定不变。第二种是只把 roll 和 pitch 作为观测yaw 不纳入这样 IMU 的 yaw 漂移就不会被引入到融合结果里。这里要展开说说 IMU yaw 的坑。大多数消费级和工业级 IMU 的 yaw 都是从陀螺仪积分出来的或者上电时有一个初始偏置它和 odom 世界坐标系的 yaw 之间没有一个固定的基准对齐。如果你直接把 IMU 的 yaw 当作绝对观测去修正融合结果的 yaw会发现机器人明明直着走估计的航向却跟着 IMU 的 yaw 慢慢漂。所以如果没有磁力计或外部航向修正我强烈建议只使用 IMU 的 roll 和 pitch让 yaw 主要靠里程计维持。如果一定要用 IMU 的 yaw就需要在启动阶段做一次 yaw 偏置标定把 IMU yaw 与 odom yaw 的初始差值记下来在观测前补偿掉。3. 从零写一个可用的 EKF 核心类3.1 类的接口设计我习惯把滤波核心和 ROS 消息处理彻底分开。滤波核心是一个纯 Eigen 的类不依赖任何 ROS 头文件。这样最大的好处是方便单元测试直接在终端里造一组数据就能验证滤波逻辑不用启动完整仿真。类的大致结构如下class SimpleEKF { public: SimpleEKF(int state_dim, int meas_dim); void setInitialState(const Eigen::VectorXd x0, const Eigen::MatrixXd P0); void setNoise(const Eigen::MatrixXd Q, const Eigen::MatrixXd R); void predict(const Eigen::VectorXd u, double dt); void update(const Eigen::VectorXd z, const std::functionEigen::VectorXd(const Eigen::VectorXd) h); Eigen::VectorXd state() const { return x_; } Eigen::MatrixXd covariance() const { return P_; } private: Eigen::VectorXd x_; Eigen::MatrixXd P_; Eigen::MatrixXd Q_; Eigen::MatrixXd R_; };注意 update 接口的第二个参数我传的是一个函数对象 h。也就是说滤波核心只负责“给定一个状态 x计算观测预测值 h(x)”至于这个观测来自 IMU、GPS 还是视觉里程计它一概不关心。这样做的好处是多传感器融合时只需要在上层写不同的 h 函数滤波核心不用改一行代码。3.2 预测步的实现predict 里做三件事状态积分、求 F、协方差传播。代码写出来是这个样子void SimpleEKF::predict(const Eigen::VectorXd u, double dt) { // 1. 用当前状态和控制量积分出新状态 Eigen::VectorXd x_pred stateTransition(x_, u, dt); // 2. 数值雅可比 Eigen::MatrixXd F computeF(x_, u, dt); // 3. 协方差传播 P_ F * P_ * F.transpose() Q_; x_ x_pred; }stateTransition 的实现对应第 2 章的运动模型Eigen::VectorXd stateTransition(const Eigen::VectorXd x, const Eigen::VectorXd u, double dt) { Eigen::VectorXd xn x; // 位置更新此处按 odom 的速度是世界系来写 xn(0) u(0) * dt; xn(1) u(1) * dt; xn(2) u(2) * dt; // 姿态更新四元数一阶积分 double wx u(3), wy u(4), wz u(5); Eigen::Vector4d q x.segment4(3); double half_norm 0.5 * std::sqrt(wx*wx wy*wy wz*wz); if (half_norm 1e-12) { // 四元数增量的一阶近似 Eigen::Vector4d delta_q; delta_q 1.0, 0.5 * wx * dt, 0.5 * wy * dt, 0.5 * wz * dt; // q_new delta_q ⊗ q Eigen::Vector4d q_new; q_new(0) delta_q(0) * q(0) - delta_q(1) * q(1) - delta_q(2) * q(2) - delta_q(3) * q(3); q_new(1) delta_q(0) * q(1) delta_q(1) * q(0) delta_q(2) * q(3) - delta_q(3) * q(2); q_new(2) delta_q(0) * q(2) - delta_q(1) * q(3) delta_q(2) * q(0) delta_q(3) * q(1); q_new(3) delta_q(0) * q(3) delta_q(1) * q(2) - delta_q(2) * q(1) delta_q(3) * q(0); q_new.normalize(); xn.segment4(3) q_new; } return xn; }这里有一个实操细节。四元数更新的严格做法是用角速度构造四元数增量再做一次四元数乘法也就是q_new Δq ⊗ q。我代码里Δq [1, 0.5wxdt, 0.5wydt, 0.5wzdt]这是一阶近似。对于 dt 在 0.01s 量级的普通里程计更新来说精度完全足够。如果你的场景里 dt 更大、角速度更大建议改用指数映射或者二阶近似否则姿态积分误差会积累得比较快。3.3 更新步的实现update 就是标准的卡尔曼更新公式。我统一用数值法求 H这样不需要每个传感器都手动推导雅可比矩阵。代码如下void SimpleEKF::update(const Eigen::VectorXd z, const std::functionEigen::VectorXd(const Eigen::VectorXd) h) { int m z.size(); if (m 0) return; // 1. 计算观测预测值和 H Eigen::VectorXd hx h(x_); Eigen::MatrixXd H(m, x_.size()); double eps 1e-6; for (int i 0; i x_.size(); i) { Eigen::VectorXd xp x_; Eigen::VectorXd xm x_; xp(i) eps; xm(i) - eps; Eigen::VectorXd hp h(xp); Eigen::VectorXd hm h(xm); H.col(i) (hp - hm) / (2.0 * eps); } // 2. 新息 Eigen::VectorXd innovation z - hx; // 3. 卡尔曼增益 Eigen::MatrixXd S H * P_ * H.transpose() R_; Eigen::MatrixXd K P_ * H.transpose() * S.inverse(); // 4. 状态更新 x_ x_ K * innovation; // 5. 协方差更新 int n x_.size(); P_ (Eigen::MatrixXd::Identity(n, n) - K * H) * P_; // 6. 四元数归一化 Eigen::Vector4d q x_.segment4(3); q.normalize(); x_.segment4(3) q; // 7. 协方差对称化 P_ 0.5 * (P_ P_.transpose()); }这里有一个重要细节新息如果是角度量需要做角度归一化到 [-π, π]。但当你的观测是四元数分量而不是欧拉角时这个操作可以省掉。这也是我推荐用四元数分量做观测的原因之一代码简单还省得处理角度缠绕问题。另一个细节是 S 矩阵求逆我直接用了.inverse()。对于 3×3 或 6×6 的小矩阵没问题如果状态维数变大建议改用 QR 分解或者 LLT 分解求解 K避免数值问题。实际中 7 维状态下直接求逆完全够用我就没有上更复杂的分解。3.4 初始化和协方差保护EKF 启动时需要一个初始状态和初始协方差。第一个消息进来时我通常的做法是把第一个 odom 的位置作为初始位置把 IMU 的姿态换算为初始四元数P 设成一个较大的对角阵比如每个对角元取 1.0表示初始不确定度很大。如果 P 设得太小滤波器一开始就会对错误初始状态过度自信可能出现长时间无法收敛。协方差保护机制一定要写。我在实际调试中遇到过 P 矩阵非正定的情况原因通常是 Q 或 R 给得不合理或者 F 算错。一个简单的兜底是每次更新后检查 P 是否包含 NaN再做对称化并把负特征值强制置零。代码可以这样写void SimpleEKF::stabilizeCovariance() { if (!P_.allFinite()) { std::cerr EKF covariance has NaN, resetting to identity std::endl; P_ Eigen::MatrixXd::Identity(x_.size(), x_.size()); return; } P_ 0.5 * (P_ P_.transpose()); Eigen::SelfAdjointEigenSolverEigen::MatrixXd solver(P_); Eigen::VectorXd eig solver.eigenvalues(); if (eig.minCoeff() 0.0) { eig eig.cwiseMax(0.0); P_ solver.eigenvectors() * eig.asDiagonal() * solver.eigenvectors().transpose(); } }这个保护函数在调试阶段特别有用它不会修复根本问题但能防止一次数值异常直接带崩整个节点。等滤波器稳定运行后可以把这个保护函数留着反正开销很小关键时刻能兜底。4. 接进 ROS替换 robot_pose_ekf 的两种方案4.1 方案一改源码在 robot_pose_ekf 包内替换 EKF这个方案最贴合标题字面意思去 robot_pose_ekf 包的源码里把内部的 EKF 实现替换成自己写的类。具体动作是把你的 SimpleEKF 类编译成库再在 robot_pose_ekf 的 filter_base 实现中调用它的接口。但我对这个方案持谨慎态度。robot_pose_ekf 的代码结构比较老接口和现在惯用的 ROS 写法不太一致改动容易引入兼容性问题。比如它内部对消息协方差的转换、拒测逻辑和你的滤波核心耦合得很深替换后还要顺带适配它的 sensor 封装。如果纯粹是为了学习可以做这个替换跑通一次功能演示如果是业务项目我建议走方案二风险小得多。4.2 方案二写一个独立节点直接替代整个节点更干净的做法是新建一个节点订阅与 robot_pose_ekf 相同的话题发布相同的输出话题。这样下游节点完全无感知还可以随时在 launch 文件里切换原版和自己的版本方便做对比实验。我最初搭建的节点框架大致如下class EkfLocalizer { public: EkfLocalizer(ros::NodeHandle nh, ros::NodeHandle pnh) { ekf_ std::make_uniqueSimpleEKF(7, 3); ekf_-setNoise(Q_, R_); sub_odom_ nh.subscribe(odom, 10, EkfLocalizer::odomCallback, this); sub_imu_ nh.subscribe(imu/data, 10, EkfLocalizer::imuCallback, this); pub_pose_ nh.advertisegeometry_msgs::PoseWithCovarianceStamped(odom_combined, 10); pub_path_ nh.advertisenav_msgs::Path(ekf_path, 10); } private: void odomCallback(const nav_msgs::OdometryConstPtr msg) { double t msg-header.stamp.toSec(); double dt t - last_time_; if (dt 1e-4) { // 用最近的速度做一次预测 Eigen::VectorXd u(6); u msg-twist.twist.linear.x, msg-twist.twist.linear.y, msg-twist.twist.linear.z, msg-twist.twist.angular.x, msg-twist.twist.angular.y, msg-twist.twist.angular.z; ekf_-predict(u, dt); last_time_ t; } } void imuCallback(const sensor_msgs::ImuConstPtr msg) { double t msg-header.stamp.toSec(); double dt t - last_time_; if (dt 1e-4) { // 如果一帧 odom 都没来就直接用上一次的运动状态做预测 // 这里省略运动状态缓存细节 } // IMU 四元数虚部作为观测 Eigen::VectorXd z(3); z msg-orientation.x, msg-orientation.y, msg-orientation.z; ekf_-update(z, [](const Eigen::VectorXd x) { return x.segment4(3).head3(); // [qx; qy; qz] }); } std::unique_ptrSimpleEKF ekf_; ros::Subscriber sub_odom_; ros::Subscriber sub_imu_; ros::Publisher pub_pose_; ros::Publisher pub_path_; double last_time_ 0.0; };关键的点在消息回调之间的状态管理。odom 和 imu 消息不会严格对齐我维护一个last_time_变量。每收到任一话题先计算dt如果 dt 大于阈值就执行一次预测再根据当前消息类型执行对应的更新。这样无论是两种传感器哪个先到、频率孰高孰低都能保持时间上的连贯。4.3 时间同步与消息管理时间管理这一步看似简单实际是替换原版后最影响效果的地方。原版对每个传感器都设置了传感器超时剔除机制超过sensor_timeout就把它从融合中剔除。我在自己的实现里也借鉴了这个思想但做了简化每个传感器维护一个最近消息时间戳超过 0.2 秒没有新数据就认为该传感器暂不可用。每次预测时如果 dt 超过 0.2 秒就认为连续消息断裂主动将协方差增大到一个上限值防止长时间无消息时滤波器仍然保持过高的置信度。如果新消息时间戳比 last_time 还小直接丢弃并打印警告。ROS 中多话题发布频率不同乱序很常见这个问题不处理EKF 的时间线会被搅乱。这套逻辑写进节点后即使某个传感器断线再重连滤波器也能自动恢复而不是彻底发散。5. 参数调优与现场调试实录5.1 Q 和 R 怎么给一个能用的初值这是所有滤波器最容易让人崩溃的部分我在这里先给一个可复现的经验思路。过程噪声 Q 可以理解为“对运动模型的不信任程度”。假设机器人最大线速度 0.5 m/s最大角速度 0.5 rad/s那么位置过程噪声的标准差可以取最大值的 1% 到 5%也就是大约 0.005 到 0.025 m/s 量级。Q 的位置对角元就是(σ_v · dt)^2。如果 dt 是 0.01sσ_v 取 0.01那么 Q 的位置对角元是(0.01 * 0.01)^2 1e-8这个数值非常小。这是因为 Q 表示的是每一步预测中噪声的增量它会通过协方差传播不断累积一次性给太大滤波就会变得很“飘”。观测噪声 R 的初值我的土办法是把 IMU 固定静止录制 60 秒数据统计四元数虚部 qx、qy、qz 的标准差。假设标准差是 0.005那么 R 可以设成 2.5e-5 量级。这比直接抄别人的参数可靠得多因为每台 IMU 的噪声底噪都不一样。实际测试时先把 R 设小一点观察姿态是否抖动再把 R 设大一点观察姿态是否跟踪太慢。反复几次找到一个中间值。这个步骤没有捷径但盯着 rqt_plot 里的姿态曲线比看 log 日志高效得多。5.2 常见发散症状与排查顺序我把自己调试中遇到的几类典型问题整理成了一张速查表你可以按表对着排查。现象可能原因处理方式协方差爆炸数值超 1e6Q 过大、F 计算错误、dt 过大减小 Q检查数值雅可比步长检查时间戳是否跳变估计姿态抖动剧烈R 过小观测权重过大IMU 噪声比预期大增大 R重新统计 IMU 静置噪声轨迹往某个方向飞odom twist 坐标系定义与模型不一致确认 twist 是机体系还是世界系调整位置更新公式与真值相比有固定偏置传感器安装外参未标定检查 IMU 与 base_link 的静态外参融合输出滞后明显Q 太小或 R 太大滤波器过于信赖预测调小 R或调大 Q消息中断后位置跳变长时间无预测协方差过小增加协方差重置机制超过阈值自动增大 P这张表解决的是方向问题。每一个现象背后都有不止一种原因所以排查时要先把时间戳和坐标系统一这两件事排除掉再动滤波参数。我自己踩过最深的坑就是坐标系问题一开始把机体系速度当成世界系速度用位置更新公式写错了调了一整天参数都没用最后才发现是基础定义错了。5.3 用 bag 回放和 rqt_plot 做验证我建议的验证流程是先用rosbag record录制一段包含 odom、imu 和参考真值如果有的数据然后离线回放对比原版 robot_pose_ekf 和自己 EKF 的输出。回放时注意关闭 TF 的持久广播避免时间戳混乱。rqt_plot 可以直接绘制话题字段。比如对比融合后位置和原始 odom 位置命令差不多是这样的rqt_plot /odom_combined/pose/pose/position/x \ /odom_combined/pose/pose/position/y \ /odom/pose/pose/position/x \ /odom/pose/pose/position/y如果希望量化对比我推荐用 evo 工具专门做里程计轨迹误差评估支持 ROS bag 格式。输入两个话题输出 APE 和 RPE 的均值以及曲线图替换前后各跑一遍效果一目了然# 回放数据 rosbag play your_bag.bag # 用 evo 量化估计轨迹与参考真值的误差 evo_ape bag your_bag.bag /ground_truth/odom /odom_combined/pose自己写的 EKF 不一定比原版强。对标准传感器组合原版参数调好了完全够用。这个工作的价值在于你可以按需定制把慢漂移的 IMU yaw 排除、把里程计置信度根据打滑程度在线缩放、加入自定义传感器时不需要大改框架。这些都是原版很难做到的事情。6. 这套实现后续还能怎么扩展写完一个能跑通的 EKF 只是一个开始。我实际用下来最大的感受是当你拥有了一个自己能控制的滤波核心改代码的成本会降低一个量级。后面我再往里面加传感器、加状态量都是在这个框架上做增量。一个很自然的扩展是增加状态维度把 IMU 的陀螺零偏 b_g 和加速度计零偏 b_a 也估计进去。状态从 7 维变成 15 维只需要修改 stateTransition 里的姿态更新和位置更新加入 bias 的反馈项然后在预测时给 bias 加一个随机游走模型。因为我的类不绑定固定维度维度一改F 和 P 自动跟着变改动量很小。这正是第 1 章说的“状态扩展受限”问题自研后彻底解决。第二个扩展是把 EKF 换成 UKF。如果系统非线性程度很高尤其观测模型里加入了 GPS 的球面坐标转换、或者加入带大角度旋转的视觉观测时EKF 的一阶线性化可能不够。UKF 不需要求雅可比直接用一组 sigma 点传播分布工程上更省心。我自己现在倾向于在强非线性场景下使用 UKF但 EKF 的计算量小、成熟度高依然是优先选项。第三个扩展是自适应噪声。机器人打滑时odom 的置信度应该自动下降IMU 受振动时姿态观测的置信度也应该自动下降。一个粗粒度的实现是维护一个新息序列的滑动窗口当窗口内新息方差明显大于 R 的预期值时就按一定比例放大 R 或 Q。这个做法不完美但能明显改善长时间运行的效果我试过之后体感很好。最后说一句我自己实际操作中的体会。很多人一开始把 Q 和 R 的数值当成玄学其实这两个东西和你的传感器噪声直接相关。认真做一次 IMU 静止噪声统计认真确认一次 odom twist 的坐标系定义比盲目调参数管用得多。我第一版 EKF 跑起来的时候在车间里开了几十圈最后低头一看轨迹明显比原版平滑长走廊上的航向保持也好了很多那时候才觉得这一步做得值。 从零手写EKF：替换robot_pose_ekf实现里程计与IMU融合
robot_pose_ekf 大概是 ROS 里做里程计与 IMU 融合时最常用的现成节点室内小车、室外巡检车、双轮差速底盘很多人拿到手第一件事就是把它跑起来。我最初也这么干配置一个 launch 文件订阅 odom 和 imu输出一段融合后的位姿看起来一切正常。但用着用着就发现它像个黑盒状态结构写死了、噪声参数全局固定、想加一路视觉里程计或者把 IMU 零偏估进去根本无从下手。这篇文章就是我后续做的一件完整事情的记录——从零写一个自己的 EKF并拿它替换掉 robot_pose_ekf 中的 EKF 滤波逻辑。如果你正在做机器人定位想真正吃透 EKF 的工程实现或者对原版滤波不满意想做二次开发这篇文章应该能帮你省下不少时间。我会从原版的问题开始讲逐步拆解 EKF 的数学模型、完整 C 实现、ROS 节点接入方式最后聊一聊调参和实际调试中我踩过的坑。所有代码都基于 Eigen不需要额外安装第三方滤波库。1. robot_pose_ekf 到底在做什么以及它的问题出在哪1.1 一句话说透它的消息流robot_pose_ekf 做的事情用一个句话概括订阅多个传感器的位姿或速度消息用 EKF 做多传感器融合输出一个带协方差的融合位姿和 TF。默认支持的输入有四类odomnav_msgs/Odometry、imusensor_msgs/Imu、vo视觉里程计nav_msgs/Odometry和 gpssensor_msgs/NavSatFix。消息进入节点后内部的 FilterBase 会根据传感器类型决定这帧数据是作为运动模型的输入还是作为观测模型的输入。大多数情况下odom 的 Twist 当作预测输入IMU 的姿态当作观测输入。每收到一帧数据就按“先做预测、再做更新”的流程处理最终发布odom_combined这个话题和对应的 TF。这个设计本身是合理的用里程计的速度作为运动先验用 IMU 的姿态修正横滚和俯仰漂移室内室外都能跑。真正的问题出现在你想改动它的时候。1.2 原版 EKF 的状态结构原版维护的状态是一个 7 维向量位置 3 维x, y, z加上姿态四元数 4 维qx, qy, qz, qw。注意不是 6 维的 roll/pitch/yaw而是带单位约束的四元数。这个选择的目的是避免在欧拉角奇异点附近线性化出错工程上是正确的。协方差矩阵 P 是 7×7。运动模型是“上一时刻状态 控制量积分”观测模型根据传感器类型有不同的h(x)和H矩阵。源码里这套逻辑封装在filter_base和ekf两个类中FilterBase 定义接口EKF 实现具体滤波过程。如果你只是改某个观测方程还能勉强打补丁但你要增加状态量、增加传感器类型或者把过程噪声改成在线自适应就得动不少底层代码。我最初只是想加一个轮速比例系数估计结果发现原版的设计里根本没有地方放这个系数硬塞进去会破坏四元数的约束关系。1.3 为什么值得写一个自己的 EKF我决定自己实现原因有三个。第一是状态扩展受限。原版状态固定 7 维IMU bias、轮速比例系数、陀螺尺度因子这类扩展状态根本塞不进去。第二是噪声模型太刚性。它使用固定的 Q 和 R实际中机器人打滑、IMU 受震动噪声水平是时变的固定的噪声矩阵要么让滤波过激要么让滤波发懒。第三是调试不透明。原版打印日志很少当估计结果不对的时候很难判断是运动模型有问题还是观测模型有问题还是参数没调好。自己实现后我可以随时打印中间量看新息序列直接定位是预测还是更新出了问题。这个能力在工程调试中价值很大。2. EKF 核心原理与工程取舍2.1 为什么这里必须用 EKF标准卡尔曼滤波 KF 要求系统是线性的状态转移和观测都必须是矩阵乘法。但机器人定位里的运动模型哪怕最简单的“位置加四元数姿态”也不是线性的。四元数更新里有旋转乘积位置更新可能涉及旋转矩阵乘速度这些都是非线性函数。EKF 的思路就是在每个时刻把非线性函数在当前状态附近做一阶泰勒展开用雅可比矩阵代替原函数然后继续沿用 KF 的公式计算预测和更新。数学上不展开核心公式就是下面两套预测x_pred f(x_prev, u) P_pred F · P_prev · F^T Q更新K P_pred · H^T · (H · P_pred · H^T R)^(-1) x_new x_pred K · (z - h(x_pred)) P_new (I - K·H) · P_pred这里的 F 是 f 对 x 的雅可比H 是 h 对 x 的雅可比Q 是过程噪声协方差R 是观测噪声协方差。所有工程细节最后都落在三件事上F 怎么求、H 怎么求、Q 和 R 怎么给。2.2 运动模型的工程版本我使用的运动模型是把里程计的 Twist 作为控制量u [vx, vy, vz, wx, wy, wz]做位置和姿态更新。位置更新写成p_new p R(q) · v_body · dt其中 R(q) 是当前姿态对应的旋转矩阵v_body 是里程计输出的机体坐标系线速度。这里有一个特别容易踩坑的点很多 ROS 底盘返回的nav_msgs/Odometry里的 twist 是世界坐标系下的速度不是机体坐标系下的速度。你需要先确认你到底用的底盘给的是哪种定义。如果是世界系速度位置更新就是p_new p v·dt简单很多如果是机体系速度就必须乘上 R(q)。这个搞错了融合出来的轨迹方向全是乱的而且光看一两秒很难发现问题跑久了才会暴露。姿态更新采用四元数的一阶积分q_new q ⊗ [1, 0.5·wx·dt, 0.5·wy·dt, 0.5·wz·dt]积分完必须做一次 normalize。如果跳过这一步跑几十秒后协方差就会出现异常甚至整个状态的数值越走越偏。这个细节我会在第 3 章的代码里体现。2.3 雅可比矩阵手动推导还是数值求解F 的解析雅可比可以推但对“位置加四元数”这种状态来说四元数部分的链式法则很容易出错。我的建议是第一步先用数值雅可比把整体流程跑通确认滤波不发散之后再决定要不要换解析形式。数值法在 7 维状态下完全够用而且省去大量手推导数的精力和出错概率。数值雅可比的实现很直接对状态向量的每个分量施加一个小扰动计算状态转移函数的差分比。步长一般取 1e-6 到 1e-5。基本代码如下Eigen::MatrixXd computeF(const Eigen::VectorXd x, const Eigen::VectorXd u, double dt) { Eigen::MatrixXd F Eigen::MatrixXd::Zero(x.size(), x.size()); double eps 1e-6; for (int i 0; i x.size(); i) { Eigen::VectorXd xp x; Eigen::VectorXd xm x; xp(i) eps; xm(i) - eps; Eigen::VectorXd fp stateTransition(xp, u, dt); Eigen::VectorXd fm stateTransition(xm, u, dt); F.col(i) (fp - fm) / (2.0 * eps); } return F; }这个写法最大的优点是通用。以后你改运动模型、加状态量F 会自动跟着变不用重新推导一遍。缺点是每步预测要多算 14 次状态转移但对 7 维状态来说开销完全可以忽略一次循环在微秒级不会影响实时性。H 矩阵同理。观测方程如果是四元数分量H 是恒定稀疏矩阵如果是自定义的传感器模型则同样用数值法求。这个我在第 3 章会展开。2.4 观测模型选型欧拉角还是四元数这是原版和许多教材都不太展开讲的问题。IMU 给的是sensor_msgs/Imu其中的orientation是四元数。如果你把四元数转成欧拉角再作为观测工程上会撞上两个问题。第一个问题是欧拉角在 roll、pitch、yaw 之间的非线性耦合。第二个问题是奇异点当 pitch 接近 ±90° 时roll 和 yaw 会缠绕在一起观测方程 h(x) 的雅可比瞬间变得不可靠直接导致滤波跳动。我只推荐两种做法。第一种是直接取四元数的虚部 qx、qy、qz 作为观测向量观测方程h(x) [qx; qy; qz]此时的 H 矩阵就是[0_{3×3} | I_{3×3}]固定不变。第二种是只把 roll 和 pitch 作为观测yaw 不纳入这样 IMU 的 yaw 漂移就不会被引入到融合结果里。这里要展开说说 IMU yaw 的坑。大多数消费级和工业级 IMU 的 yaw 都是从陀螺仪积分出来的或者上电时有一个初始偏置它和 odom 世界坐标系的 yaw 之间没有一个固定的基准对齐。如果你直接把 IMU 的 yaw 当作绝对观测去修正融合结果的 yaw会发现机器人明明直着走估计的航向却跟着 IMU 的 yaw 慢慢漂。所以如果没有磁力计或外部航向修正我强烈建议只使用 IMU 的 roll 和 pitch让 yaw 主要靠里程计维持。如果一定要用 IMU 的 yaw就需要在启动阶段做一次 yaw 偏置标定把 IMU yaw 与 odom yaw 的初始差值记下来在观测前补偿掉。3. 从零写一个可用的 EKF 核心类3.1 类的接口设计我习惯把滤波核心和 ROS 消息处理彻底分开。滤波核心是一个纯 Eigen 的类不依赖任何 ROS 头文件。这样最大的好处是方便单元测试直接在终端里造一组数据就能验证滤波逻辑不用启动完整仿真。类的大致结构如下class SimpleEKF { public: SimpleEKF(int state_dim, int meas_dim); void setInitialState(const Eigen::VectorXd x0, const Eigen::MatrixXd P0); void setNoise(const Eigen::MatrixXd Q, const Eigen::MatrixXd R); void predict(const Eigen::VectorXd u, double dt); void update(const Eigen::VectorXd z, const std::functionEigen::VectorXd(const Eigen::VectorXd) h); Eigen::VectorXd state() const { return x_; } Eigen::MatrixXd covariance() const { return P_; } private: Eigen::VectorXd x_; Eigen::MatrixXd P_; Eigen::MatrixXd Q_; Eigen::MatrixXd R_; };注意 update 接口的第二个参数我传的是一个函数对象 h。也就是说滤波核心只负责“给定一个状态 x计算观测预测值 h(x)”至于这个观测来自 IMU、GPS 还是视觉里程计它一概不关心。这样做的好处是多传感器融合时只需要在上层写不同的 h 函数滤波核心不用改一行代码。3.2 预测步的实现predict 里做三件事状态积分、求 F、协方差传播。代码写出来是这个样子void SimpleEKF::predict(const Eigen::VectorXd u, double dt) { // 1. 用当前状态和控制量积分出新状态 Eigen::VectorXd x_pred stateTransition(x_, u, dt); // 2. 数值雅可比 Eigen::MatrixXd F computeF(x_, u, dt); // 3. 协方差传播 P_ F * P_ * F.transpose() Q_; x_ x_pred; }stateTransition 的实现对应第 2 章的运动模型Eigen::VectorXd stateTransition(const Eigen::VectorXd x, const Eigen::VectorXd u, double dt) { Eigen::VectorXd xn x; // 位置更新此处按 odom 的速度是世界系来写 xn(0) u(0) * dt; xn(1) u(1) * dt; xn(2) u(2) * dt; // 姿态更新四元数一阶积分 double wx u(3), wy u(4), wz u(5); Eigen::Vector4d q x.segment4(3); double half_norm 0.5 * std::sqrt(wx*wx wy*wy wz*wz); if (half_norm 1e-12) { // 四元数增量的一阶近似 Eigen::Vector4d delta_q; delta_q 1.0, 0.5 * wx * dt, 0.5 * wy * dt, 0.5 * wz * dt; // q_new delta_q ⊗ q Eigen::Vector4d q_new; q_new(0) delta_q(0) * q(0) - delta_q(1) * q(1) - delta_q(2) * q(2) - delta_q(3) * q(3); q_new(1) delta_q(0) * q(1) delta_q(1) * q(0) delta_q(2) * q(3) - delta_q(3) * q(2); q_new(2) delta_q(0) * q(2) - delta_q(1) * q(3) delta_q(2) * q(0) delta_q(3) * q(1); q_new(3) delta_q(0) * q(3) delta_q(1) * q(2) - delta_q(2) * q(1) delta_q(3) * q(0); q_new.normalize(); xn.segment4(3) q_new; } return xn; }这里有一个实操细节。四元数更新的严格做法是用角速度构造四元数增量再做一次四元数乘法也就是q_new Δq ⊗ q。我代码里Δq [1, 0.5wxdt, 0.5wydt, 0.5wzdt]这是一阶近似。对于 dt 在 0.01s 量级的普通里程计更新来说精度完全足够。如果你的场景里 dt 更大、角速度更大建议改用指数映射或者二阶近似否则姿态积分误差会积累得比较快。3.3 更新步的实现update 就是标准的卡尔曼更新公式。我统一用数值法求 H这样不需要每个传感器都手动推导雅可比矩阵。代码如下void SimpleEKF::update(const Eigen::VectorXd z, const std::functionEigen::VectorXd(const Eigen::VectorXd) h) { int m z.size(); if (m 0) return; // 1. 计算观测预测值和 H Eigen::VectorXd hx h(x_); Eigen::MatrixXd H(m, x_.size()); double eps 1e-6; for (int i 0; i x_.size(); i) { Eigen::VectorXd xp x_; Eigen::VectorXd xm x_; xp(i) eps; xm(i) - eps; Eigen::VectorXd hp h(xp); Eigen::VectorXd hm h(xm); H.col(i) (hp - hm) / (2.0 * eps); } // 2. 新息 Eigen::VectorXd innovation z - hx; // 3. 卡尔曼增益 Eigen::MatrixXd S H * P_ * H.transpose() R_; Eigen::MatrixXd K P_ * H.transpose() * S.inverse(); // 4. 状态更新 x_ x_ K * innovation; // 5. 协方差更新 int n x_.size(); P_ (Eigen::MatrixXd::Identity(n, n) - K * H) * P_; // 6. 四元数归一化 Eigen::Vector4d q x_.segment4(3); q.normalize(); x_.segment4(3) q; // 7. 协方差对称化 P_ 0.5 * (P_ P_.transpose()); }这里有一个重要细节新息如果是角度量需要做角度归一化到 [-π, π]。但当你的观测是四元数分量而不是欧拉角时这个操作可以省掉。这也是我推荐用四元数分量做观测的原因之一代码简单还省得处理角度缠绕问题。另一个细节是 S 矩阵求逆我直接用了.inverse()。对于 3×3 或 6×6 的小矩阵没问题如果状态维数变大建议改用 QR 分解或者 LLT 分解求解 K避免数值问题。实际中 7 维状态下直接求逆完全够用我就没有上更复杂的分解。3.4 初始化和协方差保护EKF 启动时需要一个初始状态和初始协方差。第一个消息进来时我通常的做法是把第一个 odom 的位置作为初始位置把 IMU 的姿态换算为初始四元数P 设成一个较大的对角阵比如每个对角元取 1.0表示初始不确定度很大。如果 P 设得太小滤波器一开始就会对错误初始状态过度自信可能出现长时间无法收敛。协方差保护机制一定要写。我在实际调试中遇到过 P 矩阵非正定的情况原因通常是 Q 或 R 给得不合理或者 F 算错。一个简单的兜底是每次更新后检查 P 是否包含 NaN再做对称化并把负特征值强制置零。代码可以这样写void SimpleEKF::stabilizeCovariance() { if (!P_.allFinite()) { std::cerr EKF covariance has NaN, resetting to identity std::endl; P_ Eigen::MatrixXd::Identity(x_.size(), x_.size()); return; } P_ 0.5 * (P_ P_.transpose()); Eigen::SelfAdjointEigenSolverEigen::MatrixXd solver(P_); Eigen::VectorXd eig solver.eigenvalues(); if (eig.minCoeff() 0.0) { eig eig.cwiseMax(0.0); P_ solver.eigenvectors() * eig.asDiagonal() * solver.eigenvectors().transpose(); } }这个保护函数在调试阶段特别有用它不会修复根本问题但能防止一次数值异常直接带崩整个节点。等滤波器稳定运行后可以把这个保护函数留着反正开销很小关键时刻能兜底。4. 接进 ROS替换 robot_pose_ekf 的两种方案4.1 方案一改源码在 robot_pose_ekf 包内替换 EKF这个方案最贴合标题字面意思去 robot_pose_ekf 包的源码里把内部的 EKF 实现替换成自己写的类。具体动作是把你的 SimpleEKF 类编译成库再在 robot_pose_ekf 的 filter_base 实现中调用它的接口。但我对这个方案持谨慎态度。robot_pose_ekf 的代码结构比较老接口和现在惯用的 ROS 写法不太一致改动容易引入兼容性问题。比如它内部对消息协方差的转换、拒测逻辑和你的滤波核心耦合得很深替换后还要顺带适配它的 sensor 封装。如果纯粹是为了学习可以做这个替换跑通一次功能演示如果是业务项目我建议走方案二风险小得多。4.2 方案二写一个独立节点直接替代整个节点更干净的做法是新建一个节点订阅与 robot_pose_ekf 相同的话题发布相同的输出话题。这样下游节点完全无感知还可以随时在 launch 文件里切换原版和自己的版本方便做对比实验。我最初搭建的节点框架大致如下class EkfLocalizer { public: EkfLocalizer(ros::NodeHandle nh, ros::NodeHandle pnh) { ekf_ std::make_uniqueSimpleEKF(7, 3); ekf_-setNoise(Q_, R_); sub_odom_ nh.subscribe(odom, 10, EkfLocalizer::odomCallback, this); sub_imu_ nh.subscribe(imu/data, 10, EkfLocalizer::imuCallback, this); pub_pose_ nh.advertisegeometry_msgs::PoseWithCovarianceStamped(odom_combined, 10); pub_path_ nh.advertisenav_msgs::Path(ekf_path, 10); } private: void odomCallback(const nav_msgs::OdometryConstPtr msg) { double t msg-header.stamp.toSec(); double dt t - last_time_; if (dt 1e-4) { // 用最近的速度做一次预测 Eigen::VectorXd u(6); u msg-twist.twist.linear.x, msg-twist.twist.linear.y, msg-twist.twist.linear.z, msg-twist.twist.angular.x, msg-twist.twist.angular.y, msg-twist.twist.angular.z; ekf_-predict(u, dt); last_time_ t; } } void imuCallback(const sensor_msgs::ImuConstPtr msg) { double t msg-header.stamp.toSec(); double dt t - last_time_; if (dt 1e-4) { // 如果一帧 odom 都没来就直接用上一次的运动状态做预测 // 这里省略运动状态缓存细节 } // IMU 四元数虚部作为观测 Eigen::VectorXd z(3); z msg-orientation.x, msg-orientation.y, msg-orientation.z; ekf_-update(z, [](const Eigen::VectorXd x) { return x.segment4(3).head3(); // [qx; qy; qz] }); } std::unique_ptrSimpleEKF ekf_; ros::Subscriber sub_odom_; ros::Subscriber sub_imu_; ros::Publisher pub_pose_; ros::Publisher pub_path_; double last_time_ 0.0; };关键的点在消息回调之间的状态管理。odom 和 imu 消息不会严格对齐我维护一个last_time_变量。每收到任一话题先计算dt如果 dt 大于阈值就执行一次预测再根据当前消息类型执行对应的更新。这样无论是两种传感器哪个先到、频率孰高孰低都能保持时间上的连贯。4.3 时间同步与消息管理时间管理这一步看似简单实际是替换原版后最影响效果的地方。原版对每个传感器都设置了传感器超时剔除机制超过sensor_timeout就把它从融合中剔除。我在自己的实现里也借鉴了这个思想但做了简化每个传感器维护一个最近消息时间戳超过 0.2 秒没有新数据就认为该传感器暂不可用。每次预测时如果 dt 超过 0.2 秒就认为连续消息断裂主动将协方差增大到一个上限值防止长时间无消息时滤波器仍然保持过高的置信度。如果新消息时间戳比 last_time 还小直接丢弃并打印警告。ROS 中多话题发布频率不同乱序很常见这个问题不处理EKF 的时间线会被搅乱。这套逻辑写进节点后即使某个传感器断线再重连滤波器也能自动恢复而不是彻底发散。5. 参数调优与现场调试实录5.1 Q 和 R 怎么给一个能用的初值这是所有滤波器最容易让人崩溃的部分我在这里先给一个可复现的经验思路。过程噪声 Q 可以理解为“对运动模型的不信任程度”。假设机器人最大线速度 0.5 m/s最大角速度 0.5 rad/s那么位置过程噪声的标准差可以取最大值的 1% 到 5%也就是大约 0.005 到 0.025 m/s 量级。Q 的位置对角元就是(σ_v · dt)^2。如果 dt 是 0.01sσ_v 取 0.01那么 Q 的位置对角元是(0.01 * 0.01)^2 1e-8这个数值非常小。这是因为 Q 表示的是每一步预测中噪声的增量它会通过协方差传播不断累积一次性给太大滤波就会变得很“飘”。观测噪声 R 的初值我的土办法是把 IMU 固定静止录制 60 秒数据统计四元数虚部 qx、qy、qz 的标准差。假设标准差是 0.005那么 R 可以设成 2.5e-5 量级。这比直接抄别人的参数可靠得多因为每台 IMU 的噪声底噪都不一样。实际测试时先把 R 设小一点观察姿态是否抖动再把 R 设大一点观察姿态是否跟踪太慢。反复几次找到一个中间值。这个步骤没有捷径但盯着 rqt_plot 里的姿态曲线比看 log 日志高效得多。5.2 常见发散症状与排查顺序我把自己调试中遇到的几类典型问题整理成了一张速查表你可以按表对着排查。现象可能原因处理方式协方差爆炸数值超 1e6Q 过大、F 计算错误、dt 过大减小 Q检查数值雅可比步长检查时间戳是否跳变估计姿态抖动剧烈R 过小观测权重过大IMU 噪声比预期大增大 R重新统计 IMU 静置噪声轨迹往某个方向飞odom twist 坐标系定义与模型不一致确认 twist 是机体系还是世界系调整位置更新公式与真值相比有固定偏置传感器安装外参未标定检查 IMU 与 base_link 的静态外参融合输出滞后明显Q 太小或 R 太大滤波器过于信赖预测调小 R或调大 Q消息中断后位置跳变长时间无预测协方差过小增加协方差重置机制超过阈值自动增大 P这张表解决的是方向问题。每一个现象背后都有不止一种原因所以排查时要先把时间戳和坐标系统一这两件事排除掉再动滤波参数。我自己踩过最深的坑就是坐标系问题一开始把机体系速度当成世界系速度用位置更新公式写错了调了一整天参数都没用最后才发现是基础定义错了。5.3 用 bag 回放和 rqt_plot 做验证我建议的验证流程是先用rosbag record录制一段包含 odom、imu 和参考真值如果有的数据然后离线回放对比原版 robot_pose_ekf 和自己 EKF 的输出。回放时注意关闭 TF 的持久广播避免时间戳混乱。rqt_plot 可以直接绘制话题字段。比如对比融合后位置和原始 odom 位置命令差不多是这样的rqt_plot /odom_combined/pose/pose/position/x \ /odom_combined/pose/pose/position/y \ /odom/pose/pose/position/x \ /odom/pose/pose/position/y如果希望量化对比我推荐用 evo 工具专门做里程计轨迹误差评估支持 ROS bag 格式。输入两个话题输出 APE 和 RPE 的均值以及曲线图替换前后各跑一遍效果一目了然# 回放数据 rosbag play your_bag.bag # 用 evo 量化估计轨迹与参考真值的误差 evo_ape bag your_bag.bag /ground_truth/odom /odom_combined/pose自己写的 EKF 不一定比原版强。对标准传感器组合原版参数调好了完全够用。这个工作的价值在于你可以按需定制把慢漂移的 IMU yaw 排除、把里程计置信度根据打滑程度在线缩放、加入自定义传感器时不需要大改框架。这些都是原版很难做到的事情。6. 这套实现后续还能怎么扩展写完一个能跑通的 EKF 只是一个开始。我实际用下来最大的感受是当你拥有了一个自己能控制的滤波核心改代码的成本会降低一个量级。后面我再往里面加传感器、加状态量都是在这个框架上做增量。一个很自然的扩展是增加状态维度把 IMU 的陀螺零偏 b_g 和加速度计零偏 b_a 也估计进去。状态从 7 维变成 15 维只需要修改 stateTransition 里的姿态更新和位置更新加入 bias 的反馈项然后在预测时给 bias 加一个随机游走模型。因为我的类不绑定固定维度维度一改F 和 P 自动跟着变改动量很小。这正是第 1 章说的“状态扩展受限”问题自研后彻底解决。第二个扩展是把 EKF 换成 UKF。如果系统非线性程度很高尤其观测模型里加入了 GPS 的球面坐标转换、或者加入带大角度旋转的视觉观测时EKF 的一阶线性化可能不够。UKF 不需要求雅可比直接用一组 sigma 点传播分布工程上更省心。我自己现在倾向于在强非线性场景下使用 UKF但 EKF 的计算量小、成熟度高依然是优先选项。第三个扩展是自适应噪声。机器人打滑时odom 的置信度应该自动下降IMU 受振动时姿态观测的置信度也应该自动下降。一个粗粒度的实现是维护一个新息序列的滑动窗口当窗口内新息方差明显大于 R 的预期值时就按一定比例放大 R 或 Q。这个做法不完美但能明显改善长时间运行的效果我试过之后体感很好。最后说一句我自己实际操作中的体会。很多人一开始把 Q 和 R 的数值当成玄学其实这两个东西和你的传感器噪声直接相关。认真做一次 IMU 静止噪声统计认真确认一次 odom twist 的坐标系定义比盲目调参数管用得多。我第一版 EKF 跑起来的时候在车间里开了几十圈最后低头一看轨迹明显比原版平滑长走廊上的航向保持也好了很多那时候才觉得这一步做得值。