ArduPilot开源代码之NavEKF3

1. 源由

NavEKF3 类用于无人机系统中的导航和状态估计。它是扩展卡尔曼滤波器（EKF）实现的一部分，用于根据传感器数据估计移动物体的状态。

• 自主设备: 用于无人机、车、船等，利用传感器数据保持稳定飞行/导航。
• 传感器融合: 整合来自不同来源的数据（IMU、GPS、光流）以提高准确性。
• 错误处理: 许多方法返回布尔值以指示成功，表明有稳健的错误检查。
• 实时处理: 设计为在高频率调用时处理新数据（例如 IMU 更新）。

如果需要具体方法或概念的进一步细节，请随时询问！

2. 框架设计

2.1 友元类

• NavEKF3_core 是一个友元类，可以访问 NavEKF3 的私有成员。

注：详见ArduPilot开源代码之NavEKF3_core

2.2 禁止复制

• CLASS_NO_COPY(NavEKF3); 防止类实例被复制。

2.3 参数和配置

• 静态结构 var_info 和 var_info2 可能用于存储配置参数。

注：定义和初始化，详见AP_NavEKF3.cpp/h文件。

2.4 核心功能

• 初始化: InitialiseFilter() 设置滤波器。
• 更新: UpdateFilter() 在有新的 IMU 数据时调用。
• 健康检查: 像 healthy() 和 pre_arm_check() 确保系统完整性。

2.4.1 InitialiseFilter

EKF3算法核心初始化过程，主要进行内存分配和变量初始化

// 初始化滤波器
bool NavEKF3::InitialiseFilter(void)
{
   
   
    // 如果禁用或IMU掩码为0，则返回false
    if (_enable == 0 || _imuMask == 0) {
   
   
        return false;
    }
    const auto &ins = AP::dal().ins();

    AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF3);

    // 获取当前IMU采样时间
    imuSampleTime_us = AP::dal().micros64();

    // 计算预期的帧时间
    const float loop_rate = ins.get_loop_rate_hz();
    if (!is_positive(loop_rate)) {
   
   
        return false;
    }
    _frameTimeUsec = 1e6 / loop_rate;

    // 预测时每次预期的IMU帧数
    _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));

#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone)
    // 如有必要，转换参数
    convert_parameters();
#endif

#if APM_BUILD_TYPE(APM_BUILD_Replay)
    // 如果没有加速度计，返回false
    if (ins.get_accel_count() == 0) {
   
   
        return false;
    }
#endif

    // 如果核心为空，则代表第一次调用，属于初始化阶段，需要进行core的内存分配
    if (core == nullptr) {
   
   

        // 对于一个IMU，不运行多个滤波器
        uint8_t mask = (1U<<ins.get_accel_count())-1;
        _imuMask.set_and_default(_imuMask.get() & mask);
        
        // 初始化设置变量
        for (uint8_t i=0; i<MAX_EKF_CORES; i++) {
   
   
            coreSetupRequired[i] = false;
            coreImuIndex[i] = 0;
        }
        num_cores = 0;

        // 从掩码计数IMU
        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
   
   
            if (_imuMask & (1U<<i)) {
   
   
                coreSetupRequired[num_cores] = true;
                coreImuIndex[num_cores] = i;
                num_cores++;
            }
        }

        // 检查是否有足够的内存创建EKF核心
        if (AP::dal().available_memory() < sizeof(NavEKF3_core)*num_cores + 4096) {
   
   
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3内存不足");
            _enable.set(0);
            num_cores = 0;
            return false;
        }

        // 尝试从CCM RAM分配，如果不可用或已满则回退到普通RAM
        core = (NavEKF3_core*)AP::dal().malloc_type(sizeof(NavEKF3_core)*num_cores, AP::dal().MEM_FAST);
        if (core == nullptr) {
   
   
            _enable.set(0);
            num_cores = 0;
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3分配失败");
            return false;
        }

        // 对所有核心调用构造函数
        for (uint8_t i = 0; i < num_cores; i++) {
   
   
            new (&core[i]) NavEKF3_core(this);
        }
    }

    // 设置已创建的核心
    // 指定使用的IMU并创建数据缓冲区
    // 如果无法设置核心，返回false并下次调用函数时再试
    bool core_setup_success = true;
    for (uint8_t core_index=0; core_index<num_cores; core_index++) {
   
   
        if (coreSetupRequired[core_index]) {
   
   
            coreSetupRequired[core_index] = !core[core_index].setup_core(coreImuIndex[core_index], core_index);
            if (coreSetupRequired[core_index]) {
   
   
                core_setup_success = false;
            }
        }
    }
    // 如果任何核心无法设置，则失败退出
    if (!core_setup_success)