EKF INS/GPS松组合导航15状态地理系采用NED(北东地)北东地坐标系下的惯性导航总是带着某种神秘感。当我们把IMU数据塞进算法时那些跳动的数值就像在三维空间跳华尔兹。今天咱们聊聊怎么用15个状态的扩展卡尔曼滤波EKF把INS和GPS揉在一起——注意不是做面包但确实需要精准的发酵比例。EKF INS/GPS松组合导航15状态地理系采用NED(北东地)先看状态向量这个容器要装什么货。位置误差三个纬度、经度、高度、速度误差三个北东地、姿态误差三个滚转俯仰偏航、陀螺零偏三个、加速度计零偏三个刚好凑成15维。代码里大概长这样class States: def __init__(self): self.lat_err 0.0 # 纬度误差 self.lon_err 0.0 self.alt_err 0.0 self.vn_err 0.0 # 北向速度误差 self.ve_err 0.0 self.vd_err 0.0 self.roll_err 0.0 # 滚转角误差 self.pitch_err 0.0 self.yaw_err 0.0 self.gyro_bias np.zeros(3) # 陀螺零偏 self.accel_bias np.zeros(3) # 加速度计零偏系统模型是EKF的重头戏。状态转移矩阵F像张蜘蛛网把各状态变量联系起来。特别要注意地理系下的速度微分方程科氏力和重力项的处理容易让人头大。这里给个简化版的状态转移片段def compute_F_matrix(phi, v_n, R_N, R_E): F np.zeros((15,15)) F[3:6,6:9] skew_matrix([0, 0, gravity]) # 重力投影项 # 位置误差与速度误差的关系 F[0,3] 1/(R_N h) F[1,4] 1/((R_E h)*cos(lat)) F[2,5] -1 # 惯性器件误差模型 F[9:12,9:12] -1/tau_gyro * np.eye(3) # 陀螺零偏一阶马尔可夫 F[12:15,12:15] -1/tau_accel * np.eye(3) return F处理GPS量测更新时有个小技巧当GPS信号丢失时直接把对应量测噪声调到极大值相当于让系统纯惯性推算。代码实现可以用numpy的掩码操作if gps_available: H np.zeros((6,15)) H[0:3,0:3] np.eye(3) # 位置观测 H[3:6,3:6] np.eye(3) # 速度观测 R diag([sigma_pos**2, sigma_vel**2]) else: H np.zeros((0,15)) # 空观测矩阵 R np.eye(0) # 空噪声矩阵实际调试时会遇到两个魔鬼细节1) 姿态误差的线性化范围别超过5度否则方向余弦矩阵展开会崩2) 地理系更新周期别超过1秒否则地球自转补偿项会累积误差。有个工程师曾经把更新周期设成10秒结果无人机在测试场画出了抽象派轨迹。最后说说协方差矩阵的初始化。别傻乎乎地用单位矩阵试试这样的经验值P np.diag([ 1e-4, 1e-4, 1e-2, # 位置误差 (rad)^2, m^2 0.1, 0.1, 0.1, # 速度误差 (m/s)^2 np.deg2rad(1)**2 * 3, # 姿态误差 1e-6, 1e-6, 1e-6, # 陀螺零偏 1e-5, 1e-5, 1e-5 # 加速度计零偏 ])当看到滤波器在GPS中断期间靠纯惯性推算还能保持30秒精度时那种感觉就像看着走钢丝的人稳稳到达对岸——虽然背后是一堆微分方程在拼命平衡。