INS推算阶段

多传感器信息融合，介绍中图片为INS+ DVL组合程序，此外还有imu+ gps组合等其他程序

水下机器人刚启动DVL（多普勒计程仪）的时候，我盯着调试界面不断跳动的数据直挠头——INS（惯性导航系统）给出的位置轨迹像喝醉了酒似的到处飘。这时候才深刻理解为什么工程师们总说："单传感器导航，约等于闭着眼走钢丝"。

传感器融合最实在的玩法就是把不同特性的设备拧成一股绳。比如INS能提供高频姿态数据但误差会累积，DVL虽然只能测相对速度但精度稳如老狗。下面这段Python伪代码展示了如何用卡尔曼滤波把两者捆在一起：

class INS_DVL_Fusion: def __init__(self): self.x = np.zeros(6) # 位置+速度状态量 self.P = np.eye(6) # 协方差矩阵 self.Q = np.diag([0.1,0.1,0.01,0.05,0.05,0.01]) # 过程噪声 self.R_dvl = np.diag([0.3,0.3,0.1]) # DVL观测噪声 def predict(self, imu_data, dt): F = np.eye(6) F[0,3] = F[1,4] = F[2,5] = dt self.x = F.dot(self.x) + imu_data * dt self.P = F.dot(self.P).dot(F.T) + self.Q def update(self, dvl_velocity): # DVL校正阶段 H = np.zeros((3,6)) H[:,3:6] = np.eye(3) K = self.P.dot(H.T).dot(np.linalg.inv(H.dot(self.P).dot(H.T)+self.R_dvl)) self.x += K.dot(dvl_velocity - H.dot(self.x)) self.P = (np.eye(6)-K.dot(H)).dot(self.P)

代码里的Q矩阵调参绝对是门玄学——刚开始我按教科书设了个保守值，结果融合后的轨迹还是飘。后来发现IMU在动态环境下噪声特性会变化，改成动态调整Q矩阵对角线元素才稳住。

多传感器信息融合，介绍中图片为INS+ DVL组合程序，此外还有imu+ gps组合等其他程序

陆地上的玩法又不一样。无人机用的IMU+GPS组合就像个精明的商人：IMU每毫秒都在疯狂输出数据（但误差越来越大），GPS每秒才给个靠谱但延迟的位置报价。这时候得用松耦合融合：

// 简化的C++融合逻辑 void fuseIMUGPS(const IMUData& imu, const GPSData& gps) { static KalmanFilter kf; if(gps.available()) { // GPS有效时校正 MatrixXd H(3,6); H << 1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0; kf.update(gps.position, H); } // 持续用IMU预测 kf.predict(imu.accel, imu.gyro); }

注意这里的状态量设计暗藏心机——同时包含位置和速度，但更新时只用GPS的位置观测。实测发现这种半更新策略比全状态更新更适合处理GPS的跳变数据。

最近在折腾水下项目的同事还贡献了个骚操作：当DVL信号丢失时，用加速度计积分估算速度，同时根据前10秒的平均速度做趋势预测。虽然听着像在走钢丝，但实测30秒内定位误差能控制在2米内。

传感器融合最魔幻的地方在于，有时候1+1真的能大于2。就像上周在湖试时，INS突然抽风导致定位漂移了50米，但DVL的速度观测硬是通过卡尔曼增益把系统慢慢拽回了正确轨迹——这比任何数学证明都更有说服力。