ArduPilot开源代码之NavEKF3

lida2003

于 2024-07-30 15:54:58 发布

阅读量561

点赞数 14

分类专栏： ArduPilot 文章标签：开源 ardupilot c++

本文链接：https://blog.csdn.net/lida2003/article/details/140799008

版权

ArduPilot 专栏收录该内容

146 篇文章 90 订阅

订阅专栏

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) {
        return false;
    }

    // 将所有核心的相对误差得分设为0
    resetCoreErrors();

    // 最初将主核心设为用户选择的主核心
    primary = uint8_t(_primary_core) < num_cores? _primary_core : 0;

    // 使共享原点无效
    common_origin_valid = false;

    // 初始化核心。仅当所有核心初始化成功时返回成功
    bool ret = true;
    for (uint8_t i=0; i<num_cores; i++) {
        ret &= core[i].InitialiseFilterBootstrap();
    }

    // 将最后一次核心为主的时间设为0
    memset(coreLastTimePrimary_us, 0, sizeof(coreLastTimePrimary_us));

    // 将用于捕捉复位事件的结构体清零
    memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
    memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
    memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));

    return ret;
}

2.4.2 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();

    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
    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. 健康，但有更好的核心可用
        // 还需更新偏航和位置重置数据，以捕捉由于通道切换而产生的变化
        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;
    }

    // 将非活动源的位置信息对齐到ahrs
    sources.align_inactive_sources();
}

2.4.3 healthy

采用《ArduPilot开源代码之NavEKF3_core》中的方法进行判断。

// Check basic filter health metrics and return a consolidated health status
bool NavEKF3::healthy(void) const
{
    if (!core) {
        return false;
    }
    return core[primary].healthy();
}

2.4.4 pre_arm_check

解锁前检查

// 如果进行准备检查失败，则返回 false，此时缓冲区将填充失败消息
// 如果需要检查水平位置配置，则 requires_position 应该为 true
bool NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
{
    // 检查源配置
    if (!sources.pre_arm_check(requires_position, failure_msg, failure_msg_len)) {
        return false;
    }

    // 检查是否使用了磁罗盘（即 EK3_SRCn_YAW）且使用了已废弃的 MAG_CAL 值（5 代表 EXTERNAL_YAW，6 代表 EXTERNAL_YAW_FALLBACK）
    const int8_t magCalParamVal = _magCal.get();
    const AP_NavEKF_Source::SourceYaw yaw_source = sources.getYawSource();
    if (((magCalParamVal == 5) || (magCalParamVal == 6)) && (yaw_source != AP_NavEKF_Source::SourceYaw::GPS)) {
        // 磁罗盘作为偏航源，但 MAG_CAL 的有效值已废弃
        AP::dal().snprintf(failure_msg, failure_msg_len, "EK3_MAG_CAL and EK3_SRC1_YAW inconsistent");
        return false;
    }

    if (!core) {
        AP::dal().snprintf(failure_msg, failure_msg_len, "no EKF3 cores");
        return false;
    }
    for (uint8_t i = 0; i < num_cores; i++) {
        if (!core[i].healthy()) {
            const char *failure = core[i].prearm_failure_reason();
            if (failure != nullptr) {
                AP::dal().snprintf(failure_msg, failure_msg_len, failure);
            } else {
                AP::dal().snprintf(failure_msg, failure_msg_len, "EKF3 core %d unhealthy", (int)i);
            }
            return false;
        }
    }
    return true;
}

2.5 状态检索

获取位置 (getPosNE, getPosD)、速度 (getVelNED)、空速、风估计等的方法。

2.5.1 getPosNE

获取相对于参考点的最后计算出的NE坐标系下的位置（米）矢量

// 如果没有可用的计算结果，使用最好的可用数据并返回false
// 如果返回false，不要用于飞行控制
bool NavEKF3::getPosNE(Vector2f &posNE) const
{
    if (!core) {
        return false;
    }
    return core[primary].getPosNE(posNE);
}

2.5.2 getPosD

获取最后计算的相对于参考点的 D 位置（单位：米）

// 如果没有可用的计算结果，则使用最好的可用数据并返回 false
// 如果返回 false，则不要用于飞行控制
bool NavEKF3::getPosD(float &posD) const
{
    if (!core) {
        return false;
    }
    return core[primary].getPosD(posD);
}

2.5.3 getVelNED

以米每秒为单位返回 NED 速度矢量

void NavEKF3::getVelNED(Vector3f &vel) const
{
    if (core) {
        core[primary].getVelNED(vel);
    }
}

2.5.4 getAirSpdVec

返回机体框架中真实空速向量的估计值，单位为米/秒

// 如果估计不可用，返回 false
bool NavEKF3::getAirSpdVec(Vector3f &vel) const
{
    if (core) {
        return core[primary].getAirSpdVec(vel);
    }
    return false;
}

2.5.5 getWind

获取北东地风速矢量估算值，单位为米/秒（正值表示空气沿轴方向移动）

// 如果风状态估算处于活动状态，则返回 true
bool NavEKF3::getWind(Vector3f &wind) const
{
    if (core == nullptr) {
        return false;
    }
    return core[primary].getWind(wind);
}

2.6 偏差和偏移管理

getGyroBias() 和 getAccelBias() 提供传感器偏差信息。
resetGyroBias() 和 resetHeightDatum() 用于重置偏差和高度参考。

2.6.1 getGyroBias

返回指定实例的机身轴陀螺仪偏差估计值，单位为弧度/秒

// 超出范围的实例（例如 -1）返回主实例的数据
void NavEKF3::getGyroBias(int8_t instance, Vector3f &gyroBias) const
{
    if (instance < 0 || instance >= num_cores) instance = primary;
    if (core) {
        core[instance].getGyroBias(gyroBias);
    }
}

2.6.2 getAccelBias

返回加速度计偏差估计值，单位为 m/s/s

// 超出范围的实例（例如 -1）将返回主实例的数据
void NavEKF3::getAccelBias(int8_t instance, Vector3f &accelBias) const
{
    if (instance < 0 || instance >= num_cores) instance = primary;
    if (core) {
        core[instance].getAccelBias(accelBias);
    }
}

2.6.3 resetGyroBias

重置机体轴陀螺仪偏置估计

void NavEKF3::resetGyroBias(void)
{
    AP::dal().log_event3(AP_DAL::Event::resetGyroBias);

    if (core) {
        for (uint8_t i=0; i<num_cores; i++) {
            core[i].resetGyroBias();
        }
    }
}

2.6.4 resetHeightDatum

将气压计重置为在当前高度读数为零

// 将 EKF 高度重置为零
// 调整 EKF 原点高度，以使 EKF 高度 + 原点高度与之前相同
// 如果已执行高度基准重置，返回 true
// 如果使用范围探测器测量高度，则不执行重置，返回 false
bool NavEKF3::resetHeightDatum(void)
{
    AP::dal().log_event3(AP_DAL::Event::resetHeightDatum);

    bool status = true;
    if (core) {
        for (uint8_t i=0; i<num_cores; i++) {
            if (!core[i].resetHeightDatum()) {
                status = false;
            }
        }
    } else {
        status = false;
    }
    return status;
}

2.7 外部数据处理

支持与外部导航系统和传感器集成 (writeOptFlowMeas, writeBodyFrameOdom)。

2.7.1 writeOptFlowMeas

将原始光流测量数据通过算法接口输入

// rawFlowQuality 是质量的测量值，范围从 0 到 255，255 表示最佳质量
// rawFlowRates 是 X 和 Y 传感器轴上的光流速率，单位为 rad/sec（弧度/秒）
// rawGyroRates 是传感器内部陀螺仪测量的传感器旋转速率，单位为 rad/sec（弧度/秒）
// 符号约定是：传感器在某轴上的右手旋转会产生正的光流和陀螺仪速率
// msecFlowMeas 是接收到光流数据的调度时间，单位为毫秒
// posOffset 是传感器在机体坐标系中的 XYZ 位置，单位为米
// heightOverride 是传感器在地面上的固定高度，单位为米，当在探测车上使用时。如果不使用则为 0
void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
    AP::dal().writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);

    if (core) {
        for (uint8_t i=0; i<num_cores; i++) {
            core[i].writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset, heightOverride);
        }
    }
}

2.7.2 writeBodyFrameOdom

将视觉里程计传感器的机身框架线性和角位移测量值通过算法接口输入

// quality 是一个从 0 到 100 的归一化置信度值
// delPos 是在 timeStamp_ms 时间点测量的相对于惯性参考系的机身框架中的 XYZ 线性位置变化 (单位：米)
// delAng 是在 timeStamp_ms 时间点测量的相对于惯性参考系的机身框架中的 XYZ 角旋转 (单位：弧度)
// delTime 是测量 delPos 和 delAng 的时间间隔 (单位：秒)
// timeStamp_ms 是用于计算 delPos 和 delAng 的最后图像的时间戳 (单位：毫秒)
// delay_ms 是外部导航系统测量相对于惯性测量的平均延迟时间 (单位：毫秒)
// posOffset 是相机焦点的机身框架 XYZ 位置偏移 (单位：米)
void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
    AP::dal().writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);

    if (core) {
        for (uint8_t i=0; i<num_cores; i++) {
            core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset);
        }
    }
}

2.8 日志和报告

Log_Write() 用于记录日志。
send_status_report() 用于通信状态。

2.8.1 Log_Write

将EKF信息写入机载日志

void NavEKF3::Log_Write()
{
    // 仅当日志记录启用时才记录
    if (activeCores() <= 0) {
        return;
    }
    if (lastLogWrite_us == imuSampleTime_us) {
        // 车辆正在重复记录日志
        return;
    }
    lastLogWrite_us = imuSampleTime_us;

    uint64_t time_us = AP::dal().micros64();

    for (uint8_t i = 0; i < activeCores(); i++) {
        core[i].Log_Write(time_us);
    }

    AP::dal().start_frame(AP_DAL::FrameType::LogWriteEKF3);
}

2.8.2 send_status_report

发送一个EKF_STATUS_REPORT消息到地面控制站（GCS）

void NavEKF3::send_status_report(GCS_MAVLINK &link) const
{
    if (core) {
        core[primary].send_status_report(link);
    }
}

2.9 故障和状态管理

getFilterFaults() 和 getFilterStatus() 提供诊断信息。

2.9.1 getFilterFaults

返回滤波器故障状态作为位掩码整数

// 0 = 四元数是 NaN
// 1 = 速度是 NaN
// 2 = X 轴磁力计融合条件差
// 3 = Y 轴磁力计融合条件差
// 4 = Z 轴磁力计融合条件差
// 5 = 空速融合条件差
// 6 = 合成侧滑融合条件差
// 7 = 滤波器未初始化
void NavEKF3::getFilterFaults(uint16_t &faults) const
{
    if (core) {
        core[primary].getFilterFaults(faults);
    } else {
        faults = 0;
    }
}

2.9.2 getFilterStatus

返回滤波器状态标志

union nav_filter_status {
    struct {
        bool attitude           : 1; // 0 - true if attitude estimate is valid
        bool horiz_vel          : 1; // 1 - true if horizontal velocity estimate is valid
        bool vert_vel           : 1; // 2 - true if the vertical velocity estimate is valid
        bool horiz_pos_rel      : 1; // 3 - true if the relative horizontal position estimate is valid
        bool horiz_pos_abs      : 1; // 4 - true if the absolute horizontal position estimate is valid
        bool vert_pos           : 1; // 5 - true if the vertical position estimate is valid
        bool terrain_alt        : 1; // 6 - true if the terrain height estimate is valid
        bool const_pos_mode     : 1; // 7 - true if we are in const position mode
        bool pred_horiz_pos_rel : 1; // 8 - true if filter expects it can produce a good relative horizontal position estimate - used before takeoff
        bool pred_horiz_pos_abs : 1; // 9 - true if filter expects it can produce a good absolute horizontal position estimate - used before takeoff
        bool takeoff_detected   : 1; // 10 - true if optical flow takeoff has been detected
        bool takeoff            : 1; // 11 - true if filter is compensating for baro errors during takeoff
        bool touchdown          : 1; // 12 - true if filter is compensating for baro errors during touchdown
        bool using_gps          : 1; // 13 - true if we are using GPS position
        bool gps_glitching      : 1; // 14 - true if GPS glitching is affecting navigation accuracy
        bool gps_quality_good   : 1; // 15 - true if we can use GPS for navigation
        bool initalized         : 1; // 16 - true if the EKF has ever been healthy
        bool rejecting_airspeed : 1; // 17 - true if we are rejecting airspeed data
        bool dead_reckoning     : 1; // 18 - true if we are dead reckoning (e.g. no position or velocity source)
    } flags;
    uint32_t value;
};

void NavEKF3::getFilterStatus(nav_filter_status &status) const
{
    if (core) { // 如果core指针有效
        core[primary].getFilterStatus(status); // 获取主要滤波器的状态并存储在status中
    } else { // 如果core指针无效
        memset(&status, 0, sizeof(status)); // 将status置零
    }
}

2.10 参数管理

- 管理和转换参数的方法，如 `set_baro_alt_noise()`。

2.10.1 set_baro_alt_noise

    // set and save the _baroAltNoise parameter
    void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); };

3. 重要逻辑

结合《ArduPilot开源飞控之AP_AHRS》，重要逻辑过程如下：

3.1 初始化

主要逻辑调用关系

Copter::init_ardupilot
 └──> Copter::startup_INS_ground
     └──> AP_AHRS::reset
         └──> NavEKF3::InitialiseFilter
 
FAST_TASK(read_AHRS)
 └──> Copter::read_AHRS
     └──> AP_AHRS::update
         └──> AP_AHRS::update_EKF3
             └──> NavEKF3::InitialiseFilter

注：以下调用关系更上层未知??./Tools/Replay/LR_MsgHandler.cpp?

AP_DAL::handle_message
 └──> NavEKF3::InitialiseFilter

3.2 任务调度

主要逻辑调用关系(采用了FAST_TASK任务)

*注：详见以下文章

FAST_TASK(read_AHRS)
 └──> Copter::read_AHRS
     └──> AP_AHRS::update
         └──> AP_AHRS::update_EKF3
             └──> NavEKF3::UpdateFilter

注：以下调用关系更上层未知??./Tools/Replay/LR_MsgHandler.cpp?

AP_DAL::handle_message
 └──> NavEKF3::UpdateFilter

4. 总结

通过AHRS(Attitude and Heading Reference System)流程框架，聚焦NavEKF3算法，根据传感器数据估计移动物体的导航和状态。

注：当前在Ardupilot上比较主流的是EKF3算法，当然当EKF3失效时会回退到DCM，具体理论可以参考《BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论》。

5. 参考资料

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

lida2003

关注

14
点赞
踩
7

收藏

觉得还不错? 一键收藏
0
评论
ArduPilot开源代码之NavEKF3

通过AHRS(Attitude and Heading Reference System)流程框架，聚焦NavEKF3算法，根据传感器数据估计移动物体的导航和状态。注：当前在Ardupilot上比较主流的是EKF3算法，当然当EKF3失效时会回退到DCM，具体理论可以参考《BetaFlight深入传感设计之五：MahonyAHRS & 方向余弦矩阵理论》。
复制链接

扫一扫

专栏目录