速度误差与姿态误差的关系

EKF INS/GPS松组合导航，15状态，地理系采用NED(北东地)

北东地坐标系下的惯性导航总是带着某种神秘感。当我们把IMU数据塞进算法时，那些跳动的数值就像在三维空间跳华尔兹。今天咱们聊聊怎么用15个状态的扩展卡尔曼滤波（EKF）把INS和GPS揉在一起——注意不是做面包，但确实需要精准的"发酵"比例。

先看状态向量这个容器要装什么货。位置误差三个（纬度、经度、高度）、速度误差三个（北东地）、姿态误差三个（滚转俯仰偏航）、陀螺零偏三个、加速度计零偏三个，刚好凑成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秒精度时，那种感觉就像看着走钢丝的人稳稳到达对岸——虽然背后是一堆微分方程在拼命平衡。