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;
last_drift_constrain_time = local_time;
      • 速度融合
        • 速度融合函数FuseVelocity()，输入状态量为一步预测获得一步预测状态量和一步预测协方差，GPS速度量测，GPS速度的Gate（设为5），GPS速度量测的方差，输出为状态量，协方差，速度的新息，速度新息的协方差。
          • 初始化
            innovation = zeros(1,3);
varInnov = zeros(1,3);
H = zeros(3,24);
          • 新息innovation、雅克比矩阵、新息协方差varInnov的计算
            for obsIndex = 1:3
stateIndex = 4 + obsIndex;速度对应的状态索引（5:7）
            innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);速度量测新息
            H(obsIndex,stateIndex) = 1;观测雅克比矩阵
            varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS);
            速度新息的协方差
          • NED量测速度与门限值的比较判断
            (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0
          • 计算kalman增益、状态和协方差，循环1:3
            K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex);增益
            xk = K * innovation(obsIndex);
states = states - xk;状态
            P = P - K*H(obsIndex,:)*P;协方差
            P = 0.5*(P + transpose(P));保证协方差的对称性
            if P(i,i) < 0 P(i,i) = 0; end对角线负值置0
        • 速度融合数据记录
          output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
      • 位置融合FusePosition(),与速度融合相一致
    • 如果不满足条件，首先判断GPS是否5s数据都没有参与融合，如果是，则按照5Hz进行零位置修正。

---

• *气压计融合
  • 考虑延迟，计算最近的气压计数据ID
    latest_baro_index = find((baro_data.time_us-1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex),1, 'last')
  • 当前状态更新
    last_baro_index = latest_baro_index;
baro_fuse_index = baro_fuse_index + 1;
  • 融合高度计信息FuseBaroHeight(),与GPS速度融合相似，观测方差设为3，门限值设为5。
  • 融合后的数据输出
    output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time;
output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;

---

• 磁航向融合
  • 考虑数据延迟，计算最近的磁力计数据ID
    latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' )
  • 当前状态更新
    last_mag_index = latest_mag_index;
mag_fuse_index = mag_fuse_index + 1;
  • 三轴磁模量计算并输出
    output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:)));
  • 按照磁航向融合param.fusion.magFuseMethod == 2
    FuseMagHeading()输入为状态量，协方差，三轴磁力计数据，当地磁偏角转换弧度，Hdg门限值5，Hdg误差0.1745
    • 获取states四元数q0 = states(1); q1 = states(2); q2 = states(3); q3 = states(4);和磁力计数据magX = magData(1); magY = magData(2); magZ = magData(3);
    • 建立磁航向的量测矩阵H = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3);
    • 计算总方差和卡尔曼增益varInnov = (H*P*transpose(H) + R_MAG); Kfusion = (P*transpose(H))/varInnov;
    • 新息计算
      应用当前四元数计算捷联矩阵Tbn = Quat2Tbn(states(1:4));
      将磁力计输出转到地理系magMeasNED = Tbn*[magX;magY;magZ];
      应用地理系下的磁力计输出计算磁偏角
      predDec = atan2(magMeasNED(2),magMeasNED(1));
      计算信息：当地磁偏角与磁力计获得的相减
      innovation = predDec - measDec;
    • 新息判断和限制
      新息限制在-pi~+pi之间：
      if (innovation > pi) innovation = innovation - 2*pi; elseif (innovation < -pi) innovation = innovation + 2*pi; end
      新息和innovGate判断：
      if (innovation^2 / (innovGate^2 * varInnov)) > 1.0 innovation = NaN; varInnov = NaN; return; end
    • 状态更新
      states = states - Kfusion * innovation;
    • 四元数归一化
      quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
if (quatMag > 1e-12) states(1:4) = states(1:4) / quatMag; end
    • 计算协方差P
      P = P - Kfusion*H*P;
P = 0.5*(P + transpose(P));保证对称性
      for i=1:24 if P(i,i) < 0 P(i,i) = 0; end对角线为负则置0
  • 输出数据保存
    output.innovations.mag_time_lapsed(mag_fuse_index) = local_time;
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;

---

至此全部融合结束，输出时间更新，输出平均时间
output.dt = dtCovInt / covIndex;dtCovInt 为累积的时间，covIndex为累积的帧数