文章目录
1.1 void IRAM_ATTR Copter::fast_loop()
1.2 void Copter::read_AHRS(void)
前言
故事的开始，要从参数 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 update
gcs().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
// priority
update_EKF2();
update_EKF3();
} else {
// otherwise run EKF3 first
update_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 scheduling
if (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 changes
controlFilterModes();
...
// Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
if (runUpdates) {
// Predict states using IMU data from the delayed time horizon
UpdateStrapdownEquationsNED();
// Predict the covariance growth
CovariancePrediction();
// Update states using magnetometer or external yaw sensor data
SelectMagFusion();
// Update states using GPS and altimeter data
SelectVelPosFusion();
// Update states using range beacon data
SelectRngBcnFusion();
// Update states using optical flow data
SelectFlowFusion();
// Update states using body frame odometry data
SelectBodyOdomFusion();
// Update states using airspeed data
SelectTasFusion();
// Update states using sideslip constraint assumption for fly-forward vehicles
SelectBetaFusion();
// Update the filter status
updateFilterStatus();
}
...
}这里有两个函数和 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 used
setAidingMode();
...
}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 required
if (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 aiding
gcs().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 filter
if (flowDataToFuse && tiltOK) {
if (frontend->_flowUse == FLOW_USE_NAV) {
// Set the flow noise used by the fusion processes
R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
// Fuse the optical flow X and Y axis data into the main filter sequentially
FuseOptFlow();
}
// reset flag to indicate that no new flow data is available for fusion
flowDataToFuse = false;
}
...
}6.2 void NavEKF3_core::FuseOptFlow()
依次将光流 X 轴和 Y 轴数据融合到主滤波器中。
首次融合光流传感器数据，会提示："EKF3 IMU%u fusing optical flow"。
void NavEKF3_core::FuseOptFlow()
{
...
// notify first time only
if (!flowFusionActive) {
flowFusionActive = true;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);
}
...
}