《从零搭建无人船控制系统》系列第六篇

摘要本文介绍一种面向嵌入式平台的 6 状态扩展卡尔曼滤波器EKF实现方案通过「顺序卡尔曼」将 6×6 矩阵求逆退化为标量除法在 Cortex-M7 上节省约 50 倍计算量。文章从状态向量设计、Predict/Update 循环、R 自适应、F·P·Fᵀ 手写展开、INS 模式到工程踩坑完整覆盖算法原理与 C 代码优化。适合需要在资源受限 MCU 上实现实时状态估计的嵌入式开发者阅读。第六章6-State EKF — 不用矩阵求逆的卡尔曼滤波标准扩展卡尔曼需要 6×6 矩阵求逆。在 Cortex-M7 上意味着几千个 CPU 周期。但我们发现 H 矩阵“每次只观测一维”——矩阵求逆退化为标量除法。数学原理 C 代码全文无公式恐惧。1. 为什么需要 EKFGPS 只有 10 Hz但控制回路需要 50 Hz 的状态更新。GPS 还偶尔丢信号桥下、高楼旁。纯靠 GPS 导航船每 100 ms “瞎”一次——在 2 m/s 速度下等于 20 cm 的位置跳变。IMU 100 Hz 输出角速度和加速度可以通过“积分”推算位置。但 IMU 有偏置误差——gyro 偏置 0.1°/s积 10 秒就是 1°的航向偏差。EKF 做的事用 IMU 做高频预测用 GPS 做低频校正。取两者的长处。2. 6 个状态量选什么状态向量 x: x[0] pos_north (北向位置, m) x[1] pos_east (东向位置, m) x[2] speed (地速, m/s) x[3] heading (航向, deg) x[4] gyro_bias (角速度偏置, deg/s) ← 关键 x[5] accel_bias (加速度偏置, m/s²) 为什么选这 6 个 pos speed → 知道船在哪、走多快 heading → 知道船朝哪和船的朝向、COG 不一定一样 bias×2 → 在线估计 IMU 误差并自动修正6 个状态刚好够——再少位置速度航向3无法估计 IMU 偏置再多加横滚/俯仰/角加速度船用不上。3. 两步循环Predict Update每 20ms (IMU 更新, 50Hz): ① Predict: 用 gyro 和 accel 推状态向前一步 heading (gyro_z - gyro_bias) * dt speed (accel_x - accel_bias) * dt pos speed * dt (沿当前航向分解) 每 100ms (GPS 更新, 10Hz): ② Update: 用 GPS 测量值校正状态 GPS 给我 pos → 校正 x[0], x[1] GPS 给我 speed → 校正 x[2] GPS 给我 heading → 校正 x[3]4. 关键技巧顺序卡尔曼标准卡尔曼滤波的 update 步骤需要K P * Hᵀ * (H * P * Hᵀ R)⁻¹ ← 矩阵求逆6×6 矩阵求逆 → Cortex-M7 上几千个周期。但我们的 H 有特殊结构GPS 的每次观测只涉及状态的一小部分GPS 位置 → 只更新 pos_north 和 pos_east2 个状态GPS 航向 → 只更新 heading1 个状态GPS 速度 → 只更新 speed1 个状态顺序卡尔曼的核心把一个 6 维的联合更新拆成多个 1 维/2 维的独立更新顺序执行。每次只观测一维 → H 是 1×6 的向量 → HPHᵀ 是标量 → 矩阵求逆变标量除法。// 顺序卡尔曼: 位置更新 (2维联合)voidekf_update_position(floatgps_north,floatgps_east,floatR){// H [1, 0, 0, 0, 0, 0] (只观测北向位置)// [0, 1, 0, 0, 0, 0] (只观测东向位置)// 北向残差floaty_ngps_north-x[0];floatS_nP[0][0]R;// ← HPHᵀ 退化为标量floatK_nP[0][0]/S_n;// ← 除法替代矩阵求逆x[0]K_n*y_n;// 状态修正// 更新 P ... (省略协方差传播)// 东向残差 (同理)floaty_egps_east-x[1];floatS_eP[1][1]R;floatK_eP[1][1]/S_e;x[1]K_e*y_e;}// 同理: 航向更新 (1维)voidekf_update_heading(floatgps_heading,floatR){...}// 同理: 速度更新 (1维)voidekf_update_speed(floatgps_speed,floatR){...}数学效果6×6 矩阵求逆~3000 周期→ 4 次标量除法~60 周期省 50 倍。5. R 自适应RTK 信得重单点信得轻// R 测量噪声协方差 → 自适应if(fix_typeRTK_FIX)R_pos0.0001f;// 厘米级 → 高度信任elseif(fix_typeRTK_FLOAT)R_pos0.01f;// 分米级 → 中等信任elseR_pos4.0f;// 单点定位 → 不太信// 数值含义: R越小 → 卡尔曼增益越大 → 更信GPS → 跳得快// R越大 → 卡尔曼增益越小 → 更信IMU → 平滑但滞后效果RTK 厘米级定位时GPS 修正权重很大 → 船对 GPS 位置敏捷响应。GPS 掉到单点误差 3-5m时卡尔曼自动降低对 GPS 的信赖 → 更多依赖 IMU 预测 → 不会被错误定位拉偏。6. Predict 步骤F·P·Fᵀ 手写展开标准矩阵乘法 F·P·Fᵀ 是 6×6×6×6 1296 次乘加。但 F 矩阵非常稀疏——运动学模型只有对角和非对角几个元素非零F 矩阵非零元: pos_north ← pos_north speed*cos(heading)*dt (4个非零) pos_east ← pos_east speed*sin(heading)*dt (4个非零) speed ← speed accel*dt (3个非零) heading ← heading gyro*dt (3个非零) bias ← bias (对角, 2个)手写展开后只用 66 次乘加——比通用矩阵乘省 95%。在 50 Hz 的 Predict 步中每次省 ~1200 周期 × 50 Hz 60000 周期/秒 可忽略的 CPU 占用。7. INS 模式GPS 丢了我怎么硬撑// GPS丢了 → 只做 Predict, 不做 Updatewhile(gps_lost){ekf_predict(gyro_z,accel_x,dt);// 纯IMU推算// gyro_bias 在线估计 → 抑制航向漂移// 10秒超时 → EMERGENCY急停}没有 GPS 修正纯 IMU 推算位置会漂移。但 gyro_bias 的在线估计减缓了航向漂移船不转时 gyro 偏置会被学习。10 秒内 GPS 恢复 → 切回正常模式。10 秒超时 → 急停漂太多了不敢走了。8. 踩过的坑坑 1协方差矩阵忘初始化 → 一直发散P 矩阵初始值全设 0 → 卡尔曼以为状态完美已知 → 永远不信 GPS → 位置永远不更新。修复P 对角线设适当的初始不确定度pos10m², heading100deg²。坑 2航向 wrap 在 EKF 中的处理航向从 359° 更新到 1°修正量是 2° 而不是 -358°。EKF 里忘记 wrap卡尔曼增益算出了巨大的修正量 → 协方差爆炸。修复y gps_heading - x[3]之后加while (y 180) y - 360。坑 3速度观测的方向分解GPS 只给出地速标量但卡尔曼需要 vx 和 vy。我们沿当前融合航向分解vx speed * sin(heading), vy speed * cos(heading)。但如果航向未收敛刚启动分解的方向是错的后续位置更新跟着错。修复航向未收敛时 R_vel 放大——告诉卡尔曼“这次分解不太可信”。9. 下一章预告第七章L1 航点导航 — 船怎么跟着航线走PX4 同款 L1 前瞻制导算法。船怎么沿着折线路径走出平滑弧线拐角怎么自动减速GPS 经纬度怎么转成米制坐标本文是《从零搭建无人船控制系统》系列第六篇。项目地址[煜坤 YuKun]开源准备中。