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 的处理。主要功能包括初始化滤波器、更新姿态数据、陀螺仪漂移修正、加速度计偏置校正等。
未完待续~~
标签:11,DCM,--,EKF,update,AHRS,EKF2,gyro From: https://blog.csdn.net/weixin_49839886/article/details/142551540