Ardupilot — EKF3使用光流室内定位代码梳理
文章目录
前言
1 Copter.cpp
1.1 void IRAM_ATTR Copter::fast_loop()
1.2 void Copter::read_AHRS(void)
1.3 对象ahrs说明
2.3 对象EKF3说明
3.2 对象core说明
前言
故事的开始，要从参数 EK3_FLOW_USE 说起。
注意：该参数适用于高级用户。
控制是否将光流数据融合到 24 状态导航估算器或 1 状态地形高度估算器中。
| RebootRequired | Values | ||||||||
|---|---|---|---|---|---|---|---|---|---|
| True |
|
本文主要梳理一下，在旋翼中 EKF3 的整个运行流程，以及在哪一步融合光流数据进行室内定位飞行。
前置参数：
1、AHRS_EKF_TYPE = 3；
使用 EKF3 卡尔曼滤波器进行姿态和位置估算。
2、EK3_GPS_TYPE = 3；
禁止使用 GPS - 当在 GPS 质量较差、多径误差较大的环境中使用光流量传感器飞行时，这一点非常有用。
1 Copter.cpp
1.1 void IRAM_ATTR Copter::fast_loop()
Ardupilot 代码中，需求资源多，运算频率高的任务，一般在 fast_loop() 函数中。这里我们只展示和 EKF3 运行相关的代码段。
运行 EKF 状态估算器（耗资巨大）。
// Main loop - 400hz
void IRAM_ATTR Copter::fast_loop()
{...// run EKF state estimator (expensive)// --------------------read_AHRS();...
}1.2 void Copter::read_AHRS(void)
读取姿态航向参考系统信息的入口函数。
我们告诉 AHRS 跳过 INS 更新，因为我们已经在 fast_loop() 中进行了更新。
void Copter::read_AHRS(void)
{// Perform IMU calculations and get attitude info//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED// update hil before ahrs updategcs().update_receive();gcs().update_send();
#endif// we tell AHRS to skip INS update as we have already done it in fast_loop()ahrs.update(true);
}1.3 对象ahrs说明
在 Copter.h 中，我们用 AP_AHRS_NavEKF 类定义了 ahrs 对象。
AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};2 AP_AHRS_NavEKF.cpp
2.1 void AP_AHRS_NavEKF::update(bool skip_ins_update)
所以，我们在跳转 update() 这个成员函数的时候，跳转到 AP_AHRS_NavEKF 类的 update() 函数。
根据 AHRS_EKF_TYPE = 3，我们运行 update_EKF3()。
void AP_AHRS_NavEKF::update(bool skip_ins_update)
{...if (_ekf_type == 2) {// if EK2 is primary then run EKF2 first to give it CPU// priorityupdate_EKF2();update_EKF3();} else {// otherwise run EKF3 firstupdate_EKF3();update_EKF2();}...
}2.2 void AP_AHRS_NavEKF::update_EKF3(void)
更新 EKF3。
void AP_AHRS_NavEKF::update_EKF3(void)
{...if (_ekf3_started) {EKF3.UpdateFilter();...}
}2.3 对象EKF3说明
在 AP_AHRS_NavEKF.h 中，我们用 NavEKF3 类定义了 EKF3 对象。
NavEKF3 &EKF3;3 AP_NavEKF3.cpp
3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)
所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3 类的 UpdateFilter() 函数。
更新滤波器状态 - 只要有新的 IMU 数据，就应调用该函数。
// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3::UpdateFilter(void)
{if (!core) {return;}imuSampleTime_us = AP_HAL::micros64();const AP_InertialSensor &ins = AP::ins();bool statePredictEnabled[num_cores];for (uint8_t i=0; i<num_cores; i++) {// if we have not overrun by more than 3 IMU frames, and we// have already used more than 1/3 of the CPU budget for this// loop then suppress the prediction step. This allows// multiple EKF instances to cooperate on schedulingif (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&(AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {statePredictEnabled[i] = false;} else {statePredictEnabled[i] = true;}core[i].UpdateFilter(statePredictEnabled[i]);}...
}3.2 对象core说明
在 AP_NavEKF3.h 中，我们用 NavEKF3_core 类定义了 core 对象。
NavEKF3_core *core = nullptr;4 AP_NavEKF3_core.cpp
4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3_core 类的 UpdateFilter() 函数。
如果缓冲区中有新的 IMU 数据，则运行 EKF 方程，在融合时间跨度上进行估算。
/********************************************************
* UPDATE FUNCTIONS *
********************************************************/
// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
{...// Check arm status and perform required checks and mode changescontrolFilterModes();...// Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the bufferif (runUpdates) {// Predict states using IMU data from the delayed time horizonUpdateStrapdownEquationsNED();// Predict the covariance growthCovariancePrediction();// Update states using magnetometer or external yaw sensor dataSelectMagFusion();// Update states using GPS and altimeter dataSelectVelPosFusion();// Update states using range beacon dataSelectRngBcnFusion();// Update states using optical flow dataSelectFlowFusion();// Update states using body frame odometry dataSelectBodyOdomFusion();// Update states using airspeed dataSelectTasFusion();// Update states using sideslip constraint assumption for fly-forward vehiclesSelectBetaFusion();// Update the filter statusupdateFilterStatus();}...
}这里有两个函数和 EKF3 使用光流传感器有关：controlFilterModes()，SelectFlowFusion()。
5 AP_NavEKF3_Control.cpp
5.1 void NavEKF3_core::controlFilterModes()
控制滤波器模式转换。
// Control filter mode transitions
void NavEKF3_core::controlFilterModes()
{...// Set the type of inertial navigation aiding usedsetAidingMode();...
}5.2 void NavEKF3_core::setAidingMode()
设置所使用的惯性导航辅助类型。
我们把飞控连接 QGC，小喇叭会不断的弹出“...stopped aiding”和“...started relative aiding”消息。
根据 AidingMode 的枚举定义，分为三种情况。
1、AID_ABSOLUTE = 0；正在使用 GPS 或其他形式的绝对位置参考辅助（也可同时使用光流），因此位置估算是绝对的。
2、AID_NONE = 1；不使用辅助，因此只有姿态和高度估计值。必须使用 constVelMode 或 constPosMode 来限制倾斜漂移。
3、AID_RELATIVE = 2；只使用光流辅助，因此位置估算值将是相对的。
这里，如果光流传感器数据良好，我们运行 AID_RELATIVE；如果光流数据较差或没有，我们运行 AID_NONE。
// Set inertial navigation aiding mode
void NavEKF3_core::setAidingMode()
{...// 检查我们是否开始或停止援助，并根据需要设置状态和模式// check to see if we are starting or stopping aiding and set states and modes as requiredif (PV_AidingMode != PV_AidingModePrev) {// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.switch (PV_AidingMode) {case AID_NONE:// We have ceased aidinggcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors// 无辅助时，利用合成恒定位置和零速度测量来估计方位和高度，以限制倾斜误差...case AID_RELATIVE:// We are doing relative position navigation where velocity errors are constrained, but position drift will occur// 我们正在进行相对位置导航，速度误差受到限制，但位置漂移会发生gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);...
}6 AP_NavEKF3_OptFlowFusion.cpp
6.1 void NavEKF3_core::SelectFlowFusion()
选择性融合光学流量传感器的测量。
// select fusion of optical flow measurements
void NavEKF3_core::SelectFlowFusion()
{...// 将光流数据融合到主滤波器中// Fuse optical flow data into the main filterif (flowDataToFuse && tiltOK) {if (frontend->_flowUse == FLOW_USE_NAV) {// Set the flow noise used by the fusion processesR_LOS = sq(MAX(frontend->_flowNoise, 0.05f));// Fuse the optical flow X and Y axis data into the main filter sequentiallyFuseOptFlow();}// reset flag to indicate that no new flow data is available for fusionflowDataToFuse = false;}...
}6.2 void NavEKF3_core::FuseOptFlow()
依次将光流 X 轴和 Y 轴数据融合到主滤波器中。
首次融合光流传感器数据，会提示："EKF3 IMU%u fusing optical flow"。
void NavEKF3_core::FuseOptFlow()
{...// notify first time onlyif (!flowFusionActive) {flowFusionActive = true;gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);}...
}