ArduSub程序学习（11）--EKF实现逻辑①

1.read_AHRS()

进入EKF，路径ArduSub.cpp里面的fast_loop()里面的read_AHRS();

//从 AHRS（姿态与航向参考系统）中读取并更新与飞行器姿态有关的信息
void Sub::read_AHRS()
{
    // Perform IMU calculations and get attitude info
    //-----------------------------------------------
    // <true> tells AHRS to skip INS update as we have already done it in fast_loop()
    //告诉 AHRS 跳过惯性导航系统（INS）的更新，因为 INS 的更新已经在主循环的快速循环（fast_loop()）中完成了。
    ahrs.update(true);
    ahrs_view.update(true);
}

2.update（ture）

负责更新AHRS的各个组件，并管理不同类型的扩展卡尔曼滤波器（EKF）

//通过EKF，读取姿态信息
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    // support locked access functions to AHRS data
    //信号量锁定
    WITH_SEMAPHORE(_rsem);
    
    // drop back to normal priority if we were boosted by the INS
    // calling delay_microseconds_boost()
    //在完成传感器更新后，恢复调度器的正常优先级，之前可能使用过临时提升。
    hal.scheduler->boost_end();

    //更新DCM
    update_DCM(skip_ins_update);

    //针对SITL的条件编译
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    update_SITL();
#endif

    //如果外部AHRS启用，则调用外部更新函数。
#if HAL_EXTERNAL_AHRS_ENABLED
    update_external();
#endif

    //hal.console->printf("Current EKF Type: %d\n", static_cast<int>(_ekf_type.get()));

    //EKF选择和执行
    if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
#if HAL_NAVEKF2_AVAILABLE
        update_EKF2();
#endif
#if HAL_NAVEKF3_AVAILABLE
        update_EKF3();
#endif
    } else {
        // otherwise run EKF3 first
#if HAL_NAVEKF3_AVAILABLE
        update_EKF3();
#endif
#if HAL_NAVEKF2_AVAILABLE
        update_EKF2();
#endif
    }

#if AP_MODULE_SUPPORTED
    // call AHRS_update hook if any
    //如果支持任何模块，调用钩子以允许它们执行与AHRS相关的附加更新。
    AP_Module::call_hook_AHRS_update(*this);
#endif

    // push gyros if optical flow present
    //如果光流传感器可用，获取陀螺仪漂移（偏差）并用此信息更新光流系统。
    if (hal.opticalflow) {
        const Vector3f &exported_gyro_bias = get_gyro_drift();
        hal.opticalflow->push_gyro_bias(exported_gyro_bias.x, exported_gyro_bias.y);
    }

    if (_view != nullptr) {
        // update optional alternative attitude view
        //如果存在可选的替代视图，则更新该视图的姿态。
        _view->update(skip_ins_update);
    }

#if !HAL_MINIMIZE_FEATURES && AP_AHRS_NAVEKF_AVAILABLE
    // update NMEA output
    //如果未最小化功能且EKF可用，更新NMEA输出，这是用于海洋导航的标准。
    update_nmea_out();
#endif

    //检查当前活动EKF类型是否自上次更新以来发生变化
    EKFType active = active_EKF_type();
    //如果发生变化，则向地面控制站（GCS）发送关于当前活动EKF类型的消息。
    if (active != last_active_ekf_type) {
        last_active_ekf_type = active;
        const char *shortname = "???";
        switch ((EKFType)active) {
        case EKFType::NONE:
            shortname = "DCM";
            break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        case EKFType::SITL:
            shortname = "SITL";
            break;
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
        case EKFType::EXTERNAL:
            shortname = "External";
            break;
#endif
#if HAL_NAVEKF3_AVAILABLE
        case EKFType::THREE:
            shortname = "EKF3";
            break;
#endif
#if HAL_NAVEKF2_AVAILABLE
        case EKFType::TWO:
            shortname = "EKF2";
            break;
#endif
        }
        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AHRS: %s active", shortname);
    }
}

 2.1update_DCM(bool skip_ins_update)

        更新DCM（方向余弦矩阵，Direction Cosine Matrix）的作用是保持和更新飞行器或自主车辆的姿态信息，使得其在三维空间中的朝向能够持续精确地被估计和修正。

        陀螺仪在长时间运行时会产生漂移误差。更新DCM时，可以利用传感器（如加速度计、磁力计等）的测量值，计算与当前DCM表示的姿态之间的误差，并据此调整陀螺仪读数。通过这种方式，DCM能够起到校正漂移的作用，确保姿态估计的准确性。

//更新姿态和航向参考系统（AHRS）中的方向余弦矩阵（DCM）。DCM是一种用来表示三维空间中物体姿态的数学模型。
void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update)
{
    // we need to restore the old DCM attitude values as these are
    // used internally in DCM to calculate error values for gyro drift
    // correction
    //恢复旧的DCM姿态值：
    roll = _dcm_attitude.x;
    pitch = _dcm_attitude.y;
    yaw = _dcm_attitude.z;
    //更新DCM的内部数据，确保当前姿态和其他相关值同步。
    update_cd_values();

    AP_AHRS_DCM::update(skip_ins_update);

    // keep DCM attitude available for get_secondary_attitude()
    //将更新后的 roll、pitch 和 yaw 值重新赋值回 _dcm_attitude
    _dcm_attitude = {roll, pitch, yaw};
}

        ★★在使用EKF进行姿态估计的系统中，DCM通常作为备用机制存在。当EKF失效或者不稳定时，系统可以回退到DCM来提供可靠的姿态估计。通过不断更新DCM，系统可以确保即使在EKF不可用的情况下，仍然有一个稳定的姿态估计可用。 

if (_ekf_type == 2) {
    update_EKF2();
    update_EKF3();
} else {
    update_EKF3();
    update_EKF2();
}
update_DCM(skip_ins_update);

• EKF2和EKF3 是不同的卡尔曼滤波器版本，它们可能分别处理不同的数据源或者计算策略。系统根据具体情况优先更新某个EKF，并依次更新另一个EKF。
• DCM的更新 则在EKF更新后进行，确保即便EKF更新失败或不稳定，系统仍能通过DCM获得姿态数据。

        在一些导航系统中，DCM可能作为EKF的初始估计工具。由于DCM计算效率高，姿态估计在短期内较为准确，EKF可以使用DCM的估计值作为其初始状态，逐渐根据传感器数据和预测模型进行更复杂的修正。

        如果EKF在某些情况下出现问题（如传感器数据丢失、不可靠的传感器输入等），系统可以回退到DCM，继续保持姿态估计的基本功能。因此，DCM作为EKF的一种补充或者冗余备份，能够增强系统的鲁棒性。

2.2update_EKF2();

 这段代码实现了AP_AHRS_NavEKF::update_EKF2()函数，用于更新飞行器导航系统中使用的扩展卡尔曼滤波器2（EKF2）的状态。该函数主要负责初始化EKF2滤波器并在之后的每个周期内更新滤波器状态，获取姿态估计、陀螺仪漂移修正、加速度计数据等。

//更新飞行器导航系统中使用的扩展卡尔曼滤波器2（EKF2）的状态
void AP_AHRS_NavEKF::update_EKF2(void)
{
    //hal.console->printf("_ekf2_started=%d\r\n",_ekf2_started);
   
    if (!_ekf2_started) {
        // wait 1 second for DCM to output a valid tilt error estimate
        //hal.console->printf("AAAAAA1\r\n");
         //等待1秒以便DCM输出有效的倾斜误差估计
        if (start_time_ms == 0) {
            start_time_ms = AP_HAL::millis();
        }
        // if we're doing Replay logging then don't allow any data
        // into the EKF yet.  Don't allow it to block us for long.
        // 检查看门狗是否重置并确保不长时间阻塞数据
        if (!hal.util->was_watchdog_reset()) {
            if (AP_HAL::millis() - start_time_ms < 5000) {
                //hal.console->printf("AAAAAA2\r\n");
                if (!AP::logger().allow_start_ekf()) {
                    return;
                }
            }
        }
         等待预设的启动延迟时间，然后初始化EKF2
        if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
            _ekf2_started = EKF2.InitialiseFilter();
        }
    }
    if (_ekf2_started) {
        //hal.console->printf("BBBBB1\r\n");
        //EKF2更新，融合传感器数据来更新姿态、速度和位置估计。
        EKF2.UpdateFilter();
        //hal.console->printf("BBBBB2\r\n");
        if (active_EKF_type() == EKFType::TWO) {
            //hal.console->printf("CCCCC1\r\n");
            Vector3f eulers;
            //矩阵更新
            EKF2.getRotationBodyToNED(_dcm_matrix);
            //姿态获取
            EKF2.getEulerAngles(-1,eulers);
            roll  = eulers.x;
            pitch = eulers.y;
            yaw   = eulers.z;

            update_cd_values();
            update_trig();

            // Use the primary EKF to select the primary gyro
            //陀螺仪漂移修正
            const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();

            const AP_InertialSensor &_ins = AP::ins();

            // get gyro bias for primary EKF and change sign to give gyro drift
            // Note sign convention used by EKF is bias = measurement - truth
            //计算修正后的陀螺仪数据
            _gyro_drift.zero();
            EKF2.getGyroBias(-1,_gyro_drift);
            _gyro_drift = -_gyro_drift;

            // calculate corrected gyro estimate for get_gyro()
            _gyro_estimate.zero();
            if (primary_imu == -1 || !_ins.get_gyro_health(primary_imu)) {
                // the primary IMU is undefined so use an uncorrected default value from the INS library
                _gyro_estimate = _ins.get_gyro();
            } else {
                // use the same IMU as the primary EKF and correct for gyro drift
                _gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
            }

            // get z accel bias estimate from active EKF (this is usually for the primary IMU)
            // 加速度计数据的处理与校正
            float abias = 0;
            EKF2.getAccelZBias(-1,abias);

            // This EKF is currently using primary_imu, and abias applies to only that IMU
            for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
                Vector3f accel = _ins.get_accel(i);
                if (i == primary_imu) {
                    accel.z -= abias;
                }
                if (_ins.get_accel_health(i)) {
                    _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
                }
            }
            _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
            //滤波器状态更新
            nav_filter_status filt_state;
            EKF2.getFilterStatus(-1,filt_state);
            AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS.
            AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching;
            AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs;
        }
    }
}

 

• 初始化EKF2：函数首先检查EKF2是否启动，并在适当时机进行初始化。
• 姿态和传感器数据更新：在EKF2成功启动后，获取姿态数据并修正陀螺仪和加速度计的漂移，确保姿态估计准确。
• 滤波器状态和融合：不断更新EKF2的状态，结合传感器数据（如GPS）进行位置和姿态融合，确保系统导航的可靠性。

2.3 update_EKF3();

AP_AHRS_NavEKF::update_EKF3() 函数实现了扩展卡尔曼滤波器3（EKF3）的更新流程。它与 update_EKF2() 类似，但专门用于 EKF3 的处理。主要功能包括初始化滤波器、更新姿态数据、陀螺仪漂移修正、加速度计偏置校正等。

未完待续~~