如何在Expo移动应用中用EKF实现INS与GPS融合优化GPS路径？

一、核心概念纠正 #

你之前将所有传感器原始数据放入状态向量的思路有误。状态向量应定义为需要跟踪/估计的核心变量，而非传感器输入。针对行人步行场景，推荐使用如下5维状态向量：

X = [ 纬度(弧度), 经度(弧度), 北向速度(m/s), 东向速度(m/s), 航向角(弧度) ]^T

• 纬度/经度：最终要输出的优化值（转弧度便于计算）
• 北向/东向速度：适配惯性更新的行人运动分量
• 航向角：行人前进方向（由陀螺仪+磁力计校准得到）

传感器数据（加速度计、陀螺仪、磁力计）作为控制输入驱动状态预测；GPS数据作为观测值修正预测误差。

二、kalman-filter库参数配置 #

1. Dynamic（状态转移模型） #

对应系统运动规律，每200ms用INS数据执行预测步：

const EARTH_RADIUS = 6378137; // 地球半径（米）

const kFilter = new KalmanFilter({
  observation: { /* 下文定义 */ },
  dynamic: {
    // 状态向量维度：5
    dimension: 5,
    // 状态转移函数：基于时间间隔Δt和INS输入更新状态
    transition: (opts) => {
      const { state, input, dt } = opts;
      const [lat, lon, vN, vE, heading] = state;
      // input为预处理后的INS数据：北向加速度aN、东向加速度aE、航向角变化率dHeading
      const [aN, aE, dHeading] = input;

      // 计算更新后的状态
      const newLat = lat + (vN * dt) / EARTH_RADIUS;
      const newLon = lon + (vE * dt) / (EARTH_RADIUS * Math.cos(lat));
      const newVN = vN + aN * dt;
      const newVE = vE + aE * dt;
      const newHeading = heading + dHeading * dt;

      return [newLat, newLon, newVN, newVE, newHeading];
    },
    // 过程噪声协方差矩阵（5x5）：数值越大代表对模型不确定性容忍度越高
    covariance: () => {
      return [
        [1e-12, 0, 0, 0, 0],    // 纬度噪声
        [0, 1e-12, 0, 0, 0],    // 经度噪声
        [0, 0, 1e-4, 0, 0],     // 北向速度噪声（加速度计误差）
        [0, 0, 0, 1e-4, 0],     // 东向速度噪声
        [0, 0, 0, 0, 1e-6]      // 航向角噪声（陀螺仪误差）
      ];
    }
  }
});

2. Observation（观测模型） #

对应GPS数据修正逻辑，每1秒执行更新步：

observation: {
  // 观测值维度：3（GPS返回的纬度、经度、速度）
  dimension: 3,
  // 状态投影函数：将状态向量映射到观测空间
  stateProjection: (opts) => {
    const { state } = opts;
    const [lat, lon, vN, vE] = state;
    const speed = Math.sqrt(vN**2 + vE**2); // 计算合速度匹配GPS输出
    return [lat, lon, speed];
  },
  // 观测噪声协方差矩阵（3x3）：基于GPS精度设置
  covariance: () => {
    const gpsPosNoise = (5 / EARTH_RADIUS)**2; // 5米误差转弧度
    const gpsSpeedNoise = 0.5**2; // 0.5m/s速度误差
    return [
      [gpsPosNoise, 0, 0],
      [0, gpsPosNoise, 0],
      [0, 0, gpsSpeedNoise]
    ];
  }
}

三、预处理与数据同步流程 #

1. 传感器数据预处理 #

• 加速度计：将手机坐标系的x/y/z加速度转换为东北天（ENU）坐标系的北向/东向分量，同时减去重力影响（需结合当前航向角和手机姿态计算）。
• 陀螺仪：取z轴角速度作为航向角变化率（若手机姿态波动大，需结合加速度计/磁力计做姿态解算）。
• 磁力计：校准航向角初始值，或在陀螺仪漂移时修正航向（可先用互补滤波得到稳定航向）。

2. 多频率数据同步 #

• INS数据（200ms触发）：调用kFilter.predict({ input: [aN, aE, dHeading], dt: 0.2 })，传入预处理后的输入和时间间隔。
• GPS数据（1秒触发）：将GPS经纬度转为弧度后，调用kFilter.update({ observation: [gpsLatRad, gpsLonRad, gpsSpeed] })。

3. 输出优化结果 #

从滤波后状态中提取经纬度并转换为度数：

const optimizedState = kFilter.getState();
const optimizedLat = optimizedState[0] * (180 / Math.PI);
const optimizedLon = optimizedState[1] * (180 / Math.PI);

四、额外注意事项 #

• 初始状态：用首次GPS数据初始化状态向量，速度设为GPS返回值，航向角用磁力计校准值。
• 参数调优：根据设备传感器精度、环境（如城市峡谷）调整协方差矩阵数值，找到最优平滑效果。
• 海拔：若无需高度信息，可忽略，状态向量也无需包含海拔。

内容的提问来源于stack exchange，提问作者rablentain