ArduPilot开源代码之NavEKF2_core
1. 源由
NavEKF2_core
类EKF2的第二层抽象,对于无人机等系统中的导航任务至关重要,提供实时状态估计和传感器融合功能。
2. 框架设计
2.1 构造函数
- NavEKF2_core: 使用前端引用初始化核心EKF。
2.1.1 NavEKF2_core
没有具体的初始化流程,主要是成员变量赋值。
- frontend = _frontend
- dal = AP::dal()
NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
frontend(_frontend),
dal(AP::dal())
{
}
初始化流程
NavEKF2::InitialiseFilter
└──> core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MEM_FAST);
2.2 设置和初始化
- setup_core: 使用特定索引配置核心后端。
- InitialiseFilterBootstrap: 在静止状态下使用加速度计和磁力计数据初始化状态。
2.2.1 setup_core
核心后端设置缓冲(ring buffer)大小。
bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
{
imu_index = _imu_index;
gyro_index_active = _imu_index;
accel_index_active = _imu_index;
core_index = _core_index;
/*
imu_buffer_length 需要处理在 100Hz 最大融合速率下的 260ms 延迟。
非 IMU 数据以超过 100Hz 的速度输入时会被下采样。
对于 50Hz 的主循环速率,我们需要较短的缓冲区。
*/
if (dal.ins().get_loop_rate_hz() < 100) {
imu_buffer_length = 13;
} else {
// 在 100 Hz 融合速率下最多 260 毫秒延迟
imu_buffer_length = 26;
}
if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedMag.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedBaro.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedTAS.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedOF.init(FLOW_BUFFER_LENGTH)) {
return false;
}
// 注意:使用双测距仪可能会加倍存储量
if(!storedRange.init(2*OBS_BUFFER_LENGTH)) {
return false;
}
// 注意:测距信标数据一次读取一个,并且可能以高速度到达
if(!storedRangeBeacon.init(imu_buffer_length)) {
return false;
}
if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {
return false;
}
if(!storedOutput.init(imu_buffer_length)) {
return false;
}
if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
return false;
}
if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
// 检查是否有足够的内存来创建 EKF-GSF 对象
if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 内存不足", (unsigned)imu_index);
return false;
}
// 尝试实例化
yawEstimator = NEW_NOTHROW EKFGSF_yaw();
if (yawEstimator == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: 分配失败", (unsigned)imu_index);
return false;
}
}
return true;
}
2.2.2 InitialiseFilterBootstrap
滤波器初始化
// 从加速度计和磁力计数据初始化状态(如果存在)
// 该方法只能在车辆静止时使用
bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
// 如果是飞机且没有GPS锁定,则不初始化
if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF2 init failure: No GPS lock");
statesInitialised = false;
return false;
}
if (statesInitialised) {
// 我们已经初始化,但在IMU缓冲区填满之前不返回true。这防止在滤波器启动期间IMU数据暂停的时间漏洞
readIMUData();
readMagData();
readGpsData();
readBaroData();
return storedIMU.is_filled();
}
// 将重复使用的变量置零
InitialiseVariables();
const auto &ins = dal.ins();
// 初始化IMU数据
dtIMUavg = ins.get_loop_delta_t();
readIMUData();
storedIMU.reset_history(imuDataNew);
imuDataDelayed = imuDataNew;
// 加速度计测得的XYZ机体轴上的加速度向量(m/s^2)
Vector3F initAccVec;
// TODO 我们应该在多个周期内平均加速度读数
initAccVec = ins.get_accel(accel_index_active).toftype();
// 读取磁力计数据
readMagData();
// 规范化加速度向量
ftype pitch=0, roll=0;
if (initAccVec.length() > 0.001f) {
initAccVec.normalize();
// 计算初始俯仰角
pitch = asinF(initAccVec.x);
// 计算初始滚转角
roll = atan2F(-initAccVec.y , -initAccVec.z);
}
// 计算初始滚转和俯仰方向
stateStruct.quat.from_euler(roll, pitch, 0.0f);
// 初始化动态状态
stateStruct.velocity.zero();
stateStruct.position.zero();
stateStruct.angErr.zero();
// 初始化静态过程模型状态
stateStruct.gyro_bias.zero();
stateStruct.gyro_scale.x = 1.0f;
stateStruct.gyro_scale.y = 1.0f;
stateStruct.gyro_scale.z = 1.0f;
stateStruct.accel_zbias = 0.0f;
stateStruct.wind_vel.zero();
stateStruct.earth_magfield.zero();
stateStruct.body_magfield.zero();
// 读取GPS并设置位置和速度状态
readGpsData();
ResetVelocity();
ResetPosition();
// 读取气压计并设置高度状态
readBaroData();
ResetHeight();
// 在NED导航框架中定义地球旋转向量
calcEarthRateNED(earthRateNED, dal.get_home().lat);
// 初始化协方差矩阵
CovarianceInit();
// 重置输出状态
StoreOutputReset();
// 现在状态已初始化,设置为true
statesInitialised = true;
// 重置不活动的偏差
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
inactiveBias[i].gyro_bias.zero();
inactiveBias[i].accel_zbias = 0;
inactiveBias[i].gyro_scale.x = 1;
inactiveBias[i].gyro_scale.y = 1;
inactiveBias[i].gyro_scale.z = 1;
}
// 我们最初返回false以等待IMU缓冲区填满
return false;
}
2.3 更新和健康检查
- UpdateFilter: 在新IMU数据可用时更新滤波器状态。
- healthy: 检查滤波器的健康状况。
- errorScore: 提供滤波器健康状况的错误评分。
2.3.1 UpdateFilter
更新滤波器状态 - 每当有新的IMU数据时都应该调用。
void NavEKF2_core::UpdateFilter(bool predict)
{
// 设置标志,指示滤波器前端已经允许开始新的状态预测周期
startPredictEnabled = predict;
// 如果状态尚未初始化,则不运行滤波器更新
if (!statesInitialised) {
return;
}
// 启动用于负载测量的定时器
#if ENABLE_EKF_TIMING
void *istate = hal.scheduler->disable_interrupts_save();
static uint32_t timing_start_us;
timing_start_us = dal.micros();
#endif
fill_scratch_variables();
// TODO - 在飞行中重启的方法
// 获取更新步骤的起始时间
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
// 检查臂状态并执行所需的检查和模式更改
controlFilterModes();
// 读取IMU数据作为增量角度和速度
readIMUData();
// 如果缓冲区中有新的IMU数据,运行EKF方程以估计融合时间地平线
if (runUpdates) {
// 使用来自延迟时间地平线的IMU数据预测状态
UpdateStrapdownEquationsNED();
// 预测协方差增长
CovariancePrediction();
// 运行IMU预测步骤以获得GSF偏航估计器算法
// 使用IMU和可选的真空气速数据。
// 必须在SelectMagFusion()之前运行,以提供最新的偏航估计
runYawEstimatorPrediction();
// 使用磁力计数据更新状态
SelectMagFusion();
// 使用GPS和高度计数据更新状态
SelectVelPosFusion();
// 运行GPS速度修正步骤以获得GSF偏航估计器算法
// 并使用偏航估计值重置主EKF偏航(如果请求)
// 必须在SelectVelPosFusion()之后运行,以确保有最新的GPS数据
runYawEstimatorCorrection();
#if AP_BEACON_ENABLED
// 使用范围信标数据更新状态
SelectRngBcnFusion();
#endif
// 使用光流数据更新状态
SelectFlowFusion();
// 使用空气速度数据更新状态
SelectTasFusion();
// 使用侧滑约束假设更新状态(适用于前进飞行的车辆)
SelectBetaFusion();
// 更新滤波器状态
updateFilterStatus();
}
// 从融合中将风输出转发到输出时间地平线
calcOutputStates();
// 停止用于负载测量的定时器
#if ENABLE_EKF_TIMING
static uint32_t total_us;
static uint32_t timing_counter;
total_us += dal.micros() - timing_start_us;
if (timing_counter++ == 4000) {
DEV_PRINTF("ekf2 avg %.2f us\n", total_us / float(timing_counter));
total_us = 0;
timing_counter = 0;
}
hal.scheduler->restore_interrupts(istate);
#endif
/*
这是一个检查,用来应对车辆静止在地面时过于自信的状态。
这种情况下,用户尝试解锁时会出现“陀螺仪仍在稳定”现象。
在这种状态下,EKF无法恢复,所以我们进行硬重置,让它再试一次。
*/
if (filterStatus.value != 0) {
last_filter_ok_ms = dal.millis();
}
if (filterStatus.value == 0 &&
last_filter_ok_ms != 0 &&
dal.millis() - last_filter_ok_ms > 5000 &&
!dal.get_armed()) {
// 在健康状态之后不健康5秒,重置滤波器
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u 强制重置", (unsigned)imu_index);
last_filter_ok_ms = 0;
statesInitialised = false;
InitialiseFilterBootstrap();
}
}
2.3.2 healthy
检查基本的滤波器健康指标,并返回合并的健康状态
bool NavEKF2_core::healthy(void) const
{
uint16_t faultInt;
getFilterFaults(faultInt);
if (faultInt > 0) {
return false;
}
if (velTestRatio > 1 && posTestRatio > 1 && hgtTestRatio > 1) {
// 如果三个指标都大于1,则说明滤波器的状态
// 非常不健康。
return false;
}
// 在使用前给滤波器一个时间来稳定
if ((imuSampleTime_ms - ekfStartTime_ms) < 1000 ) {
return false;
}
// 当处于地面且在静态模式下时,位置和高度的创新必须在限制范围内
ftype horizErrSq = sq(innovVelPos[3]) + sq(innovVelPos[4]);
if (onGround && (PV_AidingMode == AID_NONE) && ((horizErrSq > 1.0f) || (fabsF(hgtInnovFiltState) > 1.0f))) {
return false;
}
// 一切正常
return true;
}
2.3.3 errorScore
返回一个合并的错误分数,数值越高表示错误越大;旨在供前端使用,以确定哪个是主要的EKF
ftype NavEKF2_core::errorScore() const
{
ftype score = 0.0f;
if (tiltAlignComplete && yawAlignComplete) {
// 检查GPS融合性能
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
// 检查高度计融合性能
score = MAX(score, hgtTestRatio);
// 检查姿态修正
const ftype tiltErrThreshold = 0.05f;
score = MAX(score, tiltErrFilt / tiltErrThreshold);
}
return score;
}
2.4 位置和速度
- getPosNE, getPosD: 获取东北和下方向位置。
- getVelNED: 提供NED速度。
2.4.1 getPosNE
获取NE坐标系下机体相对于起始点的二维矢量。
bool NavEKF2_core::getPosNE(Vector2f &posNE) const
{
// 有三种操作模式:绝对位置(GPS 融合),相对位置(光学流融合)和恒定位置(没有位置估计可用)
if (PV_AidingMode != AID_NONE) {
// 这是正常的操作模式,我们可以使用 EKF 位置状态
// 校正 IMU 偏移(EKF 计算在 IMU 处)
posNE.x = outputDataNew.position.x + posOffsetNED.x;
posNE.y = outputDataNew.position.y + posOffsetNED.y;
return true;
} else {
// 在恒定位置模式下,EKF 位置状态在原点,因此我们不能将它们用作位置估计
if(validOrigin) {
if ((dal.gps().status(dal.gps()