NED坐标系下EKF松组合导航系统设计与实现

1. 北东地坐标系下的EKF松组合导航系统设计

在惯性导航与卫星导航融合领域，采用NED（北东地）坐标系的15状态扩展卡尔曼滤波（EKF）方案已成为工业界主流选择。这种坐标系定义与人类地理认知天然契合——X轴指向真北，Y轴指向东，Z轴垂直向下指向地心。相比ECEF（地心地固坐标系）或机体坐标系，NED系在工程实现上具有三大显著优势：

1. 导航参数直观：直接输出纬度、经度、高度和北向/东向速度，无需复杂坐标转换
2. 重力场处理简便：重力矢量始终沿Z轴负方向，简化力学方程
3. 传感器误差建模直接：陀螺和加速度计零偏可在本地坐标系下建模

1.1 状态向量设计原理

15维状态向量的设计体现了对误差源的系统化建模思想。位置误差（δφ, δλ, δh）、速度误差（δv_N, δv_E, δv_D）和姿态误差（δθ, δγ, δψ）构成导航核心误差，而陀螺零偏（ε_x, ε_y, ε_z）和加速度计零偏（∇_x, ∇_y, ∇_z）则对应传感器系统误差。这种设计背后的深层考量是：

• 位置误差采用经纬度高差而非直角坐标，避免在小范围运动时出现数值稳定性问题
• 姿态误差使用小角度近似，确保线性化有效性（通常要求<5°）
• 惯性器件误差建模为一阶马尔可夫过程，更符合物理特性

实际工程中，状态变量初始化值需要根据传感器等级调整。以MEMS级IMU为例，典型初始化值为：

python复制# 中等精度MEMS IMU的初始误差设定
self.lat_err = 1e-4          # 纬度误差约10米
self.lon_err = 1e-4          # 经度误差约10米
self.alt_err = 1.0           # 高度误差1米
self.vn_err = 0.1            # 速度误差0.1m/s
self.ve_err = 0.1
self.vd_err = 0.1
self.roll_err = np.deg2rad(1) # 姿态误差1度
self.pitch_err = np.deg2rad(1)
self.yaw_err = np.deg2rad(5)  # 航向误差较大
self.gyro_bias = np.array([0.1, 0.1, 0.1])  # 陀螺零偏0.1deg/s
se