ArduPilot开源代码之NavEKF2_core

1. 源由

NavEKF2_core类EKF2的第二层抽象，对于无人机等系统中的导航任务至关重要，提供实时状态估计和传感器融合功能。

2. 框架设计

2.1 构造函数

• NavEKF2_core: 使用前端引用初始化核心EKF。

2.1.1 NavEKF2_core

没有具体的初始化流程，主要是成员变量赋值。

• frontend = _frontend
• dal = AP::dal()

NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
    frontend(_frontend),
    dal(AP::dal())
{
}

初始化流程

NavEKF2::InitialiseFilter
 └──> core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MEM_FAST);

2.2 设置和初始化

• setup_core: 使用特定索引配置核心后端。
• InitialiseFilterBootstrap: 在静止状态下使用加速度计和磁力计数据初始化状态。

2.2.1 setup_core

核心后端设置缓冲(ring buffer)大小。

bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
    imu_index = _imu_index;
    gyro_index_active = _imu_index;
    accel_index_active = _imu_index;
    core_index = _core_index;

    /*
      imu_buffer_length 需要处理在 100Hz 最大融合速率下的 260ms 延迟。
      非 IMU 数据以超过 100Hz 的速度输入时会被下采样。
      对于 50Hz 的主循环速率，我们需要较短的缓冲区。
     */
    if (dal.ins().get_loop_rate_hz() < 100) {
        imu_buffer_length = 13;
    } else {
        // 在 100 Hz 融合速率下最多 260 毫秒延迟
        imu_buffer_length = 26;
    }
    if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
        return false;
    }
    if(!storedMag.init(OBS_BUFFER_LENGTH)) {
        return false;
    }
    if(!storedBaro.init(OBS_BUFFER_LENGTH)) {
        return false;
    } 
    if(!storedTAS.init(OBS_BUFFER_LENGTH)) {
        return false;
    }
    if(!storedOF.init(FLOW_BUFFER_LENGTH)) {
        return false;
    }
    // 注意：使用双测距仪可能会加倍存储量
    if(!storedRange.init(2*OBS_BUFFER_LENGTH)) {
        return false;
    }
    // 注意：测距信标数据一次读取一个，并且可能以高速度到达
    if(!storedRangeBeacon.init(imu_buffer_length)) {
        return false;
    }
    if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
        return false;
    }
    if(!storedIMU.init(imu_buffer_length)) {
        return false;
    }
    if(!storedOutput.init(imu_buffer_length)) {
        return false;
    }
    if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
       return false;
    }

    if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
        // 检查是否有足够的内存来创建 EKF-GSF 对象
        if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 内存不足", (unsigned)imu_index);
            return false;
        }

        // 尝试实例化
        yawEstimator = NEW_NOTHROW EKFGSF_yaw();
        if (yawEstimator == nullptr) {
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 分配失败", (unsigned)imu_index);
            return false;
        }
    }
    
    return true;
}

2.2.2 InitialiseFilterBootstrap

滤波器初始化

// 从加速度计和磁力计数据初始化状态（如果存在）
// 该方法只能在车辆静止时使用
bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
    // 如果是飞机且没有GPS锁定，则不初始化
    if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
        dal.snprintf(prearm_fail_string,
                           sizeof(prearm_fail_string),
                           "EKF2 init failure: No GPS lock");
        statesInitialised = false;
        return false;
    }

    if (statesInitialised) {
        // 我们已经初始化，但在IMU缓冲区填满之前不返回true。这防止在滤波器启动期间IMU数据暂停的时间漏洞
        readIMUData();
        readMagData();
        readGpsData();
        readBaroData();
        return storedIMU.is_filled();
    }

    // 将重复使用的变量置零
    InitialiseVariables();

    const auto &ins = dal.ins();

    // 初始化IMU数据
    dtIMUavg = ins.get_loop_delta_t();
    readIMUData();
    storedIMU.reset_history(imuDataNew);
    imuDataDelayed = imuDataNew;

    // 加速度计测得的XYZ机体轴上的加速度向量（m/s^2）
    Vector3F initAccVec;

    // TODO 我们应该在多个周期内平均加速度读数
    initAccVec = ins.get_accel(accel_index_active).toftype();

    // 读取磁力计数据
    readMagData();

    // 规范化加速度向量
    ftype pitch=0, roll=0;
    if (initAccVec.length() > 0.001f) {
        initAccVec.normalize();

        // 计算初始俯仰角
        pitch = asinF(initAccVec.x);

        // 计算初始滚转角
        roll = atan2F(-initAccVec.y , -initAccVec.z);
    }

    // 计算初始滚转和俯仰方向
    stateStruct.quat.from_euler(roll, pitch, 0.0f);

    // 初始化动态状态
    stateStruct.velocity.zero();
    stateStruct.position.zero();
    stateStruct.angErr.zero();

    // 初始化静态过程模型状态
    stateStruct.gyro_bias.zero();
    stateStruct.gyro_scale.x = 1.0f;
    stateStruct.gyro_scale.y = 1.0f;
    stateStruct.gyro_scale.z = 1.0f;
    stateStruct.accel_zbias = 0.0f;
    stateStruct.wind_vel.zero();
    stateStruct.earth_magfield.zero();
    stateStruct.body_magfield.zero();

    // 读取GPS并设置位置和速度状态
    readGpsData();
    ResetVelocity();
    ResetPosition();

    // 读取气压计并设置高度状态
    readBaroData();
    ResetHeight();

    // 在NED导航框架中定义地球旋转向量
    calcEarthRateNED(earthRateNED, dal.get_home().lat);

    // 初始化协方差矩阵
    CovarianceInit();

    // 重置输出状态
    StoreOutputReset();

    // 现在状态已初始化，设置为true
    statesInitialised = true;

    // 重置不活动的偏差
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        inactiveBias[i].gyro_bias.zero();
        inactiveBias[i].accel_zbias = 0;
        inactiveBias[i].gyro_scale.x = 1;
        inactiveBias[i].gyro_scale.y = 1;
        inactiveBias[i].gyro_scale.z = 1;
    }

    // 我们最初返回false以等待IMU缓冲区填满
    return false;
}

2.3 更新和健康检查

• UpdateFilter: 在新IMU数据可用时更新滤波器状态。
• healthy: 检查滤波器的健康状况。
• errorScore: 提供滤波器健康状况的错误评分。

2.3.1 UpdateFilter

更新滤波器状态 - 每当有新的IMU数据时都应该调用。

void NavEKF2_core::UpdateFilter(bool predict)
{
    // 设置标志，指示滤波器前端已经允许开始新的状态预测周期
    startPredictEnabled = predict;

    // 如果状态尚未初始化，则不运行滤波器更新
    if (!statesInitialised) {
        return;
    }

    // 启动用于负载测量的定时器
#if ENABLE_EKF_TIMING
    void *istate = hal.scheduler->disable_interrupts_save();
    static uint32_t timing_start_us;
    timing_start_us = dal.micros();
#endif

    fill_scratch_variables();

    // TODO - 在飞行中重启的方法

    // 获取更新步骤的起始时间
    imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

    // 检查臂状态并执行所需的检查和模式更改
    controlFilterModes();

    // 读取IMU数据作为增量角度和速度
    readIMUData();

    // 如果缓冲区中有新的IMU数据，运行EKF方程以估计融合时间地平线
    if (runUpdates) {
        // 使用来自延迟时间地平线的IMU数据预测状态
        UpdateStrapdownEquationsNED();

        // 预测协方差增长
        CovariancePrediction();

        // 运行IMU预测步骤以获得GSF偏航估计器算法
        // 使用IMU和可选的真空气速数据。
        // 必须在SelectMagFusion()之前运行，以提供最新的偏航估计
        runYawEstimatorPrediction();

        // 使用磁力计数据更新状态
        SelectMagFusion();

        // 使用GPS和高度计数据更新状态
        SelectVelPosFusion();

        // 运行GPS速度修正步骤以获得GSF偏航估计器算法
        // 并使用偏航估计值重置主EKF偏航（如果请求）
        // 必须在SelectVelPosFusion()之后运行，以确保有最新的GPS数据
        runYawEstimatorCorrection();

#if AP_BEACON_ENABLED
        // 使用范围信标数据更新状态
        SelectRngBcnFusion();
#endif

        // 使用光流数据更新状态
        SelectFlowFusion();

        // 使用空气速度数据更新状态
        SelectTasFusion();

        // 使用侧滑约束假设更新状态（适用于前进飞行的车辆）
        SelectBetaFusion();

        // 更新滤波器状态
        updateFilterStatus();
    }

    // 从融合中将风输出转发到输出时间地平线
    calcOutputStates();

    // 停止用于负载测量的定时器
#if ENABLE_EKF_TIMING
    static uint32_t total_us;
    static uint32_t timing_counter;
    total_us += dal.micros() - timing_start_us;
    if (timing_counter++ == 4000) {
        DEV_PRINTF("ekf2 avg %.2f us\n", total_us / float(timing_counter));
        total_us = 0;
        timing_counter = 0;
    }
    hal.scheduler->restore_interrupts(istate);
#endif

    /*
      这是一个检查，用来应对车辆静止在地面时过于自信的状态。
      这种情况下，用户尝试解锁时会出现“陀螺仪仍在稳定”现象。
      在这种状态下，EKF无法恢复，所以我们进行硬重置，让它再试一次。
     */
    if (filterStatus.value != 0) {
        last_filter_ok_ms = dal.millis();
    }
    if (filterStatus.value == 0 &&
        last_filter_ok_ms != 0 &&
        dal.millis() - last_filter_ok_ms > 5000 &&
        !dal.get_armed()) {
        // 在健康状态之后不健康5秒，重置滤波器
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u 强制重置", (unsigned)imu_index);
        last_filter_ok_ms = 0;
        statesInitialised = false;
        InitialiseFilterBootstrap();
    }
}

2.3.2 healthy

检查基本的滤波器健康指标，并返回合并的健康状态

bool NavEKF2_core::healthy(void) const
{
    uint16_t faultInt;
    getFilterFaults(faultInt);
    if (faultInt > 0) {
        return false;
    }
    if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
        // 如果三个指标都大于1，则说明滤波器的状态
        // 非常不健康。
        return false;
    }
    // 在使用前给滤波器一个时间来稳定
    if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
        return false;
    }
    // 当处于地面且在静态模式下时，位置和高度的创新必须在限制范围内
    ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
    if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
        return false;
    }

    // 一切正常
    return true;
}

2.3.3 errorScore

返回一个合并的错误分数，数值越高表示错误越大；旨在供前端使用，以确定哪个是主要的EKF

ftype NavEKF2_core::errorScore() const
{
    ftype score = 0.0f;
    if (tiltAlignComplete && yawAlignComplete) {
        // 检查GPS融合性能
        score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
        // 检查高度计融合性能
        score = MAX(score, hgtTestRatio);
        // 检查姿态修正
        const ftype tiltErrThreshold = 0.05f;
        score = MAX(score, tiltErrFilt / tiltErrThreshold);
    }
    return score;
}

2.4 位置和速度

• getPosNE, getPosD: 获取东北和下方向位置。
• getVelNED: 提供NED速度。

2.4.1 getPosNE

获取NE坐标系下机体相对于起始点的二维矢量。

bool NavEKF2_core::getPosNE(Vector2f &posNE) const
{
    // 有三种操作模式：绝对位置（GPS 融合），相对位置（光学流融合）和恒定位置（没有位置估计可用）
    if (PV_AidingMode != AID_NONE) {
        // 这是正常的操作模式，我们可以使用 EKF 位置状态
        // 校正 IMU 偏移（EKF 计算在 IMU 处）
        posNE.x = outputDataNew.position.x + posOffsetNED.x;
        posNE.y = outputDataNew.position.y + posOffsetNED.y;
        return true;

    } else {
        // 在恒定位置模式下，EKF 位置状态在原点，因此我们不能将它们用作位置估计
        if(validOrigin) {
            if ((dal.gps().status(dal.gps().primary_sensor()) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
                // 如果原点已设置且我们有 GPS，那么返回相对于原点的 GPS 位置
                const Location &gpsloc = dal.gps().location();
                const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc);
                posNE.x = tempPosNE.x;
                posNE.y = tempPosNE.y;
                return false;
            } else if (rngBcnAlignmentStarted) {
                // 如果我们正在使用范围信标数据进行对准，则报告位置
                posNE.x = receiverPos.x;
                posNE.y = receiverPos.y;
                return false;
            } else {
                // 如果没有 GPS 固定，我们只能提供最后已知的位置
                posNE.x = outputDataNew.position.x;
                posNE.y = outputDataNew.position.y;
                return false;
            }
        } else {
            // 如果原点尚未设置，则我们无法提供相对位置
            posNE.x = 0.0f;
            posNE.y = 0.0f;
            return false;
        }
    }
    return false;
}

2.4.2 getPosD

获取相对于起始点的高度距离（单位：米）。

bool NavEKF2_core::getPosD(float &posD) const
{
    // 无论操作模式如何，EKF总是有一个高度估计值
    // 纠正IMU框架中的偏移（EKF计算是在IMU上进行的）
    // 同时纠正原点高度的变化
    if ((frontend->_originHgtMode & (1<<2)) == 0) {
        // 任何相对于WGS-84参考的传感器高度漂移校正都应用于原点。
        posD = outputDataNew.position.z + posOffsetNED.z;
    } else {
        // 原点高度是静态的，校正应用于局部垂直位置
        // 使得getLLH()返回的高度 = getOriginLLH返回的高度 - posD
        posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
    }

    // 返回当前的高度解状态
    return filterStatus.flags.vert_pos;
}

2.4.3 getVelNED

获取身体坐标系原点的NED速度矢量，单位为米每秒（m/s）

void NavEKF2_core::getVelNED(Vector3f &vel) const
{
    // 校正IMU位置偏移（EKF计算是在IMU位置进行的）
    vel = (outputDataNew.velocity + velOffsetNED).tofloat();
}

2.5 传感器和偏差数据

• getAirSpdVec: 返回空速向量。
• getGyroBias, getGyroScaleErrorPercentage: 提供陀螺仪偏差和比例误差。
• resetGyroBias: 重置陀螺仪偏差。

2.5.1 getAirSpdVec

获取机体坐标系中的真实空速矢量（单位：米每秒）

bool NavEKF2_core::getAirSpdVec(Vector3f &vel) const
{
    if (PV_AidingMode == AID_NONE) {
        return false;
    }
    vel = (outputDataNew.velocity + velOffsetNED).tofloat();
    if (!inhibitWindStates) {
        vel.x -= stateStruct.wind_vel.x;
        vel.y -= stateStruct.wind_vel.y;
    }
    Matrix3f Tnb; // 从导航坐标系到机体坐标系的旋转矩阵
    outputDataNew.quat.inverse().rotation_matrix(Tnb);
    vel = Tnb * vel;
    return true;
}

2.5.2 getGyroBias

获取身体坐标系轴陀螺仪偏差估计值，单位为弧度/秒

void NavEKF2_core::getGyroBias(Vector3f &gyroBias) const
{
    if (dtEkfAvg < 1e-6f) {
        gyroBias.zero();
        return;
    }
    gyroBias = stateStruct.gyro_bias.tofloat() / dtEkfAvg;
}

2.5.3 getGyroScaleErrorPercentage

获取陀机体坐标系轴陀螺仪比例因子误差的百分比

void NavEKF2_core::getGyroScaleErrorPercentage(Vector3f &gyroScale) const
{
    if (!statesInitialised) {
        gyroScale.x = gyroScale.y = gyroScale.z = 0;
        return;
    }
    gyroScale.x = 100.0f/stateStruct.gyro_scale.x - 100.0f;
    gyroScale.y = 100.0f/stateStruct.gyro_scale.y - 100.0f;
    gyroScale.z = 100.0f/stateStruct.gyro_scale.z - 100.0f;
}

2.5.4 resetGyroBias

重置陀螺仪偏差

// 将机体轴陀螺偏差状态重置为零，并重新初始化相应的协方差
// 假设校准的精度为0.5度/秒，这将需要在静态条件下进行平均
// 警告 - 必须使用非阻塞的校准方法
void NavEKF2_core::resetGyroBias(void)
{
    stateStruct.gyro_bias.zero();
    zeroRows(P,9,11);
    zeroCols(P,9,11);

    P[9][9] = sq(radians(0.5 * dtIMUavg));
    P[10][10] = P[9][9];
    P[11][11] = P[9][9];
}

2.6 高度和地形

• resetHeightDatum: 重置气压高度。
• getHAGL: 返回相对于地面的高度。
• setTerrainHgtStable: 设置地形稳定性用于高度参考。

2.6.1 resetHeightDatum

重置气压高度

// 将EKF高度基准设为零
// 如果已经执行了高度基准重置，则返回true
bool NavEKF2_core::resetHeightDatum(void)
{
    if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
        // 仅当在地面上时允许重置。
        // 如果使用测距仪来测量高度，则永远不要执行
        // 高度基准重置
        return false;
    }
    // 记录旧的高度估计值
    ftype oldHgt = -stateStruct.position.z;
    // 重置气压计，使其在当前高度读数为零
    dal.baro().update_calibration();
    // 重置高度状态
    stateStruct.position.z = 0.0f;
    // 调整EKF原点的高度，以使原点加上重置前后的气压计高度相同

    if (validOrigin) {
        if (!gpsGoodToAlign) {
            // 如果没有GPS锁定，那么不应该执行
            // resetHeightDatum，但如果有GPS锁定，则最佳选择是
            // 保持旧的误差
            EKF_origin.alt += (int32_t)(100.0f * oldHgt);
        } else {
            // 如果有良好的GPS锁定，则重置为GPS
            // 高度。这确保从getLLH()获取的AMSL高度等于GPS高度，
            // 同时也确保相对高度为零
            EKF_origin.alt = dal.gps().location().alt;
        }
        ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
    }

    // 将地形状态设为零（在地面上）。帧高度的调整将会在后续约束中添加
    terrainState = 0;
    return true;
}

2.6.2 getHAGL

获取集体坐标系下相对于地面的高度

bool NavEKF2_core::getHAGL(float &HAGL) const
{
    HAGL = terrainState - outputDataNew.position.z - posOffsetNED.z;
    // 如果我们知道地形偏移和高度，那么我们就有一个有效的地面高度估计
    return !hgtTimeout && gndOffsetValid && healthy();
}

2.6.3 setTerrainHgtStable

设置地形稳定性用于高度参考

// 如果下面的地形足够稳定，可以作为高度参考，与距离传感器一起使用，则设置为 true。
// 如果飞行器下面的地形不能作为高度参考，则设置为 false。用于防止距离传感器的操作
// 否则会被 EK2_RNG_AID_HGT 和 EK2_RNG_USE_SPD 参数的组合启用。
void NavEKF2_core::setTerrainHgtStable(bool val)
{
    terrainHgtStable = val;
}

2.7 方向和旋转

• getEulerAngles, getQuaternion: 返回欧拉角和四元数。
• getRotationBodyToNED: 提供变换矩阵。

2.7.1 getEulerAngles

获取欧拉角

// return the Euler roll, pitch and yaw angle in radians
void NavEKF2_core::getEulerAngles(Vector3f &euler) const
{
    outputDataNew.quat.to_euler(euler.x, euler.y, euler.z);
    euler = euler - dal.get_trim();
}

2.7.2 getQuaternion

获取四元数

// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2_core::getQuaternion(Quaternion& ret) const
{
    ret = outputDataNew.quat.tofloat();
}

2.7.3 getRotationBodyToNED

获取机体坐标系与NED坐标系的变换矩阵

// return the transformation matrix from XYZ (body) to NED axes
void NavEKF2_core::getRotationBodyToNED(Matrix3f &mat) const
{
    outputDataNew.quat.rotation_matrix(mat);
    mat = mat * dal.get_rotation_vehicle_body_to_autopilot_body();
}

2.8 磁力计和风数据

• getMagNED, getMagXYZ: 提供磁场估计。
• getWind: 返回NED风速估计。
• getMagOffsets: 提供磁力计偏移。

2.8.1 getMagNED

获取NED坐标系下磁场矢量

// return earth magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagNED(Vector3f &magNED) const
{
    magNED = (stateStruct.earth_magfield * 1000.0).tofloat();
}

2.8.2 getMagXYZ

获取机体坐标系下磁场估计

// return body magnetic field estimates in measurement units / 1000
void NavEKF2_core::getMagXYZ(Vector3f &magXYZ) const
{
    magXYZ = (stateStruct.body_magfield*1000.0).tofloat();
}

2.8.3 getWind

获取NED坐标系下风速估计

// return the NED wind speed estimates in m/s (positive is air moving in the direction of the axis)
void NavEKF2_core::getWind(Vector3f &wind) const
{
    wind.x = stateStruct.wind_vel.x;
    wind.y = stateStruct.wind_vel.y;
    wind.z = 0.0f; // currently don't estimate this
}

2.8.4 getMagOffsets

获取NED坐标系磁力计偏移

// 返回磁力计偏移值
// 如果偏移值有效则返回 true
bool NavEKF2_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
    const auto &compass = dal.compass();
    if (!compass.available()) {
        return false;
    }

    // 如果磁场初始化已完成，磁场学习没有被禁止，
    // 主磁力计有效且状态方差已收敛，则磁力计偏移值有效
    const ftype maxMagVar = 5E-6f;
    bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
    if ((mag_idx == magSelectIndex) &&
            finalInflightMagInit &&
            !inhibitMagStates &&
            compass.healthy(magSelectIndex) &&
            variancesConverged) {
        magOffsets = compass.get_offsets(magSelectIndex) - (stateStruct.body_magfield*1000.0).tofloat();
        return true;
    } else {
        magOffsets = compass.get_offsets(magSelectIndex);
        return false;
    }
}

2.9 创新和方差

• getInnovations, getVariances: 返回测量创新和方差。

2.9.1 getInnovations

获取创新值

// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements
bool NavEKF2_core::getInnovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
{
    velInnov.x = innovVelPos[0];
    velInnov.y = innovVelPos[1];
    velInnov.z = innovVelPos[2];
    posInnov.x = innovVelPos[3];
    posInnov.y = innovVelPos[4];
    posInnov.z = innovVelPos[5];
    magInnov.x = 1e3f*innovMag[0]; // Convert back to sensor units
    magInnov.y = 1e3f*innovMag[1]; // Convert back to sensor units
    magInnov.z = 1e3f*innovMag[2]; // Convert back to sensor units
    tasInnov   = innovVtas;
    yawInnov   = innovYaw;

    return true;
}

2.9.2 getVariances

获取方差值

// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
// this indicates the amount of margin available when tuning the various error traps
// also return the delta in position due to the last position reset
bool NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
    velVar   = sqrtF(velTestRatio);
    posVar   = sqrtF(posTestRatio);
    hgtVar   = sqrtF(hgtTestRatio);
    // If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
    magVar.x = sqrtF(MAX(magTestRatio.x,yawTestRatio));
    magVar.y = sqrtF(MAX(magTestRatio.y,yawTestRatio));
    magVar.z = sqrtF(MAX(magTestRatio.z,yawTestRatio));
    tasVar   = sqrtF(tasTestRatio);
    offset   = posResetNE.tofloat();

    return true;
}

2.10 外部导航

• writeExtNavData, writeExtNavVelData: 写入外部导航系统的位置和速度数据。
• isExtNavUsedForYaw: 检查外部导航数据是否用于偏航。

2.10.1 writeExtNavData

写入外部导航系统的位置数据

• 数据验证：检查输入数据是否为 NaN，避免无效数据影响后续处理。
• 更新频率限制：确保更新频率不超过传感器和滤波器的处理能力。
• 重置标记：根据重置时间设置是否需要重置位置。
• 时间戳修正：考虑延迟和滤波器更新时间，调整时间戳。
• 数据存储：将处理后的数据存储到缓存中。

void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
    // 防止出现 NaN 值
    if (pos.is_nan() || isnan(posErr) || quat.is_nan() || isnan(angErr)) {
        return;
    }

    // 限制更新频率，确保不超过传感器缓冲区和融合处理的最大允许频率
    // 在滤波器初始化之前，不要尝试写入缓冲区
    if ((timeStamp_ms - extNavMeasTime_ms) < 20) {
        return;
    } else {
        extNavMeasTime_ms = timeStamp_ms;
    }

    // 如果重置时间与上次记录的重置时间不同，标记为需要重置
    if (resetTime_ms != extNavLastPosResetTime_ms) {
        extNavDataNew.posReset = true;
        extNavLastPosResetTime_ms = resetTime_ms;
    } else {
        extNavDataNew.posReset = false;
    }

    // 更新外部导航数据
    extNavDataNew.pos = pos.toftype();
    extNavDataNew.quat = quat.toftype();
    extNavDataNew.posErr = posErr;
    extNavDataNew.angErr = angErr;
    
    // 修正时间戳，减去延迟和滤波器更新步骤的一半
    timeStamp_ms = timeStamp_ms - delay_ms;
    timeStamp_ms -= localFilterTimeStep_ms/2;
    
    // 确保时间延迟不会超过缓冲区中最老 IMU 数据的年龄
    timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
    extNavDataNew.time_ms = timeStamp_ms;

    // 将新的外部导航数据推送到缓存中
    storedExtNav.push(extNavDataNew);
}

2.10.2 writeExtNavVelData

写入外部导航系统的速度数据

void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
    // 保护，防止 NaN
    if (vel.is_nan() || isnan(err)) {
        return;
    }

    // 如果当前时间戳与上一次时间戳差小于 20 毫秒，则返回
    if ((timeStamp_ms - extNavVelMeasTime_ms) < 20) {
        return;
    }

    extNavVelMeasTime_ms = timeStamp_ms;
    useExtNavVel = true;
    extNavVelNew.vel = vel.toftype();
    extNavVelNew.err = err;
    timeStamp_ms = timeStamp_ms - delay_ms;
    // 修正由于滤波器更新率造成的平均采样延迟
    timeStamp_ms -= localFilterTimeStep_ms/2;
    // 防止时间延迟超过缓冲区中最旧 IMU 数据的年龄
    timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
    extNavVelNew.time_ms = timeStamp_ms;
    storedExtNavVel.push(extNavVelNew);
}

2.10.3 isExtNavUsedForYaw

当外部导航数据也被用作偏航观测时返回 true

// return true when external nav data is also being used as a yaw observation
bool NavEKF2_core::isExtNavUsedForYaw() const
{
    return extNavUsedForYaw;
}

2.11 故障和状态

• getFilterFaults: 返回滤波器故障状态。
• getFilterStatus, getFilterGpsStatus: 提供滤波器状态。

2.11.1 getFilterFaults

获取滤波器故障状态

/*
return the filter fault status as a bitmasked integer
 0 = quaternions are NaN
 1 = velocities are NaN
 2 = badly conditioned X magnetometer fusion
 3 = badly conditioned Y magnetometer fusion
 4 = badly conditioned Z magnetometer fusion
 5 = badly conditioned airspeed fusion
 6 = badly conditioned synthetic sideslip fusion
 7 = filter is not initialised
*/
void  NavEKF2_core::getFilterFaults(uint16_t &faults) const
{
    faults = (stateStruct.quat.is_nan()<<0 |
              stateStruct.velocity.is_nan()<<1 |
              faultStatus.bad_xmag<<2 |
              faultStatus.bad_ymag<<3 |
              faultStatus.bad_zmag<<4 |
              faultStatus.bad_airspeed<<5 |
              faultStatus.bad_sideslip<<6 |
              !statesInitialised<<7);
}

2.11.2 getFilterStatus

获取滤波器状态

/*
return filter timeout status as a bitmasked integer
 0 = position measurement timeout
 1 = velocity measurement timeout
 2 = height measurement timeout
 3 = magnetometer measurement timeout
 4 = true airspeed measurement timeout
 5 = unassigned
 6 = unassigned
 7 = unassigned
*/
// Return the navigation filter status message
void  NavEKF2_core::getFilterStatus(nav_filter_status &status) const
{
    status = filterStatus;
}

2.11.3 getFilterGpsStatus

获取GPS滤波器状态

void  NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
{
    // init return value
    faults.value = 0;

    // set individual flags
    faults.flags.bad_sAcc           = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient
    faults.flags.bad_hAcc           = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient
    faults.flags.bad_vAcc           = gpsCheckStatus.bad_vAcc; // reported vertical position accuracy is insufficient
    faults.flags.bad_yaw            = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use
    faults.flags.bad_sats           = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient
    faults.flags.bad_horiz_drift    = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static)
    faults.flags.bad_hdop           = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS
    faults.flags.bad_vert_vel       = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static)
    faults.flags.bad_fix            = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required
    faults.flags.bad_horiz_vel      = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static)
}

2.12 其他功能

• writeOptFlowMeas: 写入光流测量。
• send_status_report: 向地面控制站发送状态报告。
• EKFGSF_requestYawReset: 请求偏航重置。
• have_aligned_tilt, have_aligned_yaw: 检查对齐状态。

2.12.1 writeOptFlowMeas

写入原始光流测量数据

注：约定传感器绕某个轴的右手物理旋转会产生正的光流和陀螺仪速率。

// rawFlowQuality 是一个质量度量值，范围在 0 到 255 之间，255 为最佳质量
// rawFlowRates 是 X 和 Y 传感器轴上的光流速率，单位是弧度/秒
// rawGyroRates 是传感器内部陀螺仪测量的传感器旋转速率，单位是弧度/秒
// msecFlowMeas 是从传感器接收到光流数据的调度时间，单位是毫秒
// posOffset 是光流传感器在机体坐标系中的位置，单位是米
// heightOverride 是传感器离地面的固定高度，单位是米，当用于探测车时。如果不使用则为0
void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
    // 原始测量数据需要是光流速率，单位是弧度/秒，时间上平均自上次更新以来
    // PX4Flow 传感器输出的光流速率具有以下轴和符号约定：
    // 正的 X 速率是由绕 X 轴的正传感器旋转产生的
    // 正的 Y 速率是由绕 Y 轴的正传感器旋转产生的
    // 该滤波器使用的光流速率定义与传感器不同，传感器上正的光流速率是由绕该轴的负旋转产生的。例如，飞行器绕 X（滚转）轴的正旋转会产生负的 X 光流速率
    flowMeaTime_ms = imuSampleTime_ms;
    // 计算光流传感器陀螺仪速率的偏差误差，但要防止数据的尖峰
    // 重置累积的机体角度变化和时间
    // 如果没有足够的时间经过，就不要进行计算以获得可靠的机体速率测量
    if (delTimeOF > 0.01f) {
        flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
        flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
        delAngBodyOF.zero();
        delTimeOF = 0.0f;
    }
    // 根据定义，如果调用了此函数，则说明提供了光流测量数据，因此我们
    // 需要运行光流起飞检测
    detectOptFlowTakeoff();

    // 计算光流观测数据的中间采样时间的旋转矩阵
    stateStruct.quat.rotation_matrix(Tbn_flow);
    // 不使用质量指示符低或速率极端的数据（有助于捕捉损坏的传感器数据）
    if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
        // 修正光流传感器的机体速率偏差并写入
        ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
        ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
        // 传感器接口没有提供 Z 轴速率，因此使用导航传感器的速率
        if (delTimeOF > 0.001f) {
            // 首选使用与光流传感器相同采样周期平均的速率
            ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
        } else if (imuDataNew.delAngDT > 0.001f){
            // 第二选择是使用最新的 IMU 数据
            ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
        } else {
            // 第三选择是使用零
            ofDataNew.bodyRadXYZ.z = 0.0f;
        }
        // 写入未经修正的光流速率测量
        // 注意修正了不同轴和 PX4Flow 传感器使用的符号约定
        ofDataNew.flowRadXY = (-rawFlowRates).toftype(); // 原始（未运动补偿）光流角速率，单位是 X 轴（弧度/秒）
        // 写入光流传感器在机体坐标系中的位置
        ofDataNew.body_offset = posOffset.toftype();
        // 写入光流传感器的高度覆盖
        ofDataNew.heightOverride = heightOverride;
        // 写入修正了机体速率的光流速率测量
        ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
        ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
        // 记录最后接收到观测数据的时间，以便我们可以检测到其他地方数据丢失
        flowValidMeaTime_ms = imuSampleTime_ms;
        // 估算测量的采样时间
        ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
        // 修正因滤波器更新率导致的平均采样间隔延迟
        ofDataNew.time_ms -= localFilterTimeStep_ms/2;
        // 防止时间延迟超过缓冲区中最旧 IMU 数据的年龄
        ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
        // 将数据保存到缓冲区
        storedOF.push(ofDataNew);
        // 检查融合时间范围内的数据
        flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
    }
}

2.12.2 send_status_report

向地面控制站发送状态报告

void NavEKF2_core::send_status_report(GCS_MAVLINK &link) const
{
    // 准备标志位
    uint16_t flags = 0;
    if (filterStatus.flags.attitude) {
        flags |= EKF_ATTITUDE; // 如果姿态数据有效，设置姿态标志位
    }
    if (filterStatus.flags.horiz_vel) {
        flags |= EKF_VELOCITY_HORIZ; // 如果水平速度数据有效，设置水平速度标志位
    }
    if (filterStatus.flags.vert_vel) {
        flags |= EKF_VELOCITY_VERT; // 如果垂直速度数据有效，设置垂直速度标志位
    }
    if (filterStatus.flags.horiz_pos_rel) {
        flags |= EKF_POS_HORIZ_REL; // 如果相对水平位置数据有效，设置相对水平位置标志位
    }
    if (filterStatus.flags.horiz_pos_abs) {
        flags |= EKF_POS_HORIZ_ABS; // 如果绝对水平位置数据有效，设置绝对水平位置标志位
    }
    if (filterStatus.flags.vert_pos) {
        flags |= EKF_POS_VERT_ABS; // 如果绝对垂直位置数据有效，设置绝对垂直位置标志位
    }
    if (filterStatus.flags.terrain_alt) {
        flags |= EKF_POS_VERT_AGL; // 如果地形高度数据有效，设置地面高度标志位
    }
    if (filterStatus.flags.const_pos_mode) {
        flags |= EKF_CONST_POS_MODE; // 如果常数位置模式有效，设置常数位置模式标志位
    }
    if (filterStatus.flags.pred_horiz_pos_rel) {
        flags |= EKF_PRED_POS_HORIZ_REL; // 如果预测的相对水平位置数据有效，设置预测相对水平位置标志位
    }
    if (filterStatus.flags.pred_horiz_pos_abs) {
        flags |= EKF_PRED_POS_HORIZ_ABS; // 如果预测的绝对水平位置数据有效，设置预测绝对水平位置标志位
    }
    if (!filterStatus.flags.initalized) {
        flags |= EKF_UNINITIALIZED; // 如果EKF尚未初始化，设置未初始化标志位
    }

    // 获取方差
    float velVar = 0, posVar = 0, hgtVar = 0, tasVar = 0;
    Vector3f magVar;
    Vector2f offset;
    getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);

    const float mag_max = fmaxF(fmaxF(magVar.x,magVar.y),magVar.z); // 计算最大磁场方差

    // 仅在EKF需要数据进行主要高度估计或光流操作时报告距离传感器的归一化创新水平。
    // 如果距离传感器用于其他应用程序，防止GCS出现误报
    float temp;
    if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) {
        temp = sqrtF(auxRngTestRatio); // 计算归一化创新水平
    } else {
        temp = 0.0f;
    }

    // 发送消息
    mavlink_msg_ekf_status_report_send(link.get_chan(), flags, velVar, posVar, hgtVar, mag_max, temp, tasVar);
}

2.12.3 EKFGSF_requestYawReset

请求偏航重置

// 请求将偏航角重置为 GSF 估计值
// 如果请求无法处理，请求将在 YAW_RESET_TO_GSF_TIMEOUT_MS 后超时
void NavEKF2_core::EKFGSF_requestYawReset()
{
    EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
}

2.12.4 have_aligned_tilt

检查倾斜对齐

    // return true if we are tilt aligned
    bool have_aligned_tilt(void) const {
        return tiltAlignComplete;
    }

2.12.5 have_aligned_yaw

检查偏航对齐

    // return true if we are yaw aligned
    bool have_aligned_yaw(void) const {
        return yawAlignComplete;
    }

3. 重要例程

NavEKF2_core类EKF2的第二层抽象，主要是配合NavEKF2类进行设计。

NavEKF2::UpdateFilter
 └──> NavEKF2_core::UpdateFilter

3.1 readIMUData

/*
 *  读取 IMU 的增量角度和增量速度测量值，并降采样到 100Hz，以便存储在 EKF 使用的数据缓冲区中。
 *  如果 IMU 数据的到达速率低于 100Hz，则不执行降采样或升采样。
 *  降采样采用不会引入摇摆或颠簸误差的方法。
 */
void NavEKF2_core::readIMUData()
{
    const auto &ins = dal.ins();

    // 平均 IMU 采样率
    dtIMUavg = ins.get_loop_delta_t();

    // 使用指定的 IMU，或如果不可用则使用主 IMU
    uint8_t accel_active, gyro_active;

    if (ins.use_accel(imu_index)) {
        accel_active = imu_index;
    } else {
        accel_active = ins.get_first_usable_accel();
    }

    if (ins.use_gyro(imu_index)) {
        gyro_active = imu_index;
    } else {
        gyro_active = ins.get_first_usable_gyro();
    }

    if (gyro_active != gyro_index_active) {
        // 我们在运行时切换活动陀螺仪。复制我们从先前不活动陀螺仪中学习到的偏差。
        // 我们不重新初始化偏差不确定性，因为它应该与之前活动的陀螺仪具有相同的不确定性。
        stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
        gyro_index_active = gyro_active;

        // 使用我们之前在此 IMU 上使用的陀螺仪比例因子（如果有）。
        // 我们不重置方差，因为我们不希望切换后错误归因于陀螺仪比例因子。
        stateStruct.gyro_scale = inactiveBias[gyro_active].gyro_scale;
    }

    if (accel_active != accel_index_active) {
        // 切换到该 IMU 的学习加速度计偏差
        stateStruct.accel_zbias = inactiveBias[accel_active].accel_zbias;
        accel_index_active = accel_active;
    }

    // 更新不活跃偏差状态
    learnInactiveBiases();

    readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
    accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
    imuDataNew.accel_index = accel_index_active;

    // 从主陀螺仪或主陀螺仪获取增量角度数据
    readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
    imuDataNew.gyro_index = gyro_index_active;

    // 获取当前时间戳
    imuDataNew.time_ms = imuSampleTime_ms;

    // 使用最新的 IMU 索引进行降采样 IMU 数据。
    // 如果我们在样本之间切换 IMU，这不完全正确。
    imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
    imuDataDownSampledNew.accel_index = imuDataNew.accel_index;

    // 累积增量速度和角度数据的测量时间间隔
    imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
    imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;

    // 从以前到新的旋转四元数姿态并规范化。
    // 使用四元数的累积可以防止由于降采样引入的摇摆误差。
    imuQuatDownSampleNew.rotate(imuDataNew.delAng);
    imuQuatDownSampleNew.normalize();

    // 将最新的增量速度旋转到积累开始时的机身框架
    Matrix3F deltaRotMat;
    imuQuatDownSampleNew.rotation_matrix(deltaRotMat);

    // 将增量速度应用于增量速度累加器
    imuDataDownSampledNew.delVel += deltaRotMat * imuDataNew.delVel;

    // 跟踪自上次状态预测以来的 IMU 帧数
    framesSincePredict++;

    /*
     * 如果目标 EKF 时间步长已累积，并且前端允许开始新预测周期，
     * 则存储累积的 IMU 数据以供状态预测使用，如果超过两倍目标时间，则忽略前端权限。
     * 根据 IMU 数据的时间抖动调整目标 EKF 步长阈值。
     */
    if ((dtIMUavg * (float)framesSincePredict >= (EKF_TARGET_DT - (dtIMUavg * 0.5)) &&
         startPredictEnabled) || (dtIMUavg * (float)framesSincePredict >= 2.0f * EKF_TARGET_DT)) {

        // 将累积的四元数转换为等效增量角度
        imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);

        // 给数据打上时间戳
        imuDataDownSampledNew.time_ms = imuSampleTime_ms;

        // 将数据写入 FIFO IMU 缓冲区
        storedIMU.push_youngest_element(imuDataDownSampledNew);

        // 计算 EKF 的实际平均时间步长率
        ftype dtNow = constrain_ftype(0.5f * (imuDataDownSampledNew.delAngDT + imuDataDownSampledNew.delVelDT), 0.0f, 10.0f * EKF_TARGET_DT);
        dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;

        // 清零累积的 IMU 数据和四元数
        imuDataDownSampledNew.delAng.zero();
        imuDataDownSampledNew.delVel.zero();
        imuDataDownSampledNew.delAngDT = 0.0f;
        imuDataDownSampledNew.delVelDT = 0.0f;
        imuDataDownSampledNew.gyro_index = gyro_index_active;
        imuDataDownSampledNew.accel_index = accel_index_active;
        imuQuatDownSampleNew[0] = 1.0f;
        imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;

        // 重置计数器，以便前端知道自我们开始新更新周期以来经过了多少帧
        framesSincePredict = 0;

        // 设置标志，让过滤器知道它有新 IMU 数据并需要运行
        runUpdates = true;

        // 从 FIFO 缓冲区中提取最旧的可用数据
        imuDataDelayed = storedIMU.get_oldest_element();

        // 防止增量时间变为零
        // TODO - 检查计算是否可以容忍 0
        ftype minDT = 0.1f * dtEkfAvg;
        imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT, minDT);
        imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT, minDT);

        updateTimingStatistics();
            
        // 校正提取的 IMU 数据的传感器误差
        delAngCorrected = imuDataDelayed.delAng;
        delVelCorrected = imuDataDelayed.delVel;
        correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
        correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);

    } else {
        // 我们在缓冲区中没有新 IMU 数据，因此在这个时间步长上不运行滤波器更新
        runUpdates = false;
    }
}

3.2 readDeltaVelocity

// 从IMU读取速度增量和相应的时间间隔
// 如果数据不可用，则返回false
bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
    const auto &ins = dal.ins();

    if (ins_index < ins.get_accel_count()) {
        Vector3f dVelF;
        float dVel_dtF;
        ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
        dVel = dVelF.toftype();
        dVel_dt = dVel_dtF;
        dVel_dt = MAX(dVel_dt,1.0e-4f);
        dVel_dt = MIN(dVel_dt,1.0e-1f);
        return true;
    }
    return false;
}

3.3 readGpsData

// 检查新的有效 GPS 数据并在可用时更新存储的测量值
void NavEKF2_core::readGpsData()
{
    if (frontend->_fusionModeGPS == 3) {
        // 如果 GPS 使用被禁用，则不读取 GPS 数据
        return;
    }

    // 检查是否有新的 GPS 数据
    // 不接受速率超过 14Hz 的数据，以避免溢出 FIFO 缓冲区
    const auto &gps = dal.gps();
    if (gps.last_message_time_ms(gps.primary_sensor()) - lastTimeGpsReceived_ms > 70) {
        if (gps.status() >= AP_DAL_GPS::GPS_OK_FIX_3D) {
            // 报告 GPS 固定状态
            gpsCheckStatus.bad_fix = false;

            // 存储之前读取的固定时间
            const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;

            // 获取当前固定时间
            lastTimeGpsReceived_ms = gps.last_message_time_ms(gps.primary_sensor());

            // 估算 GPS 固定有效时间，考虑到 GPS 处理和其他延迟
            float gps_delay = 0.0;
            gps.get_lag(gps_delay); // 忽略返回值
            gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(1e3f * gps_delay);

            // 校正由于滤波器更新率导致的平均采样间隔延迟
            gpsDataNew.time_ms -= localFilterTimeStep_ms/2;

            // 防止时间延迟超过缓冲区中最旧的 IMU 数据的年龄
            gpsDataNew.time_ms = MAX(gpsDataNew.time_ms, imuDataDelayed.time_ms);

            // 获取用于位置信息的 GPS
            gpsDataNew.sensor_idx = gps.primary_sensor();

            // 从 GPS 读取 NED 速度
            gpsDataNew.vel = gps.velocity().toftype();

            // 使用 GPS 的速度和位置精度（如果可用），否则设为零。
            // 对原始精度数据应用 5 秒时间常数的衰减包络滤波器
            ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms), 0.0f, 1.0f);
            gpsSpdAccuracy *= (1.0f - alpha);
            float gpsSpdAccRaw;
            if (!gps.speed_accuracy(gpsSpdAccRaw)) {
                gpsSpdAccuracy = 0.0f;
            } else {
                gpsSpdAccuracy = MAX(gpsSpdAccuracy, gpsSpdAccRaw);
                gpsSpdAccuracy = MIN(gpsSpdAccuracy, 50.0f);
                gpsSpdAccuracy = MAX(gpsSpdAccuracy, frontend->_gpsHorizVelNoise);
            }
            gpsPosAccuracy *= (1.0f - alpha);
            float gpsPosAccRaw;
            if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
                gpsPosAccuracy = 0.0f;
            } else {
                gpsPosAccuracy = MAX(gpsPosAccuracy, gpsPosAccRaw);
                gpsPosAccuracy = MIN(gpsPosAccuracy, 100.0f);
                gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
            }
            gpsHgtAccuracy *= (1.0f - alpha);
            float gpsHgtAccRaw;
            if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
                gpsHgtAccuracy = 0.0f;
            } else {
                gpsHgtAccuracy = MAX(gpsHgtAccuracy, gpsHgtAccRaw);
                gpsHgtAccuracy = MIN(gpsHgtAccuracy, 100.0f);
                gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
            }

            // 检查是否有足够的 GPS 卫星，如果没有则增加 GPS 噪声比例
            if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.0f;
            } else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
                gpsNoiseScaler = 1.4f;
            } else { // <= 4 颗卫星或在恒定位置模式下
                gpsNoiseScaler = 2.0f;
            }

            // 检查 GPS 是否可以输出垂直速度，如果允许使用，则设置 GPS 融合模式
            if (gps.have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
                useGpsVertVel = true;
            } else {
                useGpsVertVel = false;
            }

            // 监控 GPS 速度数据的质量（对齐前后）。这会更新 GpsGoodToAlign 类变量
            calcGpsGoodToAlign();

            // 对齐后的检查
            calcGpsGoodForFlight();

            // 查看是否可以从前端获取起源
            if (!validOrigin && frontend->common_origin_valid) {
                if (!setOrigin(frontend->common_EKF_origin)) {
                    // 设定错误，因为尝试多次设置起源
                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                    return;
                }
            }

            // 读取 WGS-84 纬度、经度、高度坐标中的 GPS 位置
            const Location &gpsloc = gps.location();

            // 如果先前未设置 EKF 起源和磁场偏角，并且 GPS 检查通过
            if (gpsGoodToAlign && !validOrigin) {
                Location gpsloc_fieldelevation = gpsloc;
                // 如果飞行中，校正起飞高度变化，使起源位于地面高度
                if (inFlight) {
                    gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
                }

                if (!setOrigin(gpsloc_fieldelevation)) {
                    // 设定错误，因为尝试多次设置起源
                    INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                    return;
                }

                // 使用已发布的偏角设置 NE 地磁场状态
                // 并设置相应的方差和协方差
                alignMagStateDeclination();

                // 设置 NED 起源的高度
                ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;

                // 设置 GPS 起源高度的不确定性
                ekfOriginHgtVar = sq(gpsHgtAccuracy);
            }

            if (gpsGoodToAlign && !have_table_earth_field) {
                const auto &compass = dal.compass();
                if (compass.have_scale_factor(magSelectIndex) &&
                    compass.auto_declination_enabled()) {
                    table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
                    table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
                                                                                gpsloc.lng*1.0e-7));
                    have_table_earth_field = true;
                    if (frontend->_mag_ef_limit > 0) {
                        // 从表中初始化地球场
                        stateStruct.earth_magfield = table_earth_field_ga;
                    }
                }
            }
            
            // 将 GPS 测量值转换为本地 NED，并保存到缓冲区以供稍后融合（如果有有效的起源）
            if (validOrigin) {
                gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
                if ((frontend->_originHgtMode & (1<<2)) == 0) {
                    gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
                } else {
                    gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
                }
                storedGPS.push(gpsDataNew);
                // 声明 GPS 可用
                gpsNotAvailable = false;
            }

        } else {
            // 报告 GPS 固定状态
            gpsCheckStatus.bad_fix = true;
            dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
        }
    }
}

3.4 readDeltaAngle

// 从IMU读取角度变化和对应的时间间隔
// 如果数据不可用，则返回false
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt) {
    const auto &ins = dal.ins();

    if (ins_index < ins.get_gyro_count()) {
        Vector3f dAngF;
        float dAng_dtF;
        ins.get_delta_angle(ins_index, dAngF, dAng_dtF);
        dAng = dAngF.toftype();
        dAng_dt = dAng_dtF;
        dAng_dt = MAX(dAng_dt,1.0e-4f);
        dAng_dt = MIN(dAng_dt,1.0e-1f);
        return true;
    }
    return false;
}

3.5 readBaroData

// 检查是否有新的压力高度测量数据，如果有则更新存储的测量数据
void NavEKF2_core::readBaroData()
{
    // 检查气压测量值是否已更改，以确定是否有新测量值到达
    // 不接受超过14Hz的频率数据，以避免溢出FIFO缓冲区
    const auto &baro = dal.baro();
    if (baro.get_last_update() - lastBaroReceived_ms > 70) {

        baroDataNew.hgt = baro.get_altitude();

        // 如果处于起飞模式，高度测量值限制为不低于起飞时的测量值
        // 这可以防止因直升机下洗在初始上升过程中干扰EKF高度
        if (dal.get_takeoff_expected() && !assume_zero_sideslip()) {
            baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
        }

        // 用于检查新测量值的时间戳
        lastBaroReceived_ms = baro.get_last_update();

        // 估计测量高度时的时间，考虑延迟
        baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;

        // 校正由于过滤器更新率造成的平均采样间隔延迟
        baroDataNew.time_ms -= localFilterTimeStep_ms / 2;

        // 防止时间延迟超过缓冲区中最旧IMU数据的年龄
        baroDataNew.time_ms = MAX(baroDataNew.time_ms, imuDataDelayed.time_ms);

        // 保存气压测量值到缓冲区，以便稍后融合
        storedBaro.push(baroDataNew);
    }
}

3.6 readMagData

// 检查是否有新的磁力计数据，并在可用时更新存储的测量值
void NavEKF2_core::readMagData()
{
    const auto &compass = dal.compass();

    if (!compass.available()) {
        allMagSensorsFailed = true;
        return;        
    }

    // 如果我们是一个通过侧滑约束来辅助偏航估计的飞行器，并且我们上次的磁力计已超时，则在本次飞行中将磁力计声明为失效
    const uint8_t maxCount = compass.get_count();
    if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
        allMagSensorsFailed = true;
        return;
    }

    if (compass.learn_offsets_enabled()) {
        // 在学习偏移时保持所有磁状态重置
        InitialiseVariablesMag();
        wasLearningCompass_ms = imuSampleTime_ms;
    } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
        wasLearningCompass_ms = 0;
        // 在学习完成后1秒内强制新的偏航对齐。延迟是为了确保任何缓冲的磁样本被丢弃
        yawAlignComplete = false;
        InitialiseVariablesMag();
    }

    // 如果磁力计已超时（被拒绝过久），并且如果可用则找到另一个磁力计使用
    // 不要在地面上执行此操作，因为可能存在磁干扰，我们需要在起飞前知道是否有问题。不要在启动后30秒内执行此操作，因为偏航错误可能是由于大的偏航陀螺偏置造成的
    // 如果超时是由于传感器故障造成的，则不论是否在地面上，都声明超时
    if (maxCount > 1) {
        bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000;
        bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
        if (fusionTimeout || sensorTimeout) {
            tryChangeCompass();
        }
    }

    // 不要以超过14Hz的速度接受新的指南针数据（标称速率为10Hz），以防止处理器负载过高，因为磁力计融合是一个耗费资源的步骤，我们可能会使FIFO缓冲区溢出
    if (use_compass() &&
        compass.healthy(magSelectIndex) &&
        compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {

        // 检测到磁力计偏移参数的变化并重置状态
        Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
        bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
        if (changeDetected) {
            // 将学习到的磁力计偏差状态归零
            stateStruct.body_magfield.zero();
            // 清除测量缓冲区
            storedMag.reset();
            // 在下次协方差预测时重置机体磁方差。这是为了处理新偏移中的可能错误
            needMagBodyVarReset = true;
        }
        lastMagOffsets = nowMagOffsets;
        lastMagOffsetsValid = true;

        // 存储上次测量更新的时间
        lastMagUpdate_us = compass.last_update_usec(magSelectIndex);

        // 当前时间范围内的磁力计数据
        mag_elements magDataNew;

        // 磁力计测量的估计时间，考虑到延迟
        magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;

        // 校正由于滤波器更新速率导致的平均采样延迟
        magDataNew.time_ms -= localFilterTimeStep_ms/2;

        // 读取指南针数据并缩放以改善数值条件
        magDataNew.mag = compass.get_field(magSelectIndex).toftype() * 0.001f;

        // 检查磁力计之间的数据一致性
        consistentMagData = compass.consistent();

        // 将磁力计测量值保存到缓冲区以便稍后融合
        storedMag.push(magDataNew);

        // 记录读取指南针的时间，以检测指南针传感器故障
        lastMagRead_ms = imuSampleTime_ms;
    }
}

3.7 readAirSpdData

// 检查新的空速数据，如果可用则更新存储的测量值
void NavEKF2_core::readAirSpdData()
{
    // 如果空速读数有效、被用户设置为使用并且已更新，
    // 我们将进行新的读取，将其从等效空速 (EAS) 转换为真空速 (TAS)，
    // 并设置标志让其他函数知道有新的测量可用。
    const auto *aspeed = dal.airspeed();
    if (aspeed &&
        aspeed->use() &&
        aspeed->healthy() &&
        aspeed->last_update_ms() != timeTasReceived_ms) {
        tasDataNew.tas = aspeed->get_airspeed() * dal.get_EAS2TAS();
        timeTasReceived_ms = aspeed->last_update_ms();
        tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;

        // 纠正由于滤波器更新速率导致的平均采样延迟
        tasDataNew.time_ms -= localFilterTimeStep_ms/2;

        // 将数据保存到缓冲区中，当融合时间轴赶上它时进行融合
        storedTAS.push(tasDataNew);
    }
    // 检查缓冲区中已被融合时间轴超越并需要融合的测量值
    tasDataToFuse = storedTAS.recall(tasDataDelayed, imuDataDelayed.time_ms);
}

3.8 readRngBcnData

// 检查是否有新的范围信标数据，并将其推送到数据缓冲区（如果有的话）
void NavEKF2_core::readRngBcnData()
{
    // 获取信标数据的位置
    const auto *beacon = dal.beacon();

    // 如果没有信标对象，立即退出
    if (beacon == nullptr) {
        return;
    }

    // 获取正在使用的信标数量
    N_beacons = beacon->count();

    // 搜索所有信标以查找新数据，如果找到新数据，则停止搜索并将数据推送到观测缓冲区
    bool newDataToPush = false;
    uint8_t numRngBcnsChecked = 0;
    // 从上次检查的位置开始搜索
    uint8_t index = lastRngBcnChecked;
    while (!newDataToPush && numRngBcnsChecked < N_beacons) {
        // 跟踪已检查的信标数量
        numRngBcnsChecked++;

        // 移动到下一个信标，必要时环绕索引
        index++;
        if (index >= N_beacons) {
            index = 0;
        }

        // 检查信标是否健康且有新数据
        if (beacon->beacon_healthy(index) &&
                beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index])
        {
            // 设置时间戳，修正测量延迟和由于滤波器更新速率造成的平均采样间隔延迟
            lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
            rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;

            // 设置范围噪声
            // TODO：范围库应该为每个信标提供噪声/准确度估计
            rngBcnDataNew.rngErr = frontend->_rngBcnNoise;

            // 设置范围测量值
            rngBcnDataNew.rng = beacon->beacon_distance(index);

            // 设置信标位置
            rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();

            // 确定信标标识符
            rngBcnDataNew.beacon_ID = index;

            // 表示我们有新数据要推送到缓冲区
            newDataToPush = true;

            // 更新最后检查的索引
            lastRngBcnChecked = index;
        }
    }

    // 检查信标系统是否返回了3D定位
    Vector3f beaconVehiclePosNEDF;
    float beaconVehiclePosErrF;
    if (beacon->get_vehicle_position_ned(beaconVehiclePosNEDF, beaconVehiclePosErrF)) {
        rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
    }
    beaconVehiclePosNED = beaconVehiclePosNEDF.toftype();
    beaconVehiclePosErr = beaconVehiclePosErrF;


    // 检查范围信标数据是否可以用来对齐车辆位置
    if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
        // 检查信标报告的位置与3-State对准滤波器的位置之间的一致性
        ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
        ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
        if (posDiffSq < 9.0f*posDiffVar) {
            rngBcnGoodToAlign = true;
            // 如果之前未设置EKF原点和磁场偏角
            if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
                // 从信标系统获取原点
                Location origin_loc;
                if (beacon->get_origin(origin_loc)) {
                    setOriginLLH(origin_loc);

                    // 使用发布的磁偏角设置NE地球磁场状态
                    // 并设置相应的方差和协方差
                    alignMagStateDeclination();

                    // 设置原点高度的不确定性
                    ekfOriginHgtVar = sq(beaconVehiclePosErr);
                }
            }
        } else {
            rngBcnGoodToAlign = false;
        }
    } else {
        rngBcnGoodToAlign = false;
    }

    // 将数据保存到缓冲区，以便在融合时间范围赶上时进行融合
    if (newDataToPush) {
        storedRangeBeacon.push(rngBcnDataNew);
    }

    // 检查缓冲区中是否有被融合时间范围超越的测量数据，并需要进行融合
    rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
}

3.9 readRangeFinder

// 读取测距仪并在有新数据时进行新的测量
// 应用中值滤波器
void NavEKF2_core::readRangeFinder(void)
{
    uint8_t midIndex;
    uint8_t maxIndex;
    uint8_t minIndex;

    // 获取理论上的正确范围，当车辆在地面上时
    // 不允许范围低于5厘米，因为这可能会导致光流处理的问题
    const auto *_rng = dal.rangefinder();
    if (_rng == nullptr) {
        return;
    }

    rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);

    // 以20Hz读取测距仪
    // TODO 需要更好的方法来判断是否有新数据
    if (_rng && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {

        // 重置用于控制测量速率的计时器
        lastRngMeasTime_ms =  imuSampleTime_ms;

        // 如果有效，将样本和样本时间存储到环形缓冲区中
        // 如果可用，则使用来自两个测距仪的数据

        for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
            auto *sensor = _rng->get_backend(sensorIndex);
            if (sensor == nullptr) {
                continue;
            }
            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
                rngMeasIndex[sensorIndex] ++;
                if (rngMeasIndex[sensorIndex] > 2) {
                    rngMeasIndex[sensorIndex] = 0;
                }
                storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
                storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
            } else {
                continue;
            }

            // 检查是否有三个新样本
            bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
            for (uint8_t index = 0; index <= 2; index++) {
                sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
            }

            // 如果有三个新样本，则找到中值
            if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
                if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
                    minIndex = 1;
                    maxIndex = 0;
                } else {
                    minIndex = 0;
                    maxIndex = 1;
                }
                if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
                    midIndex = maxIndex;
                } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
                    midIndex = minIndex;
                } else {
                    midIndex = 2;
                }

                // 不允许时间回退
                if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
                    rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
                }

                // 限制测量范围不低于地面范围
                rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);

                // 获取当前传感器在机体框架中的位置
                rangeDataNew.sensor_idx = sensorIndex;

                // 将数据写入缓冲区，并带有时间戳，以便在融合时间范围赶上时进行融合
                storedRange.push(rangeDataNew);

                // 指示我们已更新测量值
                rngValidMeaTime_ms = imuSampleTime_ms;

            } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
                // 在起飞前，如果没有数据，我们假设使用地面范围值
                rangeDataNew.time_ms = imuSampleTime_ms;
                rangeDataNew.rng = rngOnGnd;

                // 将数据写入缓冲区，并带有时间戳，以便在融合时间范围赶上时进行融合
                storedRange.push(rangeDataNew);

                // 指示我们已更新测量值
                rngValidMeaTime_ms = imuSampleTime_ms;

            }
        }
    }
}

4. 总结

NavEKF2_core类含有很多滤波、数据融合等提高数据有效性的方法。

具体提高数据有效性的方法值得深入研究，后续作为单点进行介绍，如果对此感兴趣的朋友，也请评论留言，谢谢！

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
【7】ArduPilot开源代码之AP_DAL研读系列