APM中ekf3算法

memset(&P[0][0], 0, sizeof(P));
// define the initial angle uncertainty as variances for a rotation vector
Vector3F rot_vec_var;
rot_vec_var.x = rot_vec_var.y = rot_vec_var.z = sq(0.1f);
// reset the quaternion state covariances
CovariancePrediction(&rot_vec_var);
// velocities
P[4][4]   = sq(frontend->_gpsHorizVelNoise);
P[5][5]   = P[4][4];
P[6][6]   = sq(frontend->_gpsVertVelNoise);
// positions
P[7][7]   = sq(frontend->_gpsHorizPosNoise);
P[8][8]   = P[7][7];
P[9][9]   = sq(frontend->_baroAltNoise);
// gyro delta angle biases
P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
P[11][11] = P[10][10];
P[12][12] = P[10][10];
// delta velocity biases
P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
P[14][14] = P[13][13];
P[15][15] = P[13][13];
// earth magnetic field
P[16][16] = sq(frontend->_magNoise);
P[17][17] = P[16][16];
P[18][18] = P[16][16];
// body magnetic field
P[19][19] = sq(frontend->_magNoise);
P[20][20] = P[19][19];
P[21][21] = P[19][19];
// wind velocities
P[22][22] = 0.0f;
P[23][23]  = P[22][22];

bool NavEKF3::coreBetterScore(uint8_t new_core, uint8_t current_core) const
{
    const NavEKF3_core &oldCore = core[current_core];
    const NavEKF3_core &newCore = core[new_core];
    if (!newCore.healthy()) {
        // never consider a new core that isn't healthy
        return false;
    }
    if (newCore.have_aligned_tilt() != oldCore.have_aligned_tilt()) {
        // tilt alignment is most critical, if one is tilt aligned and
        // the other isn't then use the tilt aligned lane
        return newCore.have_aligned_tilt();
    }
    if (newCore.have_aligned_yaw() != oldCore.have_aligned_yaw()) {
        // yaw alignment is next most critical, if one is yaw aligned
        // and the other isn't then use the yaw aligned lane
        return newCore.have_aligned_yaw();
    }
    // if both cores are aligned then look at relative error scores
    return coreRelativeErrors[new_core] < coreRelativeErrors[current_core];
}

struct state_elements {
    QuaternionF quat;           // quaternion defining rotation from local NED earth frame to body frame 0..3
    Vector3F    velocity;       // velocity of IMU in local NED earth frame (m/sec) 4..6
    Vector3F    position;       // position of IMU in local NED earth frame (m)     7..9
    Vector3F    gyro_bias;      // body frame delta angle IMU bias vector (rad)     10..12
    Vector3F    accel_bias;     // body frame delta velocity IMU bias vector (m/sec) 13..15
    Vector3F    earth_magfield; // earth frame magnetic field vector (Gauss)         16..18
    Vector3F    body_magfield;  // body frame magnetic field vector (Gauss)          19..21
    Vector2F    wind_vel;       // horizontal North East wind velocity vector in local NED earth frame (m/sec) 22..23
};

const auto &gps = dal.gps();	// 先获取gps的数据对象, 储存当前的gps信息
selected_gps = gps.primary_sensor();	// 设置当前的gps索引
preferred_gps = selected_gps;
if (frontend->_affinity & EKF_AFFINITY_GPS) {	// 按位与判断gps是否符合affinity
    if (core_index < gps.num_sensors() ) {
        preferred_gps = core_index;
    }	// 判断是否有超过core_index个gps, 也就是当前的core是否有对应的gps
    if (gps.status(preferred_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D) {	// 检查当前的第code_index个gps也就是当前core对应的第n个gps(如果有的话)的数据是否满足gps的3D定位的标准, 如果满足的话就改用当前core_index的gps
        selected_gps = preferred_gps;
    }
}

dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;	// 先计算imu频率时使用一个低通滤波

    // 判断gps是否可用, pose source是否为gps
    if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::GPS)) {
	// 判断是否融合gps的pose或vel
        fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS);
        fusePosData = true;
#if EK3_FEATURE_EXTERNAL_NAV
        extNavUsedForPos = false;
#endif
	// 把gps的有延迟的观测数据复制出来
        if (fuseVelData) {
            velPosObs[0] = gpsDataDelayed.vel.x;
            velPosObs[1] = gpsDataDelayed.vel.y;
            velPosObs[2] = gpsDataDelayed.vel.z;
        }
        const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
        const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
        velPosObs[3] = posxy.x;
        velPosObs[4] = posxy.y;
#if EK3_FEATURE_EXTERNAL_NAV
    } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
        // 如果启用了视觉的话观测数据就换成视觉传来的数据
        extNavUsedForPos = true;
        fusePosData = true;
        velPosObs[3] = extNavDataDelayed.pos.x;
        velPosObs[4] = extNavDataDelayed.pos.y;
#endif // EK3_FEATURE_EXTERNAL_NAV
    }