ekf2 MATLAB离线版本详解（三）

融合部分

---

• GPS融合
  • 考虑GPS延迟情况，选取离IMU当前采样时刻距离最近的GPS的索引latest_gps_index
  • 判断GPS是否blocked
  • 初次使用GPS信息，判断速度方差是否小于gpsSpdErrLim（设为1），位置方差是否小于gpsPosErrLim（设为5）。使用GPS的水平位置、速度进行初始化初始化后，将gps_use_started 设为true，以后不再进行初始化。
    states(5:7) = gps_data.vel_ned(latest_gps_index,:);
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
gps_use_started = true;
last_drift_constrain_time = local_time;
  • 判断 gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index,满足则融合GPS数据。
    • GPS速度、位置融合，不仅对速度位置修正，其他相关均修正
      • GPS相关索引更新
        last_gps_index = latest_gps_index;
gps_fuse_index = gps_fuse_index + 1;
las