简介面向惯性导航与组合导航开发者的代码资源以扩展卡尔曼滤波EKF为核心融合 INS/GPS 数据解决单一惯导累计漂移问题。压缩包共 19 个文件以 C 源码为主8 个 .cpp 与 8 个 .h另含 Makefile、README.md 与 .gitignore整体仅 21KB代码结构紧凑适合学习 EKF 原理与导航算法移植。资源包含加速度计、陀螺仪、磁力计等传感器模型以及 EKF 状态预测、观测更新、协方差更新和 GPS 滤波模块README 对工程组成和编译方式做了说明便于快速搭建仿真环境。通过阅读和运行这些代码可以理解组合导航中多传感器数据融合的完整流程掌握从传感器模拟到 EKF 估计、再到结果可视化的实现细节为后续设计实际导航系统或改进滤波算法提供可直接参考的代码基础。已有 468 人学习下载适合具备一定导航基础、希望用代码加深理解的工程师与研究生。1. 惯组卫导组合导航的C落地从INS_EKF_master读起做机器人导航或车辆定位的人收藏夹里多半有十来份组合导航资料但真到要放到板子上跑的时候大部分MATLAB课件就帮不上忙了。INS_EKF_master这套代码是C写的不是用来做学术复现的而是把加速度计、陀螺仪、磁力计、GPS这几路传感器直接接进同一个EKF状态机。它适合两种人一是惯性导航刚起步想抄一个能编译、能改参数的工程骨架二是已经在跑GPS/IMU松耦合组合导航但想知道协方差怎么设、传感器预处理怎么做、磁力计为什么只在航向漂移时才起作用。先说明一点网上不少地方把它描述成MATLAB实现实际压缩包里是C工程带Makefile和main.cpp按工程代码走更贴近真实产品设计。2. 拆解INS_EKF_master源码IMU/GPS数据读取、滤波与融合管线2.1 文件划分与数据通路压缩包里的文件看起来零散但按职责划分很清晰。先把每个模块对应到组合导航数据链路后面看代码就不容易迷路。文件模块职责为EKF提供什么Captor.cpp/h数据采集与时间戳管理原始IMU/GPS样本统一时间基准GYRO.cpp/h陀螺仪原始数据解析与单位换算角速度单位rad/s或deg/sACCELEROMETER.cpp/h加速度计原始数据解析比力单位m/s^2MAGNETOMETER.cpp/h磁力计数据解析与航向计算磁航向参考GPS.cpp/hGPS帧解析、经纬度与速度提取ECEF/经纬度位置、速度GPS_Filter.cpp/hGPS输出预处理剔除野值、平滑后的位置速度EKF.cpp/h状态预测与观测更新组合导航核心状态量main.cpp调度各模块控制滤波节奏程序入口与结果输出数据通路是一条直线Captor取原始传感器数据分别经过GYRO、ACCELEROMETER、MAGNETOMETER和GPS解析GPS再单独过一次GPS_Filter最后EKF做时间更新和量测更新。main.cpp负责把这条链路按固定频率跑起来。读取代码时建议先从main.cpp的循环结构看起确认每个传感器多久取一次数、EKF是在IMU中断里跑还是放在统一调度里这两个问题决定了之后时间同步怎么做。2.2 传感器原始测量模型比力、角速度与磁航向加速度计输出的不是纯粹的平移加速度而是比力也就是物体受到的合外力去掉重力后的等效测量值。静止时加速度计读数就是重力加速度方向指向天顶反方向这个特性是初始姿态对准的基础。陀螺仪输出角速度但存在零偏长时间积分会带来姿态漂移。磁力计输出地磁场矢量水平放置时可以推算磁航向但容易受周围铁磁物质干扰。代码里这三个传感器通常会打包成一个结构体typedef struct { double wx, wy, wz; // 陀螺仪角速度单位 rad/s double ax, ay, az; // 加速度计比力单位 m/s^2 double mx, my, mz; // 磁力计磁场强度单位 uT uint64_t timestamp_ms; // 采样时间戳 } ImuSample;使用时要注意单位一致性。加速度计如果原始输出是g要先乘以9.80665换算成m/s^2陀螺仪如果输出的是deg/s要乘以π/180换成rad/s。EKF的状态方程里速度微分项包含重力加速度向量单位不统一会出现几米每秒的常值速度偏差排查起来非常隐蔽。我一般会在解析函数入口处做一次强制转换并加一个unit_flags字段来记录单位来源这样后面换传感器时不用反复猜。2.3 预处理与滤波管线设计GPS原始输出的位置在城市峡谷或树荫下会周期性跳变直接进EKF会让协方差被拉偏。GPS_Filter的作用就是在进EKF之前先把明显不合理的测量挡掉。常见做法是检查定位标志、HDOP值以及相邻两次位置的跳变幅度。GpsSample GPS_Filter::update(const GpsSample raw) { // 定位无效或水平精度因子过大时保留上一次有效值 if (raw.fix_type 2 || raw.hdop 4.0) { return last_good_; } // 计算与上一个有效点的距离超过阈值认为是野值 double dx raw.x_enu - last_good_.x_enu; double dy raw.y_enu - last_good_.y_enu; if (dx * dx dy * dy jump_threshold_m2_) { return last_good_; } last_good_ raw; return raw; }HDOP阈值一般取3到5市区密集高楼场景可以放宽到6但超过6的定位点水平误差可能达到十几米进EKF反而拖累整体精度。跳变阈值根据载体动态来定步行机器人可以设2米车载可设10米无人机设5米左右。注意被过滤掉的GPS帧不应该直接丢弃而应该让EKF跳过这个时刻的量测更新只做预测否则时间基准会对不齐。磁力计预处理同样重要。3. EKF组合导航核心实现预测方程、量测更新与协方差递推3.1 状态向量与协方差初始化这套代码的核心是15维状态向量相比常见的12维多了加速度计零偏。经过实际对比15维模型的航向保持能力明显更好因为GPS速度更新能持续估计加速度计零偏进而修正姿态姿态修正后速度积分的位置误差也会下降。状态索引物理量单位维度0-2ECEF或ENU位置m33-5速度m/s36-9姿态四元数无量纲410-12陀螺仪零偏rad/s313-14加速度计零偏m/s^22初始化时协方差矩阵P0要体现“对初始值有多大信心”。位置可以给到1m的方差速度给0.1m/s姿态给0.1rad陀螺零偏给0.01rad/s加速度计零偏给0.05m/s^2。给太小会让滤波器在前几百毫秒猛烈收敛反而把轨迹拉出尖角。代码里一般直接用矩阵块赋值void EKF::init(const NavState init_state) { state_ init_state; P_.setZero(); P_.block3,3(0,0) Eigen::Matrix3d::Identity() * 1.0; // 位置 P_.block3,3(3,3) Eigen::Matrix3d::Identity() * 0.01; // 速度 P_.block4,4(6,6) Eigen::Matrix4d::Identity() * 0.01; // 姿态 P_.block3,3(10,10) Eigen::Matrix3d::Identity() * 1e-4; // 陀螺零偏 P_.block3,3(13,13) Eigen::Matrix3d::Identity() * 5e-4; // 加计零偏 }协方差初始化的核心思想是“宁可大一点不要太小”。EKF本质上是加权最小二乘的递推形式初始协方差过小意味着滤波器非常信任初值GPS更新要花很久才能把姿态误差拉回来。3.2 非线性状态预测函数EKF和标准KF的区别在于状态转移和观测模型都是非线性的所以要用雅可比矩阵做局部线性化。IMU采样频率一般50到200Hz预测步长dt很小四元数更新可以用一阶近似。核心逻辑类似下面这样void EKF::predict(const ImuSample imu, double dt) { // 角速度补偿零偏 Eigen::Vector3d w imu.gyro - state_.gyro_bias; // 四元数一阶积分 Eigen::Vector4d dq; dq 0.0, w.x() * dt / 2.0, w.y() * dt / 2.0, w.z() * dt / 2.0; state_.quat quatMultiply(state_.quat, dq); state_.quat.normalize(); // 比力到加速度再补偿重力 Eigen::Vector3d acc imu.acc - state_.acc_bias; Eigen::Vector3d acc_world state_.quat.toRotationMatrix() * acc; acc_world.z() - 9.80665; // 速度和位置积分 state_.vel acc_world * dt; state_.pos state_.vel * dt 0.5 * acc_world * dt * dt; // 协方差递推: P F * P * F^T Q Eigen::MatrixXd F computeJacobian(state_, imu, dt); P_ F * P_ * F.transpose() Q_; }这里最容易被忽略的是四元数归一化。连续积分后四元数模长会偏离1不归一化会导致姿态矩阵不再是正交阵重力补偿方向出错位置误差发散。协方差递推时F矩阵是15×15的雅可比代码里通常用数值扰动法或解析法计算解析法运行效率更高。还要注意dt不能直接用IMU标称周期应该从相邻两次采样的时间戳差分计算因为实际中断频率可能有抖动固定dt在高动态场景下会引入额外的速度误差。3.3 GPS观测更新与卡尔曼增益计算GPS观测更新属于松耦合方式也就是只用GPS输出的位置和速度不用原始伪距或载波相位。观测方程是线性的H矩阵不需要求导这是代码里相对简单但很关键的部分。位置观测对应状态索引0-2速度观测对应3-5。void EKF::updateWithGps(const GpsSample gps) { // 观测向量 Eigen::VectorXd z(6); z gps.x, gps.y, gps.z, gps.vx, gps.vy, gps.vz; // 预测观测 Eigen::VectorXd z_hat(6); z_hat state_.pos, state_.vel; // 观测矩阵 Eigen::MatrixXd H Eigen::MatrixXd::Zero(6, 15); H.block3,3(0,0) Eigen::Matrix3d::Identity(); H.block3,3(3,3) Eigen::Matrix3d::Identity(); // 卡尔曼增益 Eigen::MatrixXd S H * P_ * H.transpose() R_; K_ P_ * H.transpose() * S.inverse(); // 状态修正 Eigen::VectorXd dx K_ * (z - z_hat); state_.pos dx.segment3(0); state_.vel dx.segment3(3); // 四元数增量修正后重新归一化 Eigen::Vector3d dtheta dx.segment3(6); state_.quat quatMultiply(state_.quat, axisAngleToQuat(dtheta)); state_.quat.normalize(); state_.gyro_bias dx.segment3(9); state_.acc_bias dx.segment3(12); // 协方差更新 P_ (Eigen::MatrixXd::Identity(15, 15) - K_ * H) * P_; }注意位置更新时如果状态量使用的是ENU坐标系GPS的经纬高要先做一次坐标转换转到和状态向量相同的坐标系。很多组合导航工程问题的根源就在这里经纬度和ENU混用EKF增益算出来完全是错的。四元数状态增量的叠加方式也和普通向量不同角度增量要转成四元数再做乘法不能直接加到state_.quat上。4. 组合导航参数配置Q/R矩阵取值、时间同步与松耦合调优4.1 噪声矩阵R按GPS接收机精度给R矩阵表示对GPS测量的信任程度取值应该来自接收机标称精度而不是随意调。消费级GPS定位标准差一般在2.5到5米速度标准差0.05到0.2m/s价格不同性能差异很大。R设得越小滤波器越信任GPS位置轨迹会更贴GPS但动态性能会变差R设得越大轨迹更平滑但可能产生几百米的稳态误差。参数典型值调整方向位置噪声2.5-5 m城市峡谷中适当放大速度噪声0.05-0.2 m/s动态场景适当减小磁航向噪声1-3 deg磁场干扰大时放大气压计高度噪声1-2 m有气压计模块时启用实际做产品时我会先用GPS模块给出的pos_std和velocity_std字段直接填充R对角元再根据残差序列调整。EKF输出的新息innovation序列能反映R是否合理如果新息均值持续偏离零说明R偏小或系统模型有偏如果新息方差远大于理论值S说明R需要放大。4.2 过程噪声Q按IMU指标估算Q矩阵是过程噪声协方差体现了对IMU模型的信任程度和对零偏漂移速度的估计。Q给太小滤波器认为预测非常准GPS更新权重会越来越低最终路径干脆不跟GPS走Q给太大姿态和速度会跟着GPS噪声高频抖动滤波效果丧失了。代码里常用的做法是根据IMU数据手册的艾伦方差参数来构造Q。比如陀螺噪声密度0.01deg/s/√Hz加速度计噪声密度100μg/√HzIMU频率100Hz则// IMU噪声密度换算每步预测的噪声增量 double gyro_noise 0.01 * M_PI / 180.0; // deg/s/sqrt(Hz) - rad/s/sqrt(Hz) double acc_noise 100e-6 * 9.80665; // ug - m/s^2/sqrt(Hz) double dt 0.01; // 对应100Hz Eigen::MatrixXd Q Eigen::MatrixXd::Zero(15, 15); Q.block3,3(10,10) Eigen::Matrix3d::Identity() * gyro_noise * gyro_noise * dt; Q.block3,3(13,13) Eigen::Matrix3d::Identity() * acc_noise * acc_noise * dt;陀螺零偏的随机游走项是调Q时最容易超调的地方。Q_gyro_bias给太大航向就会跟着GPS噪声来回摆给太小陀螺零偏收敛后如果温度变化导致真实零偏漂移EKF要很久才能追回来。我一般先按数据手册值配置然后用罗德里格旋转测试和8字形行驶数据来观察航向残差再微调一到两个数量级。4.3 时间同步与观测调度GPS频率通常10HzIMU是50到200Hz两者时间不同步会导致量测更新用的状态是过去时刻的预测值。简单做法是用时间戳最近原则匹配但更稳的方式是维护一个短IMU缓存每次GPS数据到达时找到对应时刻的预测状态再更新。void NavigationCore::onGpsSample(const GpsSample gps) { // 找缓存里最接近GPS时间戳的IMU样本 while (!imu_cache_.empty() imu_cache_.front().timestamp_ms gps.timestamp_ms) { imuCachePush(imu_cache_.front()); imu_cache_.pop_front(); } // 用最近状态更新 ekf_.updateWithGps(gps); }这个缓冲区长度一般覆盖0.2到0.5秒就够了。时间戳必须用同一时钟源不能IMU用TICK计数、GPS用Unix时间戳否则硬件延迟会被误判为滤波发散。更精细的做法是估计GPS接收机内部处理延迟通常在20到100ms之间可以在驱动层加一个固定补偿值调试时用残差分析来验证补偿是否合理。5. 编译运行与精度验证Makefile、绘图校验与静止零偏校准技巧5.1 确认Makefile编译选项压缩包里自带Makefile可以直接用g编译。重点检查两处是否开启优化以及Eigen头文件路径是否指向正确位置。CXX g CXXFLAGS -O2 -stdc14 -I/usr/include/eigen3 OBJS main.o EKF.o GYRO.o ACCELEROMETER.o MAGNETOMETER.o GPS.o GPS_Filter.o Captor.o target: $(OBJS) $(CXX) -o ins_ekf $(OBJS) -lm第一次编译建议把-O2换成-O0逻辑跑通再换回来。浮点矩阵运算在-O2下性能差距明显但-O0更容易用gdb跟踪NaN和计算溢出。Eigen如果没装Ubuntu下用apt install libeigen3-dev就能解决。5.2 用绘图验证组合导航精度运行程序后输出轨迹文件可以用Python做一个快速校验。重点看三件事轨迹是否连续、GPS跳变点是否被EKF平滑、静止时位置漂移量是否收敛。import matplotlib.pyplot as plt import numpy as np gps np.loadtxt(gps_raw.csv, delimiter,, skiprows1) ekf np.loadtxt(ekf_out.csv, delimiter,, skiprows1) plt.figure(figsize(10, 8)) plt.plot(gps[:, 1], gps[:, 2], ., markersize2, labelRaw GPS) plt.plot(ekf[:, 1], ekf[:, 2], -, linewidth1.2, labelEKF fused) plt.axis(equal) plt.legend() plt.savefig(traj_compare.png, dpi150)两轴比例不一致会掩盖横纵向误差差异加axis equal强制等比例。静态场景下EKF轨迹应围绕GPS均值缓慢漂移漂移半径超过0.5米需要检查零偏估计是否收敛。5.3 静止零偏校准技巧EKF的零偏是逐步在线估计的但起始阶段如果陀螺零偏误差太大姿态会在前几秒内快速翻转。更可靠的做法是开机时利用静止检测做一次粗校准。判断逻辑可以通过加速度计模长来区分静止和运动bool isStatic(const ImuSample imu) { double norm sqrt(imu.ax * imu.ax imu.ay * imu.ay imu.az * imu.az); return norm 9.2 norm 10.0; // 静止时模长接近重力加速度 } void calibrateGyroBias(std::vectorImuSample samples) { Eigen::Vector3d sum Eigen::Vector3d::Zero(); for (auto s : samples) sum s.gyro; gyro_bias_ sum / samples.size(); }静止判断阈值要留足余量车辆怠速振动时加速度计模长会超过10.2容易误判。采集时间至少5秒陀螺零偏均值才会逼近真实值。粗校准后把state_.gyro_bias直接设为该值并把P矩阵中对应零偏的协方差设小EKF后续只需要做微小修正即可。把这段逻辑封装成开机自检函数每次上电自动执行。本文还有配套的精品资源点击获取
