Ardupilot — EKF3使用光流室内定位代码梳理

文章目录

前言

1 Copter.cpp

1.1 void IRAM_ATTR Copter::fast_loop()

1.2 void Copter::read_AHRS(void)

1.3 对象ahrs说明

2 AP_AHRS_NavEKF.cpp

2.1 void AP_AHRS_NavEKF::update(bool skip_ins_update)

2.2 void AP_AHRS_NavEKF::update_EKF3(void)

2.3 对象EKF3说明

3 AP_NavEKF3.cpp

3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)

3.2 对象core说明

4 AP_NavEKF3_core.cpp

4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)

5 AP_NavEKF3_Control.cpp

5.1 void NavEKF3_core::controlFilterModes()

5.2 void NavEKF3_core::setAidingMode()

6 AP_NavEKF3_OptFlowFusion.cpp

6.1 void NavEKF3_core::SelectFlowFusion()

6.2 void NavEKF3_core::FuseOptFlow()

---

前言

故事的开始，要从参数 EK3_FLOW_USE 说起。

注意：该参数适用于高级用户。

控制是否将光流数据融合到 24 状态导航估算器或 1 状态地形高度估算器中。

RebootRequired	Values
True	Value Meaning 0 None 1 Navigation 2 Terrain

---

本文主要梳理一下，在旋翼中 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);}...
}