ArduPilot开源代码之EKF3

1. 源由

EKF3是在代码AP_NavEKF3中实现。

为此，研究下相关代码实现，其算法流程应该与《ArduPilot开源代码之EKF1》类似。

2. EKF3简介

AP_NavEKF3实例化了多个称为“lanes”的滤波器实例。主要的lane提供状态估计，其余的在后台更新并可供切换。

• 可用的lane数量与启用的IMU数量完全相同。
• 按照传统，每个lane使用空速计、气压计、GPS和磁力计传感器的主要实例。
• 主要传感器可以通过修改的参数进行设置，故障情况下，系统甚至可以在飞行中稍后进行更改。
• 通过lane上所有传感器的误差分数计算，选择具有性能最佳的传感器组合的lane。

3. 举例

设备：

• 2 baros
• 2 GPS
• 2 airspeeds
• 3 mags
• 3 IMUs

此时，可以有3个lane。

LANE	1	2	3
ASD	1	2	1
BAR	1	2	1
GPS	1	2	1
MAG	1	2	3

如果GPS1故障或者信号比GPS2更差，此时系统将切换到lane#2上。相当于有一个备份系统同时工作者，当出现异常时，将会直接接管，有了更好的冗余性，从而更加安全。

4. 配置

Affinity 仅适用于 EKF3，因此请确保 EK3_ENABLE 设置为 “1”，并且 AHRS_EKF_TYPE 设置为 “3” 以确保使用它。

• EK3_AFFINITY 参数是一个位掩码，可以选择要启用 affinity 的传感器。未启用的传感器将遵循默认的主传感器分配。

• EK3_ERR_THRESH 参数控制lane切换的灵敏度。lane错误会随着时间的推移相对于活动的主lane累积。该阈值控制考虑非主lane性能更好的错误差异大小。降低此参数会使lane切换对较小的“相对”错误更敏感，实际应用中会看到更积极的lane切换，反之亦然。

注：错误配置 EK3_ERR_THRESH 参数可能会对lane切换机制产生不利影响，并可能导致严重后果，甚至导致飞机丢失。请谨慎调试。

5. 讨论

当然这里也引出了一个问题，就是这个lane上的传感器是如何配置的？

• How to configure sensors for each lanes?

6. lane切换

AP_HAL_MAIN
 └──> loop
     └──> AP_AHRS::update
         └──> AP_AHRS::update_EKF3
             └──> NavEKF3::UpdateFilter

/* 
  更新滤波器状态 - 每当有新的IMU数据可用时应调用此函数
  执行速度受 SCHED_LOOP_RATE 控制
*/
void NavEKF3::UpdateFilter(void)
{
    AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF3);  // 开始一个更新滤波器的数据帧

    if (!core) {
        return;
    }

    imuSampleTime_us = AP::dal().micros64();  // 记录IMU采样时间戳

    for (uint8_t i=0; i<num_cores; i++) {
        // 如果超过3个IMU帧的时间没有超过，且已经使用了超过此循环的1/3 CPU预算，则抑制预测步骤。
        // 这允许多个EKF实例在调度上进行合作
        bool allow_state_prediction = true;
        if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
            AP::dal().ekf_low_time_remaining(AP_DAL::EKFType::EKF3, i)) {
            allow_state_prediction = false;
        }
        core[i].UpdateFilter(allow_state_prediction);  // 更新每个核心的滤波器
    }

    // 如果当前核心选择具有较高的错误分数或不健康，则切换到最低错误分数的健康核心
    // 确保主要核心至少在过去的10秒内处于健康状态，以避免由于初始对齐波动和竞争条件而进行切换
    if (!runCoreSelection) {
        static uint64_t lastUnhealthyTime_us = 0;
        if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
            lastUnhealthyTime_us = imuSampleTime_us;
        }
        runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
    }

    const bool armed  = AP::dal().get_armed();  // 获取飞行器的武装状态

    // 核心选择仅在飞行器武装后才可用，否则强制使用第0个lane（健康的）
    if (runCoreSelection && armed) {
        // 更新所有活动核心的错误分数，并获取主要核心的错误分数
        float primaryErrorScore = updateCoreErrorScores();

        // 更新所有活动核心的相对错误累积分数
        updateCoreRelativeErrors();

        bool betterCore = false;
        bool altCoreAvailable = false;
        uint8_t newPrimaryIndex = primary;

        // 遍历所有可用核心以查找是否有可用的备用核心
        for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {
            if (coreIndex != primary) {
                float altCoreError = coreRelativeErrors[coreIndex];

                // 根据以下条件选择备用核心 -
                // 1. 健康且状态已在此时间步更新
                // 2. 相对误差低于主要核心误差
                // 3. 不是主要核心至少已经有10秒
                altCoreAvailable = coreBetterScore(coreIndex, newPrimaryIndex) &&
                    imuSampleTime_us - coreLastTimePrimary_us[coreIndex] > 1E7;

                if (altCoreAvailable) {
                    // 如果此核心的相对误差明显低于活动主要核心，则考虑切换到它
                    betterCore = altCoreError <= -BETTER_THRESH; // 如果其相对误差低于主要核心的显著水平，则为更好的核心
                    // 处理次要核心在完成航向对齐时更快的情况，这可能发生在没有磁力计时的飞行中
                    const NavEKF3_core &newCore = core[coreIndex];
                    const NavEKF3_core &oldCore = core[primary];
                    betterCore |= newCore.have_aligned_yaw() && !oldCore.have_aligned_yaw();
                    newPrimaryIndex = coreIndex;
                }
            }
        }
        altCoreAvailable = newPrimaryIndex != primary;

        // 如果有其他核心可用，并且当前主要核心满足以下条件之一，则切换核心 -
        // 1. 具有较高的错误分数
        // 2. 不健康
        // 3. 健康，但有更好的核心可用
        // 同时更新航向和位置复位数据以捕获由于lane切换引起的变化
        if (altCoreAvailable && (primaryErrorScore > 1.0f || !core[primary].healthy() || betterCore)) {
            updateLaneSwitchYawResetData(newPrimaryIndex, primary);
            updateLaneSwitchPosResetData(newPrimaryIndex, primary);
            updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
            resetCoreErrors();
            coreLastTimePrimary_us[primary] = imuSampleTime_us;
            primary = newPrimaryIndex;
            lastLaneSwitch_ms = AP::dal().millis();
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 lane switch %u", primary);
        }       
    }

    const uint8_t user_primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;
    if (primary != user_primary && core[user_primary].healthy() && !armed) {
        // 当在地面并解除武装时，强制选择用户选择的主要核心
        // 这避免了在每次飞行中使用IMU时的抽签
        // 否则，核心选择更新的时间与GPS更新的时间不一致可能导致
        // 某些飞行中使用非第一个核心作为主要核心
        // 不同的IMU可能具有非常不同的噪声特性，这会导致性能不一致
        primary = user_primary;
    }

    // 将非活动源的位置对齐到姿态估计
    sources.align_inactive_sources();
}

7. 参考资料

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