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();//更新DCMupdate_DCM(skip_ins_update);//针对SITL的条件编译
#if CONFIG_HAL_BOARD == HAL_BOARD_SITLupdate_SITL();
#endif//如果外部AHRS启用，则调用外部更新函数。
#if HAL_EXTERNAL_AHRS_ENABLEDupdate_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_AVAILABLEupdate_EKF2();
#endif
#if HAL_NAVEKF3_AVAILABLEupdate_EKF3();
#endif} else {// otherwise run EKF3 first
#if HAL_NAVEKF3_AVAILABLEupdate_EKF3();
#endif
#if HAL_NAVEKF2_AVAILABLEupdate_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_SITLcase EKFType::SITL:shortname = "SITL";break;
#endif
#if HAL_EXTERNAL_AHRS_ENABLEDcase EKFType::EXTERNAL:shortname = "External";break;
#endif
#if HAL_NAVEKF3_AVAILABLEcase EKFType::THREE:shortname = "EKF3";break;
#endif
#if HAL_NAVEKF2_AVAILABLEcase 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;}}}等待预设的启动延迟时间，然后初始化EKF2if (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 IMUfor (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;}}
}