1. 为什么我放着现成的 robot_pose_ekf 不用偏要自己写一个做机器人定位、导航的朋友对robot_pose_ekf应该都不陌生。ROS 里这套老牌扩展卡尔曼滤波包能把轮式里程计、IMU、视觉里程计等传感器数据融合成一个带协方差的位姿估计。但从我的实际工程经验来看这个包在不少场景下都让人有点使不上劲的感觉——不是它不能用而是它太像一个黑盒子了。具体往细了说有三点让我最终决定动手自己写第一调参全凭感觉。robot_pose_ekf提供了output_frame、base_footprint等参数但真正决定融合效果的噪声协方差矩阵它藏在源码里通过配置文件能调的维度非常有限。我遇到过一次实际轮式里程计打滑的场景想针对性地加大里程计噪声结果发现配置项根本不支持这种细粒度控制最后只能改源码重新编译。第二中间过程不可观测。你只能看到最终输出的PoseWithCovarianceStamped但滤波器的预测协方差、卡尔曼增益、残差这些中间量全部没有输出。这就导致一个问题一旦轨迹发散或者抖动你根本不知道是预测模型不对还是观测噪声给错了还是时间戳处理出了 bug。这种盲人摸象式的排查方式在工期紧的时候特别折磨人。第三算法扩展性受限。如果你想把 GPS 或者 UWB 定位也加进来robot_pose_ekf的代码结构对二次开发并不友好。它早期版本的代码可读性不算好重叠的消息队列管理逻辑也相对笨重。我自己在给一台室外巡检机器人做定位时想融合轮式里程计、IMU 和 RTK-GPS最后发现与其打补丁式的改robot_pose_ekf不如基于它的消息接口标准写一个自己能完全掌控的 EKF 滤波器。这篇博文就是把那次实践完整复盘一遍把源码思路、模型设计、调参陷阱和 Debug 经验都摊开来讲。这篇内容适合的读者是对 EKF 只有概念性认识、想真正动手实现一个可用滤波器的 ROS 开发者以及被robot_pose_ekf的黑盒调参折磨过、希望通过自写滤波器获得掌控感的算法工程师。我会从原理讲到代码再讲到参数调试的完整链路确保你看完能跑起来也能改得动。2. 先别急着写代码把 EKF 的原理用自己的话捋清楚很多人一看到卡尔曼滤波的五个公式就头皮发麻其实它的核心思想特别朴素你有一个运动模型帮你猜有一堆传感器帮你测最终结果就是这两者的加权平均权重由各自的置信度决定。2.1 状态向量怎么定义做robot_pose_ekf替换最标准的做法是把状态向量定义成x [x, y, z, roll, pitch, yaw]也就是三维位置加三维姿态。为什么不用四元数表示姿态因为四元数是四维单位约束更新时要额外处理归一化约束对初学者不友好。欧拉角虽然会有万向锁问题但在 AGV、室内巡检机器人这类俯仰角和横滚角都比较小的场景下实用性远大于理论隐患。这个状态向量直接映射到 ROS 的geometry_msgs/PoseWithCovarianceStamped位置放在pose.position姿态用tf2::Quaternion从欧拉角转换后放入pose.orientation6x6 协方差矩阵对应pose.covariance和robot_pose_ekf的输出格式完全一致。2.2 预测阶段到底在猜什么预测阶段要回答的问题只有一个给定上一时刻的状态和控制输入这一时刻的机器人大概在哪以轮式里程计为例假设机器人做平面运动t-1 时刻位姿是 (x, y, yaw)里程计测得位姿增量是 (dx, dy, dyaw)那么预测就是x_pred x_prev dx * cos(yaw) - dy * sin(yaw) y_pred y_prev dx * sin(yaw) dy * cos(yaw) yaw_pred yaw_prev dyaw这个公式的本质是坐标变换把里程计在机器人坐标系下测到的增量旋转到世界坐标系。很多初写 EKF 的人在这里容易搞错方向把 cos、sin 的符号弄反导致滤波结果在机器人转弯时出现奇怪的偏移。协方差预测稍微抽象一点但也别怕它的逻辑是P_pred F * P_prev * F^T Q用大白话说就是上一步的置信度经过状态转移矩阵 F 传播到这一步然后加上运动模型自身的噪声 Q。F 矩阵在常数模型中就是单位阵在考虑姿态变换时则需要对 yaw 求偏导也就是雅可比矩阵。这地方是 EKF 和线性卡尔曼滤波的唯一区别模型非线性所以用雅可比近似。2.3 更新阶段怎么把猜和测融合起来预测完以后传感器数据来了比如 IMU 告诉你 yaw 是 1.52 rad而预测值是 1.48 rad差了 0.04 rad。该信谁答案是谁噪声小就信谁多一点。这个信多少就是卡尔曼增益 K 干的事K P_pred * H^T * (H * P_pred * H^T R)^(-1)K 是一个 6x6 的矩阵它的大小取决于预测协方差 P 和测量噪声 R 的比值。如果测量噪声 R 很小传感器很准K 就偏向测量如果预测协方差 P 很大运动模型可信度低K 也偏向测量。更新才是真正的融合x_upd x_pred K * (z - H * x_pred) P_upd (I - K * H) * P_predz - H*x_pred叫残差也叫新息。它表示的是实际测量值和预测应该测到的值之间的差。残差乘以 K就是对预测值的修正量。协方差更新则是用观测信息压缩预测的不确定度。每来一次观测P 的行列式只会变小或不变不会变大这是卡尔曼滤波的一个基本性质也是判断滤波器是否正常工作的一个参考指标。2.4 五个核心符号的物理含义速记这里我用自己的经验整理一个速查表代码实现时对照着来不迷路矩阵/符号物理含义代码里的命名习惯F状态转移矩阵把上一时刻状态映射到当前时刻F_P状态协方差矩阵表示对状态估计的置信度P_Q运动模型过程噪声矩阵表示猜的不确定性Q_H测量矩阵把状态向量映射到测量空间H_R测量噪声矩阵表示测的不确定性R_K卡尔曼增益融合权重K_理解这六个量EKF 的代码就成功了一半。接下来进入真正的工程实现。3. 代码架构与核心实现如何写出可替换 robot_pose_ekf 的滤波器3.1 软件框架与消息流设计自写 EKF 要无缝替换robot_pose_ekf最核心的一点是消息接口必须对齐。robot_pose_ekf的输入输出接口是这样的输入/odomnav_msgs/Odometry轮式里程计消息/imu_datasensor_msgs/ImuIMU 消息/vonav_msgs/Odometry视觉里程计消息可选输出/robot_pose_ekf/odom_combinedgeometry_msgs/PoseWithCovarianceStamped融合后的位姿我自己写的节点直接用相同的话题名称和消息类型但名字改成了/ekf_fused/odom_combined方便和原版同时运行做对比验证。整个软件框架分两层EKF纯算法类不依赖 ROS只负责矩阵运算方便单元测试EKFNodeROS 节点类处理消息订阅、时间同步、坐标变换、发布结果这样设计的核心原因是算法和通信解耦。如果算法里混着 ROS 的类型想用 gtest 做单元测试就得拉起整套 ROS 环境非常痛苦。我把EKF类用纯 C 写成用的是 Eigen 库的矩阵类型这样在 Ubuntu 上开个终端就能编译测试。3.2 类原型设计class SimpleEKF { public: SimpleEKF(); // 预测输入轮式里程计的位姿增量 void predict(double dt, double dx, double dy, double dyaw); // 姿态更新输入 IMU 的角速度积分结果 void updateFromImu(double roll, double pitch, double yaw); // 位置更新输入外部绝对位置观测可选 void updateFromPose(double x, double y, double z); // 获取当前状态 Eigen::VectorXd state(); Eigen::MatrixXd covariance(); private: Eigen::VectorXd x_; // 6维状态向量 Eigen::MatrixXd P_; // 6x6 协方差矩阵初始化为单位阵乘小量 Eigen::MatrixXd F_; // 6x6 状态转移矩阵 Eigen::MatrixXd Q_; // 6x6 过程噪声矩阵 Eigen::MatrixXd H_; // 3x6 测量矩阵IMU 更新时 Eigen::MatrixXd R_; // 3x3 测量噪声矩阵IMU 更新时 };关键初始化SimpleEKF::SimpleEKF() { x_ Eigen::VectorXd::Zero(6); P_ Eigen::MatrixXd::Identity(6, 6) * 0.1; Q_ Eigen::MatrixXd::Identity(6, 6) * 0.01; R_ Eigen::MatrixXd::Identity(3, 3) * 0.05; }P 初始化为较小值表示对初始位姿较有信心Q、R 的初值后续要根据实际传感器数据调。3.3 预测实现从轮式里程计和 IMU 获取运动增量预测发生在两类数据到达时轮式里程计消息到达提供 dx、dy、dyawIMU 消息到达提供角速度可用于姿态预测我先写轮式里程计的预测。void SimpleEKF::predict(double dt, double dx, double dy, double dyaw) { double yaw x_(5); // 把里程计坐标系下的增量转换到世界坐标系 double delta_x_world dx * cos(yaw) - dy * sin(yaw); double delta_y_world dx * sin(yaw) dy * cos(yaw); // 状态转移矩阵 F位置更新由 yaw 决定是雅可比矩阵 F_ Eigen::MatrixXd::Identity(6, 6); F_(0, 2) -dx * sin(yaw) - dy * cos(yaw); // dx/dyaw F_(1, 2) dx * cos(yaw) - dy * sin(yaw); // dy/dyaw F_(3, 5) 1.0; // roll 受 yaw 影响简化模型 F_(4, 5) 1.0; // pitch 受 yaw 影响简化模型 // 状态预测 x_(0) delta_x_world; x_(1) delta_y_world; x_(5) dyaw; // 协方差预测 P_ F_ * P_ * F_.transpose() Q_; }F 矩阵里对位置的偏导是重点它的物理含义是yaw 误差会引起位置增量旋转所以协方差传播时必须考虑这种耦合。把 F_(0,2) 设为-dx*sin(yaw)-dy*cos(yaw)意味着当 yaw 有不确定性时x 方向的位置预测误差也会变大。这是模型中最容易漏掉的一项——如果 F 是单位阵等效于认为位置和角度完全解耦滤波效果会打折扣。然后是 IMU 辅助的预测。IMU 消息的角速度存在angular_velocity.z时间间隔dt由上一条 IMU 消息和当前消息的时间戳差算出void SimpleEKF::predictFromImu(double dt, double angular_velocity_z) { F_ Eigen::MatrixXd::Identity(6, 6); F_(3, 5) dt; // roll 受 yaw 速度影响简化模型 F_(4, 5) dt; x_(5) angular_velocity_z * dt; P_ F_ * P_ * F_.transpose() Q_; }这里我给的是简化处理方式。严格来说IMU 预测还应该包括横滚和俯仰角速度的积分工程上常用四元数积分处理姿态比欧拉角更稳定。但考虑到我的应用场景是 AGV 和巡检机器人俯仰和横滚角变化不大用欧拉角近似够用。如果你做的是无人机或足式机器人建议把姿态部分升级为四元数积分欧拉角在这种场景下会有万向锁导致的角度跳变问题。3.4 更新实现IMU 和轮式里程计的矫正3.4.1 IMU 姿态更新IMU 的 roll、pitch、yaw 可以通过加速度计和陀螺仪融合得到比如先对加速度计做atan2解算 roll/pitch再对陀螺仪做 yaw 积分这部分不在本文展开。假设我们已经从 IMU 驱动里拿到了这三个角度更新逻辑如下void SimpleEKF::updateFromImu(double roll, double pitch, double yaw) { // 测量值 Eigen::Vector3d z(roll, pitch, yaw); // 测量矩阵只看姿态不观测位置 H_ Eigen::MatrixXd::Zero(3, 6); H_(0, 3) 1.0; // roll H_(1, 4) 1.0; // pitch H_(2, 5) 1.0; // yaw // 残差处理角度环绕 Eigen::Vector3d innovation z - H_ * x_; innovation(2) wrapAngle(innovation(2)); // yaw 归一化到 [-pi, pi] // 卡尔曼增益 Eigen::MatrixXd S H_ * P_ * H_.transpose() R_; Eigen::MatrixXd K P_ * H_.transpose() * S.inverse(); // 状态更新 x_ K * innovation; // 协方差更新 P_ (Eigen::MatrixXd::Identity(6, 6) - K * H_) * P_; }注意这里残差里的innovation(2) wrapAngle(...)是角度更新的关键。如果 yaw 在 179 度IMU 测的是 -179 度两者的差值应该是 2 度而不是 -358 度。如果不做环绕处理滤波器会在角度跳变处产生巨大的修正量直接导致状态突变。这个坑我在第一次实现时踩过后面在常见问题章节展开讲。3.4.2 轮式里程计位置更新轮式里程计消息nav_msgs/Odometry里自带了pose.pose.position.x和y这相当于给了你一个绝对位置观测在轮子不打滑的假设下。虽然这个观测本身有累计误差但作为滤波器的观测输入完全可行也是robot_pose_ekf最基础的融合方式void SimpleEKF::updateFromOdom(double odom_x, double odom_y) { Eigen::Vector2d z(odom_x, odom_y); H_ Eigen::MatrixXd::Zero(2, 6); H_(0, 0) 1.0; H_(1, 1) 1.0; Eigen::Vector2d innovation z - H_ * x_; Eigen::Matrix2d S H_ * P_ * H_.transpose() R_odom_; Eigen::MatrixXd K P_ * H_.transpose() * S.inverse(); x_ K * innovation; P_ (Eigen::MatrixXd::Identity(6, 6) - K * H_) * P_; }如果只想融合位置而把航向交给 IMU可以只选x、y两维做位置更新这样 IMU 的航向和里程计的位置各司其职也是工程上常用的策略。3.5 时间戳同步与坐标系写滤波器最容易出问题的不是算法本身而是数据的时间对齐和坐标系对齐。消息订阅的时间问题我采用最简单的时间队列方式。因为这里融合的是轮式里程计和 IMU 两种来源它们各自有发布频率比如 50Hz 和 100Hz没必要用message_filters::syncPolicy做精确同步。我采用的是每个传感器各自维护一个最近消息缓存融合周期固定为 20ms50Hz到融合时刻取每个传感器缓存里时间戳最新的一条参与计算如果某传感器消息超过 100ms 没更新视为失效跳过该传感器更新坐标系问题更关键。在robot_pose_ekf中默认的输出坐标系是odom底座坐标系是base_footprint。我自己写的时候也要保持同样的框架// 发布融合位姿时把状态量塞到 PoseWithCovarianceStamped geometry_msgs::PoseWithCovarianceStamped out_msg; out_msg.header.stamp current_time; out_msg.header.frame_id odom; // 位置 out_msg.pose.pose.position.x ekf.state()(0); out_msg.pose.pose.position.y ekf.state()(1); out_msg.pose.pose.position.z ekf.state()(2); // 姿态 tf2::Quaternion q; q.setRPY(ekf.state()(3), ekf.state()(4), ekf.state()(5)); out_msg.pose.pose.orientation tf2::toMsg(q);然后tf的发布直接用tf2_ros::TransformBroadcaster把odom到base_footprint的变换发出去。这个变换就是滤波器的核心输出下游的move_base、amcl或者导航栈都依赖它。3.6 整体节点框架简析为了让你有个全局认识这里是EKFNode的核心骨架class EKFNode { public: EKFNode() : nh_(~) { odom_sub_ nh_.subscribe(/odom, 10, EKFNode::odomCallback, this); imu_sub_ nh_.subscribe(/imu_data, 10, EKFNode::imuCallback, this); fused_pub_ nh_.advertisegeometry_msgs::PoseWithCovarianceStamped(/ekf_fused/odom_combined, 10); } void odomCallback(const nav_msgs::Odometry::ConstPtr msg) { latest_odom_ msg; last_time_ msg-header.stamp.toSec(); } void imuCallback(const sensor_msgs::Imu::ConstPtr msg) { latest_imu_ msg; dt_ msg-header.stamp.toSec() - last_time_; last_time_ msg-header.stamp.toSec(); } private: ros::NodeHandle nh_; ros::Subscriber odom_sub_, imu_sub_; ros::Publisher fused_pub_; nav_msgs::Odometry::ConstPtr latest_odom_; sensor_msgs::Imu::ConstPtr latest_imu_; double dt_; SimpleEKF ekf_; };具体融合逻辑放在定时器回调里每 20ms 触发一次void EKFNode::timerCallback(const ros::TimerEvent) { if (latest_odom_) { double dx latest_odom_-twist.twist.linear.x * dt_; double dy latest_odom_-twist.twist.linear.y * dt_; double dyaw latest_odom_-twist.twist.angular.z * dt_; ekf_.predict(dt_, dx, dy, dyaw); } if (latest_imu_) { // 用 IMU 的角速度辅助预测 double wz latest_imu_-angular_velocity.z; ekf_.predictFromImu(dt_, wz); // 用 IMU 解算出的姿态做更新 ekf_.updateFromImu(roll, pitch, yaw); } // 发布融合结果 }这个定时器回调里其实还有一个隐藏的 bug 点dt_到底用谁的我最早用 IMU 消息时间戳来算dt_但在轮式里程计发布频率和 IMU 不一致时dt_就会偏大或偏小导致预测步长错误。**正确的做法是每次预测时用当前时间减去上一次融合结束的时间而不是用传感器消息的时间戳间隔。**后面我专门为这个问题加了一个last_fusion_time_变量从根上解决。4. 参数调试与避坑实录Q、R、角度环绕、协方差发散代码写完只是第一步真正决定滤波器能不能用的全在调试阶段。我从实际项目中总结了几条高频踩坑经验。4.1 Q 矩阵怎么设真的不是越大越好Q 矩阵表示运动模型的置信度Q 越大代表你越不信任运动模型滤波器就更依赖传感器测量。直觉上看好像 Q 大一点能更快跟踪测量但太大会导致输出抖动剧烈因为每一次测量噪声都会被充分采信。我的经验做法是出厂先设在 0.001 量级观察机器人原地旋转时 yaw 的输出曲线。如果曲线平稳但滞后传感器太多增大对应维度的 Q如果曲线毛刺太多减小 Q。一次只动一维不要同时改多个量否则根本无法定位是哪路噪声引起的。比如 AGV 的轮式里程计编码器分辨率较好Q_(0,0)和Q_(1,1)设在0.0001~0.001之间就够但如果机器人是在草地上跑轮胎打滑严重那就要调到0.01甚至更大否则位置预测误差会被严重低估。4.2 R 矩阵设错的典型表现R 矩阵是测量噪声矩阵表示传感器的信任度。R 太小滤波器过度相信单次测量结果就是位置跟随测量曲线剧烈跳动R 太大滤波器反应迟钝传感器提示已经转弯了输出还赖在旧值上不动。一个经典的判断标准用 rqt_plot 同时画滤波输出 yaw 和 IMU 原始 yaw。如果滤波曲线比原始数据平滑很多但在弯道处明显滞后说明 R 偏大如果滤波曲线和原始数据几乎重叠但毛刺多说明 R 偏小。反复权衡后找到一个中间值让输出既平滑又不失响应。具体到 IMU 的 R 矩阵我从工程实践中给出的参考量级是yaw 的方差可以取0.02 rad^2左右roll/pitch 因为解算精度稍差可以取0.05 rad^2左右。这个值还需要根据传感器型号调但大方向是固定的。4.3 角度环绕和姿态奇异问题角度环绕是 EKF 中最隐蔽的问题没有之一。我写的第一版代码没有 wrapAngle结果机器人每次转圈经过 180 度边界时输出角度会突然从 179 度跳到 -179 度滤波曲线看起来像被人硬生生掰断一样。解决办法是在所有涉及角度残差的地方调用 wrap 函数double wrapAngle(double angle) { while (angle M_PI) angle - 2.0 * M_PI; while (angle -M_PI) angle 2.0 * M_PI; return angle; }把一切角度差值先 wrap 到 [-pi, pi] 再进滤波器问题基本消失。姿态奇异万向锁在 AGV 场景下不常见但如果你用的是欧拉角表示横滚和俯仰且机器人存在大幅翻转的可能就要特别注意当 pitch 接近 90 度时roll 和 yaw 之间的耦合会造成雅可比矩阵奇异协方差矩阵也会因此异常。我的解决方案是在状态转移矩阵构建时对接近奇异的俯仰角做钳位处理或者干脆把状态量换成四元数难度会显著上升。对于室内 AGV做钳位处理即可。4.4 协方差发散一个必须提前防范的 bug协方差发散的表现是P 矩阵对角线元素越来越大甚至出现 NAN滤波器彻底失去意义。发散的原因主要有三个时间戳跳变。如果传感器消息的header.stamp偶尔出现异常大跳变dt会变成几十秒F 矩阵的雅可比项被放大P 直接冲上天。解决方法是加一个dt上限钳位比如dt min(dt, 0.1)。P 矩阵失去正定性。由于浮点误差累积P 矩阵可能逐渐偏离对称正定。我采用的工程措施是每隔 100 步强制对称化一次P_ 0.5 * (P_ P_.transpose()); // 强制对角线非负 P_.diagonal() P_.diagonal().cwiseMax(1e-9);测量更新频率过低。如果订阅的传感器掉线滤波器一直只做预测不更新P 会随 Q 累积而爆炸。这时不如直接输出预测值而不更新协方差或者对 P 设置一个上界。我在代码里加了 P 对角线最大 100 的限制超过就压缩回去这虽然是个粗暴手段但在工程现场经常是最实用的。4.5 和 robot_pose_ekf 的输出做对比验证替换工作做完后建议让新旧两个滤波器同时运行用相同输入数据对比输出。具体做法是录制一份 rosbag同时播放让两个节点分别订阅然后用plotjuggler对比两个输出的 x、y、yaw 曲线。我当时的实测结果非常有意思静止状态下两者差异极小都在传感器噪声范围内直线运动时新滤波器的位置响应略快于robot_pose_ekf因为它的默认参数比较保守急转弯时原版robot_pose_ekf有轻微超调而我自己调的 Q/R 比例更合适超调几乎看不到差异的原因就是 Q/R 参数的标定并不是算法本身的优劣。robot_pose_ekf作为通用方案参数必然面向大多数场景自写滤波器的优势就在于可以精确针对你的机器人平台标定。5. 常见问题速查表从症状到根因再到解法的实战笔记我把调试期间遇到和同行交流过的典型问题整理成了一张速查表强烈建议收藏。遇到问题先对照定位能省下大量翻源码的时间。症状可能原因解决方法输出角度在 ±180 度附近跳变角度残差未 wrap所有角度差值 wrap 到 [-pi, pi]滤波结果与原版 robot_pose_ekf 相比明显滞后R 偏大或 Q 偏小调低 R 或调高 Q幅度每次 10 倍级调整位置输出剧烈抖动像噪声被完全放大R 偏小过于相信单次测量增大对应测量维度的 R急转弯后位置轨迹向外飘里程计预测模型里 F 矩阵缺 yaw 偏导项补全雅可比矩阵 F 中位置与 yaw 的耦合项运行几分钟后输出变成 NAN时间戳跳变导致 dt 异常P 发散dt 钳位 P 定期对称化 对角线设下界静止时 yaw 缓慢漂移IMU 陀螺零偏未补偿先标定 IMU 零偏或在滤波前减零偏融合输出正常但 TF 树错误发布了错误的 frame_id确认输出 frame_id 为 odom子坐标系为 base_footprint轮式里程计打滑严重时定位漂移Q 太小模型置信度过高动态增大轮式里程计在打滑时的噪声 Q除了这张表我再分享三套调试工具权当压箱底的经验调试三件套之一rqt_plot 看 P 矩阵对角线。P 矩阵的对角线就是各状态变量的方差它们的变化趋势直接反映滤波器是否健康。正常运行时预测阶段 P 会上升更新阶段 P 会下降呈现规律的锯齿波形。如果你看到某条线一直单调上升说明该维度的观测量不足或 Q 太大。调试三件套之二录制 rosbag 反复回放。调参必须用同一份 bag 文件对比不同参数的效果否则场地、载荷、速度都不一样根本没法判断是参数问题还是路况问题。我会在场地里设定几个特征点直角弯、S 弯、长直线回放时观察滤波输出经过这些点时是否平滑切换。调试三件套之三固定 R 只动 Q。同时调 Q 和 R 会陷入无限循环因为它们的比值才是决定性因素。我的标准流程是先根据传感器手册量级固定 R然后只调 Q 让输出响应达到满意最后再微调 R 的绝对值控制平滑度。这套方法虽然是工程上的土办法但实测下来效率很高。最后补一个调试案例我认为很值得记录下来。某次测试中滤波输出的 yaw 在机器人原地旋转时总是滞后 IMU 约 0.2 秒。我先以为是 R 太大把 R 调小了 10 倍结果滞后反而更明显——这很反直觉。后来检查发现根本不是 R 的问题而是 IMU 更新被放在了预测之前导致上一时刻的 IMU 数据被当成当前时刻用。修复了回调顺序后滞后从 0.2 秒降到几乎为 0。这个教训说明滤波器出问题时先查数据流时序再调参数顺序别搞反。整个实践中我的体会是写一个能跑的 EKF 并不难难的是把它修到既稳又准。你必须对每个矩阵的物