ArduPilot开源代码之NavEKF3_core
1. 源由
NavEKF3_core继承自NavEKF_core_common，提供了一个用于导航的扩展卡尔曼滤波器（EKF）的实现。该滤波器用于通过整合各种传感器输入来估计车辆的状态。
所以，NavEKF3_core是在NavEKF_core_common的进一步深入研读需要。
2. 框架设计
2.1 构造函数
NavEKF3_core(class NavEKF3 *_frontend);- 用一个前端实例初始化EKF核心。
2.1.1 NavEKF3_core
初始化成员变量 //没有太多复杂的动作
// constructor
NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend) :
frontend(_frontend),
dal(AP::dal()),
public_origin(frontend->common_EKF_origin)
{
firstInitTime_ms = 0;
lastInitFailReport_ms = 0;
}
NavEKF3::InitialiseFilter
└──> core = (NavEKF3_core*)AP::dal().malloc_type(sizeof(NavEKF3_core)*num_cores, AP::dal().MEM_FAST);
2.2 设置和初始化
bool setup_core(uint8_t _imu_index, uint8_t _core_index);- 配置核心的IMU和核心索引。
bool InitialiseFilterBootstrap(void);- 在静止时，从加速度计和磁力计数据初始化滤波器。
2.2.1 setup_core
核心后端设置缓冲(ring buffer)大小。
bool NavEKF3_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 需要处理传感器延迟的最坏情况，
按照目标 EKF 状态预测速率进行处理，默认100Hz。
非 IMU 数据来得更快的会被下采样，减少负荷和IMU数据对齐。
*/
// 计算预期的 EKF 时间步长
if (dal.ins().get_loop_rate_hz() > 0) {
dtEkfAvg = 1.0f / dal.ins().get_loop_rate_hz();
dtEkfAvg = MAX(dtEkfAvg, EKF_TARGET_DT);
} else {
return false;
}
// 查找所有潜在传感器的最大时间延迟
uint16_t maxTimeDelay_ms = MAX(frontend->_hgtDelay_ms,
MAX(frontend->_flowDelay_ms,
MAX(frontend->_rngBcnDelay_ms,
MAX(frontend->magDelay_ms,
(uint16_t)(EKF_TARGET_DT_MS)
))));
// GPS 感测可能有较大的延迟，如果禁用则不应包含
if (frontend->sources.usingGPS()) {
// 等待所有 GPS 单元的配置确认。在此之前，GPS 驱动程序无法提供正确的时间延迟
float gps_delay_sec = 0;
if (!dal.gps().get_lag(selected_gps, gps_delay_sec)) {
#if HAL_GCS_ENABLED
const uint32_t now = dal.millis();
if (now - lastInitFailReport_ms > 10000) {
lastInitFailReport_ms = now;
// 提供逐步增加的消息
MAV_SEVERITY severity = MAV_SEVERITY_INFO;
if (now > 30000) {
severity = MAV_SEVERITY_ERROR;
} else if (now > 15000) {
severity = MAV_SEVERITY_WARNING;
}
GCS_SEND_TEXT(severity, "EKF3 正在等待 GPS 配置数据");
}
#endif
return false;
}
// 将 GPS 库中的时间延迟值限制为最大 250 毫秒，这是 EKF 测试的最大值。
maxTimeDelay_ms = MAX(maxTimeDelay_ms, MIN((uint16_t)(gps_delay_sec * 1000.0f), 250));
}
// 空速感测可能有较大的延迟，如果禁用则不应包含
if (dal.airspeed_sensor_enabled()) {
maxTimeDelay_ms = MAX(maxTimeDelay_ms, frontend->tasDelay_ms);
}
#if HAL_VISUALODOM_ENABLED
// 如果启用了视觉里程计，则包含延迟
const auto *visual_odom = dal.visualodom();
if ((visual_odom != nullptr) && visual_odom->enabled()) {
maxTimeDelay_ms = MAX(maxTimeDelay_ms, MIN(visual_odom->get_delay_ms(), 250));
}
#endif
// 计算 IMU 缓冲区长度以适应最大延迟并考虑抖动
imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_MS)) + 1;
// 设置观测缓冲区长度以处理观测之间的最小到达时间和从当前时间到 EKF 融合时间的最坏情况延迟
// 允许 EKF 融合时间范围延迟的最坏情况 50% 扩展，因定时抖动造成
uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilF((ftype)maxTimeDelay_ms * 0.5f));
obs_buffer_length = (ekf_delay_ms / frontend->sensorIntervalMin_ms) + 1;
// 限制为不超过 IMU 缓冲区（我们无法处理比 EKF 预测速率更快的数据）
obs_buffer_length = MIN(obs_buffer_length, imu_buffer_length);
// 计算光流数据的缓冲区大小
const uint8_t flow_buffer_length = MIN((ekf_delay_ms / frontend->flowIntervalMin_ms) + 1, imu_buffer_length);
#if EK3_FEATURE_EXTERNAL_NAV
// 计算外部导航数据的缓冲区大小
const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length);
#endif // EK3_FEATURE_EXTERNAL_NAV
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(dal.airspeed() && !storedTAS.init(obs_buffer_length)) {
return false;
}
if(dal.opticalflow_enabled() && !storedOF.init(flow_buffer_length)) {
return false;
}
#if EK3_FEATURE_BODY_ODOM
if(frontend->sources.ext_nav_enabled() && !storedBodyOdm.init(obs_buffer_length)) {
return false;
}
if(frontend->sources.wheel_encoder_enabled() && !storedWheelOdm.init(imu_buffer_length)) {
// 初始化为与 IMU 相同长度，以适应多个车轮传感器
return false;
}
#endif // EK3_FEATURE_BODY_ODOM
if(frontend->sources.gps_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
return false;
}
#if AP_RANGEFINDER_ENABLED
// 注意：使用双范围探测器可能会使存储的数据量加倍
if(dal.rangefinder() && !storedRange.init(MIN(2*obs_buffer_length, imu_buffer_length))) {
return false;
}
#endif
// 注意：范围信标数据逐一读取，每个信标的数据到达速度较快
#if EK3_FEATURE_BEACON_FUSION
if(dal.beacon() && !rngBcn.storedRange.init(imu_buffer_length+1)) {
return false;
}
#endif
#if EK3_FEATURE_EXTERNAL_NAV
if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) {
return false;
}
if (frontend->sources.ext_nav_enabled() && !storedExtNavVel.init(extnav_buffer_length)) {
return false;
}
if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
return false;
}
#endif // EK3_FEATURE_EXTERNAL_NAV
if(!storedIMU.init(imu_buffer_length)) {
return false;
}
if(!storedOutput.init(imu_buffer_length)) {
return false;
}
#if EK3_FEATURE_DRAG_FUSION
if (!storedDrag.init(obs_buffer_length)) {
return false;
}
#endif
if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
// 检查是否有足够的内存来创建 EKF-GSF 对象
if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 IMU%u GSF: 内存不足",(unsigned)imu_index);
return false;
}
// 尝试实例化
yawEstimator = NEW_NOTHROW EKFGSF_yaw();
if (yawEstimator == nullptr) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 IMU%u GSF: 分配失败",(unsigned)imu_index);
return false;
}
}
return true;
}
2.2.2 InitialiseFilterBootstrap
滤波器初始化
/*
从加速度计数据初始化状态。
1. 这假设测量到的加速度以重力为主导。如果这个假设不成立，那么扩展卡尔曼滤波器（EKF）将需要时间来减少结果的倾斜误差。
2. 该函数不执行偏航对准，但在倾斜稳定后，通过 SelectMagFusion() 函数进行偏航对准。
*/
bool NavEKF3_core::InitialiseFilterBootstrap(void)
{
// 更新传感器选择（为了亲和性）
update_sensor_selection();
// 如果我们是飞机且没有 GPS 锁定，则不初始化
if (assume_zero_sideslip() && dal.gps().status(preferred_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
dal.snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"EKF3 初始化失败：没有 GPS 锁定");
statesInitialised = false;
return false;
}
// 读取启动 EKF 所需的所有传感器数据
readIMUData(false); // 不允许预测
readMagData();
readGpsData();
readGpsYawData();
readBaroData();
if (statesInitialised) {
// 我们已经初始化，但在 IMU 缓冲区填满之前不返回 true。这可以防止滤波器启动期间 IMU 数据的暂停引起的时间漏洞
return storedIMU.is_filled();
}
// 累积足够的传感器数据以填满缓冲区
if (firstInitTime_ms == 0) {
firstInitTime_ms = imuSampleTime_ms;
return false;
} else if (imuSampleTime_ms - firstInitTime_ms < 1000) {
return false;
}
// 将重复使用的变量初始化为零
InitialiseVariables();
// IMU 测量的 XYZ 机体轴上的加速度向量（m/s^2）
Vector3F initAccVec;
// TODO 我们应该对加速度读数进行多次循环平均
initAccVec = dal.ins().get_accel(accel_index_active).toftype();
// 规范化加速度向量
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.gyro_bias.zero();
stateStruct.accel_bias.zero();
stateStruct.wind_vel.zero();
stateStruct.earth_magfield.zero();
stateStruct.body_magfield.zero();
// 设置位置、速度和高度
ResetVelocity(resetDataSource::DEFAULT);
ResetPosition(resetDataSource::DEFAULT);
ResetHeight();
// 初始化源
posxy_source_last = frontend->sources.getPosXYSource();
yaw_source_last = frontend->sources.getYawSource();
// 定义 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_bias.zero();
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u 已初始化",(unsigned)imu_index);
// 初始返回 false 以等待 IMU 缓冲区填满
return false;
}
2.3 滤波器更新
void UpdateFilter(bool predict);- 使用新的IMU数据更新滤波器状态，并可选择开始预测周期。
2.3.1 UpdateFilter
每当有新的IMU数据可用时都调用更新滤波器状态
void NavEKF3_core::UpdateFilter(bool predict)
{
// 如果状态尚未初始化，则不运行滤波器更新
if (!statesInitialised) {
return;
}
fill_scratch_variables();
// 更新传感器选择（用于亲和性）
update_sensor_selection();
// TODO - 飞行中的重启方法
// 检查臂状态并执行所需的检查和模式更改
controlFilterModes();
// 将IMU数据读取为增量角度和速度
readIMUData(predict);
// 如果缓冲区中有新的IMU数据，运行EKF方程以估计融合时间范围
if (runUpdates) {
// 使用延迟时间范围内的IMU数据预测状态
UpdateStrapdownEquationsNED();
// 预测协方差增长
CovariancePrediction(nullptr);
// 为GSF航向估计器算法运行IMU预测步骤
// 使用IMU和可选的真空速数据。
// 必须在SelectMagFusion()之前运行，以提供最新的航向估计
runYawEstimatorPrediction();
// 使用磁力计或外部航向传感器数据更新状态
SelectMagFusion();
// 使用GPS和高度计数据更新状态
SelectVelPosFusion();
// 为GSF航向估计器算法运行GPS速度修正步骤
// 并使用航向估计来重置主EKF航向（如果请求）
// 必须在SelectVelPosFusion()之后运行，以便使用新鲜的GPS数据
runYawEstimatorCorrection();
#if EK3_FEATURE_BEACON_FUSION
// 使用范围信标数据更新状态
SelectRngBcnFusion();
#endif
// 使用光流数据更新状态
SelectFlowFusion();
#if EK3_FEATURE_BODY_ODOM
// 使用机体框架里程数据更新状态
SelectBodyOdomFusion();
#endif
// 使用空速数据更新状态
SelectTasFusion();
// 使用侧滑约束假设来更新飞行前进的车辆或使用多旋翼的机体阻力
SelectBetaDragFusion();
// 更新滤波器状态
updateFilterStatus();
if (imuSampleTime_ms - last_oneHz_ms >= 1000) {
// 1Hz任务
last_oneHz_ms = imuSampleTime_ms;
moveEKFOrigin();
checkUpdateEarthField();
}
}
// 从融合输出中计算风的前向输出时间范围
calcOutputStates();
/*
这是一个检查，用于应对车辆静止在地面上
并且对状态过于自信的情况。症状是“陀螺仪仍在稳定”
当用户尝试启动时。在这种状态下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, "EKF3 IMU%u 强制重置",(unsigned)imu_index);
last_filter_ok_ms = 0;
statesInitialised = false;
InitialiseFilterBootstrap();
}
}
2.4 滤波器健康和状态
bool healthy(void) const;- 检查滤波器的基本健康状态。
float errorScore(void) const;- 返回表示滤波器健康状态的错误分数。
void getFilterStatus(nav_filter_status &status) const;- 提供详细的滤波器状态。
2.4.1 healthy
检查滤波器的综合健康状态
- 开始1s内默认不健康
- 是否有错误发生
- 速度、高度、位置测试指标均大于1
- 地面静止时，位置和高度创新值必须满足条件
bool NavEKF3_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;
}
// 当处于地面且在静态操作模式下时，位置和高度创新必须在限制范围内
float 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.4.2 errorScore
获取滤波器健康状态的错误分数(较高的数值代表更大的错误)，旨在被前端用于确定哪个是主要的EKF。
float NavEKF3_core::errorScore() const
{
float score = 0.0f;
if (tiltAlignComplete && yawAlignComplete) {
// 检查GPS融合性能
score = MAX(score, 0.5f * (velTestRatio + posTestRatio));
// 检查高度计融合性能
score = MAX(score, hgtTestRatio);
// 检查空速融合性能 - 仅在使用至少两个空速传感器时才进行，这样我们可以用更好的传感器进行切换。
// 这只在前向飞行的飞行器中生效。增加0.3的灵敏度因子，以使EKF对如强风等事件引起的创新不那么敏感，从而防止报告高错误得分
if (assume_zero_sideslip()) {
const auto *arsp = dal.airspeed();
if (arsp != nullptr && arsp->get_num_sensors() >= 2 && (frontend->_affinity & EKF_AFFINITY_ARSP)) {
score = MAX(score, 0.3f * tasTestRatio);
}
}
// 检查磁力计融合性能 - 需要在启用磁力计亲和力时进行，以覆盖固有的指南针切换机制，从而能够移动到更好的通道
if (frontend->_affinity & EKF_AFFINITY_MAG) {
score = MAX(score, 0.3f * (magTestRatio.x + magTestRatio.y + magTestRatio.z));
}
}
return score;
}
2.4.3 getFilterStatus
获取滤波器状态
// Return the navigation filter status message
void NavEKF3_core::getFilterStatus(nav_filter_status &status) const
{
status = filterStatus;
}
2.5 位置和速度
bool getPosNE(Vector2f &posNE) const;- 获取最后计算的东北（NE）位置。
bool getPosD_local(float &posD) const;- 获取本地垂直位置。
bool getPosD(float &posD) const;- 获取最后计算的垂直（D）位置。
void getVelNED(Vector3f &vel) const;- 返回东北下（NED）速度。
bool getAirSpdVec(Vector3f &vel) const;- 估计真实空气速度向量。
float getPosDownDerivative(void) const;- 提供垂直位置变化率。
2.5.1 getPosNE
获取NE坐标系下的位置
// 写入体坐标系原点相对于参考点的最后估计的NE位置（米）。
// 如果估计值有效，则返回true
bool NavEKF3_core::getPosNE(Vector2f &posNE) const
{
// 有三种操作模式：绝对位置（GPS融合），相对位置（光流融合）和固定位置（没有位置估计可用）
if (PV_AidingMode != AID_NONE) {
// 这是正常的操作模式，我们可以使用EKF位置状态
// 对IMU偏差进行修正（EKF计算是在IMU上进行的）
posNE = (outputDataNew.position.xy() + posOffsetNED.xy() + public_origin.get_distance_NE_ftype(EKF_origin)).tofloat();
return true;
} else {
// 在固定位置模式下，EKF位置状态位于原点，因此我们不能将其用作位置估计
if (validOrigin) {
auto &gps = dal.gps();
if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) {
// 如果原点已设置并且我们有GPS，则返回相对于原点的GPS位置
const Location &gpsloc = gps.location(selected_gps);
posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat();
return false;
#if EK3_FEATURE_BEACON_FUSION
} else if (rngBcn.alignmentStarted) {
// 如果我们正在尝试使用范围信标数据进行对准，则报告位置
posNE.x = rngBcn.receiverPos.x;
posNE.y = rngBcn.receiverPos.y;
return false;
#endif
} else {
// 如果没有GPS定位，则只能提供最后已知的位置
posNE = outputDataNew.position.xy().tofloat();
return false;
}
} else {
// 如果原点尚未设置，则我们无法提供相对位置
posNE.zero();
return false;
}
}
return false;
}
2.5.2 getPosD_local
获取本地垂直高度
// 写入相对于 EKF 本地原点的机体框架原点的最后计算 D 位置
// 如果估计有效则返回 true
bool NavEKF3_core::getPosD_local(float &posD) const
{
posD = outputDataNew.position.z + posOffsetNED.z;
// 返回当前高度解状态
return filterStatus.flags.vert_pos;
}
2.5.3 getPosD
获取垂直高度
// 写入相对于公共原点的机体框架原点的最后计算的 D 位置
// 如果估计有效，则返回 true
bool NavEKF3_core::getPosD(float &posD) const
{
bool ret = getPosD_local(posD);
// 调整 posD 以考虑我们原点与公共原点之间的差异
Location local_origin;
if (getOriginLLH(local_origin)) {
posD += (public_origin.alt - local_origin.alt) * 0.01;
}
return ret;
}
2.5.4 getVelNED
获取NED坐标系下速度矢量
// 返回机体坐标系原点的NED速度，单位为米每秒
void NavEKF3_core::getVelNED(Vector3f &vel) const
{
// 校正IMU位置偏移（EKF计算是在IMU处进行的）
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
}
2.5.5 getAirSpdVec
获取空速计速度矢量
// 返回在机体坐标系中真实空速矢量的估计值，单位是 m/s
// 如果估计值不可用，则返回 false
bool NavEKF3_core::getAirSpdVec(Vector3f &vel) const
{
if (PV_AidingMode == AID_NONE) {
return false;
}
vel = (outputDataNew.velocity + velOffsetNED).tofloat();
if (!inhibitWindStates) {
vel.x -= stateStruct.wind_vel.x;
vel.y -= stateStruct.wind_vel.y;
}
Matrix3f Tnb; // 从导航坐标系到机体坐标系的旋转矩阵
outputDataNew.quat.inverse().rotation_matrix(Tnb);
vel = Tnb * vel;
return true;
}
2.5.6 getPosDownDerivative
获取垂直位置变化率
// 返回身体框架原点在向下方向上的垂直位置变化率 (dPosD/dt)，单位是米/秒
float NavEKF3_core::getPosDownDerivative(void) const
{
// 返回通过对 EKF 高度和垂直加速度应用互补滤波器计算的值
// 对 IMU 偏移进行校正 (EKF 计算是在 IMU 处进行的)
return vertCompFiltState.vel + velOffsetNED.z;
}
2.6 偏差和校准
void getGyroBias(Vector3f &gyroBias) const;- 获取机体轴向陀螺仪偏差估计值。
void getAccelBias(Vector3f &accelBias) const;- 获取加速度计偏差估计值。
void resetGyroBias(void);- 重置陀螺仪偏差估计值。
2.6.1 getGyroBias
获取机体坐标系陀螺仪偏差估计值(rad/sec)
void NavEKF3_core::getGyroBias(Vector3f &gyroBias) const
{
if (dtEkfAvg < 1e-6f) {
gyroBias.zero();
return;
}
gyroBias = (stateStruct.gyro_bias / dtEkfAvg).tofloat();
}
2.6.2 getAccelBias
获取加速度计偏差估计值(m/s/s)
void NavEKF3_core::getAccelBias(Vector3f &accelBias) const
{
if (!statesInitialised) {
accelBias.zero();
return;
}
accelBias = (stateStruct.accel_bias / dtEkfAvg).tofloat();
}
2.6.3 resetGyroBias
重置陀螺仪偏差估计值
// 将机体轴陀螺仪偏置状态重置为零，并重新初始化相应的协方差
// 假设校准精度为0.5度/秒，需要在静态条件下进行平均
// 注意 - 必须使用非阻塞校准方法
void NavEKF3_core::resetGyroBias(void)
{
stateStruct.gyro_bias.zero();
zeroRows(P, 10, 12);
zeroCols(P, 10, 12);
P[10][10] = sq(radians(0.5f * dtIMUavg));
P[11][11] = P[10][10];
P[12][12] = P[10][10];
}
2.7 磁力计和传感器
void getMagNED(Vector3f &magNED) const;- 获取NED坐标系中的地球磁场估计值。
void getMagXYZ(Vector3f &magXYZ) const;- 获取机体磁场估计值。
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;- 返回估计的磁力计偏移值。
2.7.1 getMagNED
获取NED坐标系中的地球磁场估计值（单位为测量单位/1000）
void NavEKF3_core::getMagNED(Vector3f &magNED) const
{
magNED = (stateStruct.earth_magfield * 1000.0f).tofloat();
}
2.7.2 getMagXYZ
获取机体坐标系磁场估计值（单位为测量单位/1000）
void NavEKF3_core::getMagXYZ(Vector3f &magXYZ) const
{
magXYZ = (stateStruct.body_magfield*1000.0f).tofloat();
}
2.7.3 getMagOffsets
获取估计的磁力计偏移值
bool NavEKF3_core::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
{
const auto &compass = dal.compass();
if (!compass.available()) {
return false; // 如果罗盘不可用，则返回 false
}
// 如果完成了磁场初始化、未禁止磁场学习、
// 主罗盘有效并且状态方差已收敛，则磁力计偏移量有效
const float maxMagVar = 5E-6f;
bool variancesConverged = (P[19][19] < maxMagVar) && (P[20][20] < maxMagVar) && (P[21][21] < maxMagVar);
if ((mag_idx == magSelectIndex) &&
finalInflightMagInit &&
!inhibitMagStates &&
compass.healthy(magSelectIndex) &&
variancesConverged) {
magOffsets = compass.get_offsets(magSelectIndex) - stateStruct.body_magfield.tofloat()*1000.0;
return true; // 返回 true 表示偏移量有效
} else {
magOffsets = compass.get_offsets(magSelectIndex);
return false; // 返回 false 表示偏移量无效
}
}
2.8 光流和里程计
void writeOptFlowMeas(...)- 写入原始光流测量数据。
void writeBodyFrameOdom(...)- 写入来自视觉里程计的线性和角位移测量数据。
void writeWheelOdom(...)- 写入来自轮子编码器的里程计数据。
2.8.1 writeOptFlowMeas
注入原始光流测量数据
注：传感器采用右手旋转法则。
// rawFlowQuality 是质量的测量值，范围从 0 到 255，255 是最佳质量
// rawFlowRates 是关于 X 和 Y 传感器轴的光流速率，以弧度/秒为单位
// rawGyroRates 是传感器内部陀螺仪测得的传感器旋转速率，以弧度/秒为单位
// msecFlowMeas 是从传感器接收到光流数据时的调度器时间，以毫秒为单位
// posOffset 是身体框架中 XYZ 流传感器的位置偏移，以米为单位
// heightOverride 是传感器在地面以上的固定高度，以米为单位，用于车载车辆。不使用时为 0
void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
{
// 限制更新速率到传感器缓冲区允许的最大值
if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
return;
}
// 原始测量值需要是光流速率，以弧度/秒为单位，平均自上次更新以来的时间
// PX4Flow 传感器输出流速率的轴和符号约定如下：
// 正 X 速率由传感器绕 X 轴的正旋转产生
// 正 Y 速率由传感器绕 Y 轴的正旋转产生
// 此滤波器使用与传感器不同的光流速率定义，正的光流速率由绕该轴的负旋转产生。例如，飞行器绕其 X（滚转）轴的正旋转将产生负的 X 流速率
flowMeaTime_ms = imuSampleTime_ms;
// 计算流传感器陀螺仪速率的偏差错误，但要防止数据中的尖峰
// 重置累积的身体增量角度和时间
// 如果没有足够的时间进行可靠的身体速率测量，则不进行计算
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// 根据定义，如果调用此函数，则提供了流测量值，因此我们需要运行光流起飞检测
detectOptFlowTakeoff();
// 不使用质量指示器低或极端速率的数据（有助于捕获损坏的传感器数据）
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// 校正流传感器身体速率的偏差并写入
of_elements ofDataNew {};
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
// 传感器接口未提供 Z 轴速率，因此使用导航传感器的速率
if (delTimeOF > 0.001f) {
// 优先使用与流传感器相同采样周期的平均速率
ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
} else if (imuDataNew.delAngDT > 0.001f){
// 次优先使用最近的 IMU 数据
ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
} else {
// 最后优先使用零
ofDataNew.bodyRadXYZ.z = 0.0f;
}
// 写入未经校正的流速率测量
// 注意校正 PX4Flow 传感器使用的不同轴和符号约定
ofDataNew.flowRadXY = - rawFlowRates.toftype(); // 原始（非运动补偿）光流角速率关于 X 轴（弧度/秒）
// 写入身体框架中的流传感器位置
ofDataNew.body_offset = posOffset.toftype();
// 写入流传感器高度覆盖
ofDataNew.heightOverride = heightOverride;
// 写入经过身体速率校正的流速率测量
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
// 记录上次观察接收的时间，以便我们可以在其他地方检测数据丢失
flowValidMeaTime_ms = imuSampleTime_ms;
// 估算测量的采样时间
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
// 校正因滤波器更新速率导致的平均采样延迟
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
// 防止时间延迟超过缓冲区中最老 IMU 数据的年龄
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
// 将数据保存到缓冲区
storedOF.push(ofDataNew);
}
}
2.8.2 writeBodyFrameOdom
注入视觉里程计的线性和角位移测量数据(机体坐标系)
// quality 是一个从 0 到 100 的标准化置信值
// delPos 是在机体框架中测量的线性位置的 XYZ 变化，相对于惯性参考系在 time_ms 的位置（米）
// delAng 是在机体框架中测量的角旋转的 XYZ 变化，相对于惯性参考系在 time_ms 的位置（弧度）
// delTime 是 delPos 和 delAng 的测量时间间隔（秒）
// timeStamp_ms 是用于计算 delPos 和 delAng 的最后一张图像的时间戳（毫秒）
// delay_ms 是外部导航系统测量相对于惯性测量的平均延迟
// posOffset 是摄像机焦点在机体框架中的 XYZ 位置（米）
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
{
#if EK3_FEATURE_BODY_ODOM
// 防止 NaN
if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
return;
}
// 限制更新率到传感器缓冲区和融合过程允许的最大值
// 在滤波器初始化之前不尝试写入缓冲区
if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
return;
}
// 从时间戳中减去延迟
timeStamp_ms -= delay_ms;
bodyOdmDataNew.body_offset = posOffset.toftype();
bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms;
bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
bodyOdmMeasTime_ms = timeStamp_ms;
// 精度的简单模型
// TODO 将此计算从 EKF 移到传感器驱动程序中
bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);
storedBodyOdm.push(bodyOdmDataNew);
#endif // EK3_FEATURE_BODY_ODOM
}
2.8.3 writeWheelOdom
注入轮子编码器的里程计数据(机体坐标系)
注：旋转轴假设与车辆主体轴平行。
// delAng 是与前一次测量相比测得的角位置变化，其中前进运动产生正旋转（弧度）。
// delTime 是 delAng 的测量时间间隔（秒）。
// timeStamp_ms 是上次测量旋转时的时间（毫秒）。
// posOffset 是轮毂在车身坐标系中的 XYZ 位置（米）。
// radius 是轮子的有效滚动半径（米）。
void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
{
#if EK3_FEATURE_BODY_ODOM
// 这是将轮编码器数据导入 EKF 的简单方法，并验证接口的符号约定和单位。
// 它使用现有的车身坐标系速度融合。
// TODO 实现专用的轮式里程观测模型。
// 调用方应将频率限制为 50hz。
// 将更新速率限制为传感器缓冲区和融合过程允许的最大值。
// 在滤波器初始化之前，不要尝试写入缓冲区。
if ((delTime < dtEkfAvg) || !statesInitialised) {
return;
}
wheel_odm_elements wheelOdmDataNew = {};
wheelOdmDataNew.hub_offset = posOffset.toftype();
wheelOdmDataNew.delAng = delAng;
wheelOdmDataNew.radius = radius;
wheelOdmDataNew.delTime = delTime;
// 因为我们目前在融合前将其转换为等效的速度测量
// 所以测量时间移到了采样周期的中间。
wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
storedWheelOdm.push(wheelOdmDataNew);
#endif // EK3_FEATURE_BODY_ODOM
}
2.9 外部导航数据
void writeExtNavData(...)- 写入来自外部导航系统的位置和四元数数据。
void writeExtNavVelData(...)- 写入来自外部导航系统的速度数据。
2.9.1 writeExtNavData
从外部导航系统写入位置和四元数数据
// pos : 右手导航框架中的位置。如果 frameIsNED 为 true，则假定框架为 NED。（米）
// quat : 描述从导航框架到机体框架旋转的四元数
// posErr : 1-σ 球面位置误差（米）
// angErr : 1-σ 球面角误差（弧度）
// timeStamp_ms : 测量进行时的系统时间，而不是接收时间（毫秒）
// delay_ms : 相对于惯性测量的外部导航系统测量的平均延迟
// resetTime_ms : 上次位置重置请求的系统时间（毫秒）
void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
{
#if EK3_FEATURE_EXTERNAL_NAV
// 防止出现 NaN（非数字）
if (pos.is_nan() || isnan(posErr)) {
return;
}
// 限制更新速率以适应传感器缓冲区和融合过程
// 在滤波器初始化之前不写入缓冲区
if (((timeStamp_ms - extNavMeasTime_ms) < frontend->extNavIntervalMin_ms) || !statesInitialised) {
return;
} else {
extNavMeasTime_ms = timeStamp_ms;
}
ext_nav_elements extNavDataNew {};
// 检查位置重置时间是否改变
if (resetTime_ms != extNavLastPosResetTime_ms) {
extNavDataNew.posReset = true;
extNavLastPosResetTime_ms = resetTime_ms;
} else {
extNavDataNew.posReset = false;
}
// 设置新位置和误差
extNavDataNew.pos = pos.toftype();
extNavDataNew.posErr = posErr;
// 计算时间戳
timeStamp_ms = timeStamp_ms - delay_ms;
// 校正由于滤波器更新速率造成的平均采样延迟
timeStamp_ms -= localFilterTimeStep_ms / 2;
// 防止时间延迟超过缓冲区中最旧 IMU 数据的年龄
timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
extNavDataNew.time_ms = timeStamp_ms;
// 将位置数据存储到缓冲区
storedExtNav.push(extNavDataNew);
// 防止姿态或角度为 NaN
if (!quat.is_nan() && !isnan(angErr)) {
// 从姿态中提取偏航角
ftype roll_rad, pitch_rad, yaw_rad;
quat.to_euler(roll_rad, pitch_rad, yaw_rad);
yaw_elements extNavYawAngDataNew;
extNavYawAngDataNew.yawAng = yaw_rad;
extNavYawAngDataNew.yawAngErr = MAX(angErr, radians(5.0f)); // 确保偏航精度不超过 5 度（某些调用者可能会传入零）
extNavYawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; // 欧拉旋转顺序为 321 (ZYX)
extNavYawAngDataNew.time_ms = timeStamp_ms;
storedExtNavYawAng.push(extNavYawAngDataNew);
}
#endif // EK3_FEATURE_EXTERNAL_NAV
}
2.9.2 writeExtNavVelData
从外部导航系统写入速度数据
// vel : NED坐标系下的速度（米/秒）
// err : 速度误差（米/秒）
// timeStamp_ms : 测量的系统时间，而不是接收时间（毫秒）
// delay_ms : 外部导航系统测量相对于惯性测量的平均延迟
void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
{
#if EK3_FEATURE_EXTERNAL_NAV
// 对NaN进行合理性检查
if (vel.is_nan() || isnan(err)) {
return;
}
// 如果当前时间戳与上一次测量时间戳之差小于最小间隔，则不处理
if ((timeStamp_ms - extNavVelMeasTime_ms) < frontend->extNavIntervalMin_ms) {
return;
}
extNavVelMeasTime_ms = timeStamp_ms;
useExtNavVel = true;
// 计算时间戳
timeStamp_ms = timeStamp_ms - delay_ms;
// 根据滤波器更新率修正平均采样延迟
timeStamp_ms -= localFilterTimeStep_ms/2;
// 防止时间延迟超过缓冲区中最旧IMU数据的时间
timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
ext_nav_vel_elements extNavVelNew;
extNavVelNew.time_ms = timeStamp_ms;
extNavVelNew.vel = vel.toftype();
extNavVelNew.err = err;
extNavVelNew.corrected = false;
storedExtNavVel.push(extNavVelNew);
#endif // EK3_FEATURE_EXTERNAL_NAV
}
2.10 高度数据
- `bool resetHeightDatum(void);`
- 使用气压计数据重置高度数据。
- `void setTerrainHgtStable(bool val);`
- 设置地形高度是否稳定以供测距仪使用。
2.10.1 resetHeightDatum
将气压计重置为当前高度读数为零(将EKF的高度重置为零)
// 调整EKF原点高度，以使EKF高度加上原点高度与重置前相同
// 如果高度数据重置已执行，则返回true
// 如果使用测距仪来测量高度，则不执行重置并返回false
bool NavEKF3_core::resetHeightDatum(void)
{
if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER || !onGround) {
// 只有在地面上时才允许重置。
// 如果使用距离传感器来测量高度，则永远不要重置高度基准
return false;
}
// 记录旧的高度估计值
ftype oldHgt = -stateStruct.position.z;
// 重置气压计，使其在当前高度时读数为零
dal.baro().update_calibration();
// 重置高度状态
stateStruct.position.z = 0.0f;
// 调整 EKF 原点的高度，以便重置前后的气压高度相同
if (validOrigin) {
if (!gpsGoodToAlign) {
// 如果我们没有 GPS 锁定，则不应该进行
// resetHeightDatum，但如果有的话，最佳选项是
// 维护旧的误差
EKF_origin.alt += (int32_t)(100.0f * oldHgt);
} else {
// 如果我们有良好的 GPS 锁定，则重置为 GPS
// 高度。这确保了从 getLLH() 获取的 AMSL 高度
// 等于 GPS 高度，同时也确保了相对高度为零
EKF_origin.alt = dal.gps().location().alt;
}
ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
}
// 将地形状态设置为零（地面）。框架高度的调整
// 将在后续约束中添加
terrainState = 0;
return true;
}
2.10.2 setTerrainHgtStable
设置地形高度是否稳定以供测距仪使用
// 如果地形足够稳定，可以用作高度参考（与测距仪结合使用），则设置为 true
// 如果下方的地形不能用作高度参考，则设置为 false。
// 用于防止测距仪的操作，否则会受到 EK3_RNG_USE_HGT 和 EK3_RNG_USE_SPD 参数组合的影响。
void NavEKF3_core::setTerrainHgtStable(bool val)
{
terrainHgtStable = val;
}
2.11 调试和报告
- `void Log_Write(uint64_t time_us);`
- 在特定时间戳记录当前状态。
- `const char *prearm_failure_reason(void) const;`
- 报告初始化失败的原因。
2.11.1 Log_Write
void NavEKF3_core::Log_Write(uint64_t time_us)
{
const auto level = frontend->_log_level;
if (level == NavEKF3::LogLevel::NONE) { // 不记录任何日志（由 EK3_LOG_LEVEL 参数指定）
return;
}
Log_Write_XKF4(time_us);
if (level == NavEKF3::LogLevel::XKF4) { // 只记录 XKF4 缩放创新
return;
}
Log_Write_GSF(time_us);
if (level == NavEKF3::LogLevel::XKF4_GSF) { // 只记录 XKF4 缩放创新和 GSF，否则记录所有信息
return;
}
// 注意，这些函数中的几个在没有尝试记录主要核心时会提前退出。
Log_Write_XKF1(time_us);
Log_Write_XKF2(time_us);
Log_Write_XKF3(time_us);
Log_Write_XKF5(time_us);
Log_Write_XKFS(time_us);
Log_Write_Quaternion(time_us);
#if EK3_FEATURE_BEACON_FUSION
// 如果范围值非零，则写入范围信标融合调试包
Log_Write_Beacon(time_us);
#endif
#if EK3_FEATURE_BODY_ODOM
// 记录身体框架里程计融合的调试数据
Log_Write_BodyOdom(time_us);
#endif
// 每 0.49 秒记录一次状态方差
Log_Write_State_Variances(time_us);
Log_Write_Timing(time_us);
}
还有些不同封装的log日志接口：
void Log_Write_XKF1(uint64_t time_us) const;
void Log_Write_XKF2(uint64_t time_us) const;
void Log_Write_XKF3(uint64_t time_us) const;
void Log_Write_XKF4(uint64_t time_us) const;
void Log_Write_XKF5(uint64_t time_us) const;
void Log_Write_XKFS(uint64_t time_us) const;
void Log_Write_Quaternion(uint64_t time_us) const;
void Log_Write_Beacon(uint64_t time_us);
void Log_Write_BodyOdom(uint64_t time_us);
void Log_Write_State_Variances(uint64_t time_us);
void Log_Write_Timing(uint64_t time_us);
void Log_Write_GSF(uint64_t time_us);
2.11.2 prearm_failure_reason
报告后端拒绝初始化的原因
const char *NavEKF3_core::prearm_failure_reason(void) const
{
if (gpsGoodToAlign) {
// 我们没有失败
return nullptr;
}
return prearm_fail_string;
}
2.12 其他接口
bool isVibrationAffected() const- 检查状态估计是否受到振动影响。
bool have_aligned_tilt(void) const- 检查是否完成了倾斜对准。
bool have_aligned_yaw(void) const- 检查是否完成了偏航对准。
const EKFGSF_yaw *get_yawEstimator(void) const- 获取一个偏航估计器实例。
2.12.1 isVibrationAffected
// returns true when the state estimates are significantly degraded by vibration
bool isVibrationAffected() const { return badIMUdata; }
2.12.2 have_aligned_tilt
// return true if we are tilt aligned
bool have_aligned_tilt(void) const {
return tiltAlignComplete;
}
2.12.3 have_aligned_yaw
// return true if we are yaw aligned
bool have_aligned_yaw(void) const {
return yawAlignComplete;
}
2.12.4 get_yawEstimator
// get a yaw estimator instance
const EKFGSF_yaw *get_yawEstimator(void) const { return yawEstimator; }
3. 重要例程
3.1 readIMUData
/*
* 读取IMU的角度增量和速度增量测量值，并下采样至100Hz以用于EKF的数据缓冲区。
* 如果IMU数据的到达率低于100Hz，则不进行下采样或上采样。
* 下采样的方法不会引入锥形或滑动误差。
*/
void NavEKF3_core::readIMUData(bool startPredictEnabled)
{
const auto &ins = dal.ins();
// 使用尖峰和低通滤波器组合计算IMU更新速率的平均值
dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(), 0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
// IMU采样时间在整个滤波器中用作共同时间参考
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
uint8_t accel_active, gyro_active;
if (ins.use_accel(imu_index)) {
accel_active = imu_index;
} else {
accel_active = ins.get_first_usable_accel();
}
if (ins.use_gyro(imu_index)) {
gyro_active = imu_index;
} else {
gyro_active = ins.get_first_usable_gyro();
}
if (gyro_active != gyro_index_active) {
// 如果在运行时切换活跃陀螺仪，复制之前非活跃陀螺仪的已学习偏差
stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
gyro_index_active = gyro_active;
}
if (accel_active != accel_index_active) {
// 切换到此IMU的已学习加速度计偏差
stateStruct.accel_bias = inactiveBias[accel_active].accel_bias;
accel_index_active = accel_active;
}
// 更新非活跃偏差状态
learnInactiveBiases();
// 使用IMU数据进行运动检查
updateMovementCheck();
readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
imuDataNew.accel_index = accel_active;
// 从主陀螺仪获取角度增量数据
readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
imuDataNew.delAngDT = MAX(imuDataNew.delAngDT, 1.0e-4f);
imuDataNew.gyro_index = gyro_active;
// 获取当前时间戳
imuDataNew.time_ms = imuSampleTime_ms;
// 累积速度和角度数据的测量时间间隔
imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
// 使用最近的IMU索引进行下采样
imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
imuDataDownSampledNew.accel_index = imuDataNew.accel_index;
// 旋转四元数姿态并规范化
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
imuQuatDownSampleNew.normalize();
// 将最新的速度增量旋转到累积开始时的机身坐标系中
Matrix3F deltaRotMat;
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
// 应用速度增量到累积器
imuDataDownSampledNew.delVel += deltaRotMat * imuDataNew.delVel;
// 记录自上次状态预测以来的IMU帧数
framesSincePredict++;
/*
* 如果目标EKF时间步已累积，且前端允许开始新预测循环，则存储累积的IMU数据用于状态预测。
*/
if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5f)) && startPredictEnabled) ||
(imuDataDownSampledNew.delAngDT >= 2.0f * EKF_TARGET_DT)) {
// 将累积的四元数转换为等效的角度增量
imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
// 给数据加时间戳
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
// 将数据写入FIFO IMU缓冲区
storedIMU.push_youngest_element(imuDataDownSampledNew);
// 使用尖峰和低通滤波器计算EKF的平均时间步率
ftype dtNow = constrain_ftype(0.5f * (imuDataDownSampledNew.delAngDT + imuDataDownSampledNew.delVelDT), 0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
// 额外的下采样用于采样XY机身框架的阻力特定力
SampleDragData(imuDataDownSampledNew);
// 清零累积的IMU数据和四元数
imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero();
imuDataDownSampledNew.delAngDT = 0.0f;
imuDataDownSampledNew.delVelDT = 0.0f;
imuQuatDownSampleNew[0] = 1.0f;
imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
// 重置前端计数器
framesSincePredict = 0;
// 设置标志以通知滤波器有新IMU数据需要运行
runUpdates = true;
// 从FIFO缓冲区提取最早的数据
imuDataDelayed = storedIMU.get_oldest_element();
// 防止增量时间变为零
ftype minDT = 0.1f * dtEkfAvg;
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT, minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT, minDT);
updateTimingStatistics();
// 校正提取的IMU数据的传感器误差
delAngCorrected = imuDataDelayed.delAng;
delVelCorrected = imuDataDelayed.delVel;
correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);
} else {
// 没有新的IMU数据，不在此时间步运行滤波器更新
runUpdates = false;
}
}
3.2 readDeltaVelocity
// 从IMU读取变化的速度（delta velocity）和对应的时间间隔
// 如果数据不可用，返回false
bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
const auto &ins = dal.ins(); // 获取IMU实例
if (ins_index < ins.get_accel_count()) { // 检查索引是否在加速度计数量范围内
Vector3f dVelF; // 用于存储变化速度的临时变量
float dVel_dtF; // 用于存储时间间隔的临时变量
ins.get_delta_velocity(ins_index, dVelF, dVel_dtF); // 从IMU获取变化速度和时间间隔
dVel = dVelF.toftype(); // 转换数据类型
dVel_dt = dVel_dtF; // 赋值时间间隔
dVel_dt = MAX(dVel_dt,1.0e-4); // 确保时间间隔不小于1.0e-4
return true; // 成功读取数据
}
return false; // 数据不可用
}
3.3 readGpsData
// 检查新的有效GPS数据，如果可用则更新存储的测量值
void NavEKF3_core::readGpsData()
{
// 检查新GPS数据
const auto &gps = dal.gps();
// 限制更新速率以避免溢出FIFO缓冲区
if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
return;
}
// 检查GPS状态是否为3D固定
if (gps.status(selected_gps) < AP_DAL_GPS::GPS_OK_FIX_3D) {
// 报告GPS固定状态
gpsCheckStatus.bad_fix = true;
dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
return;
}
// 报告GPS固定状态
gpsCheckStatus.bad_fix = false;
// 存储上一次读取的固定时间
const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// 获取当前固定时间
lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps);
// 估算GPS固定时间，考虑处理和其他延迟
// 理想情况下应该使用GPS接收器的定时信号来设置此时间
float gps_delay_sec = 0;
gps.get_lag(selected_gps, gps_delay_sec);
gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
// 校正滤波器更新速率导致的平均采样延迟
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
// 防止时间戳超出缓冲区内IMU数据的最早和最新时间
gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
// 获取用于位置信息的GPS
gpsDataNew.sensor_idx = selected_gps;
// 从GPS读取NED速度
gpsDataNew.vel = gps.velocity(selected_gps).toftype();
gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
// 位置和速度尚未校正传感器位置
gpsDataNew.corrected = false;
// 使用GPS的速度和位置精度，如果不可用则设置为零
// 对原始精度数据应用5秒时间常数的衰减包络滤波器
ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
}
gpsPosAccuracy *= (1.0f - alpha);
float gpsPosAccRaw;
if (!gps.horizontal_accuracy(selected_gps, gpsPosAccRaw)) {
gpsPosAccuracy = 0.0f;
} else {
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
}
gpsHgtAccuracy *= (1.0f - alpha);
float gpsHgtAccRaw;
if (!gps.vertical_accuracy(selected_gps, gpsHgtAccRaw)) {
gpsHgtAccuracy = 0.0f;
} else {
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
}
// 检查是否有足够的GPS卫星，如果没有则增加GPS噪声比例
if (gps.num_sats(selected_gps) >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (gps.num_sats(selected_gps) == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4卫星或在固定位置模式
gpsNoiseScaler = 2.0f;
}
// 检查GPS是否能输出垂直速度，垂直速度使用被允许并相应设置GPS融合模式
if (gpsDataNew.have_vz && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS)) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
}
if ((frontend->_options & (int32_t)NavEKF3::Options::JammingExpected) &&
(lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) {
const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
const bool canDoWindRelNav = assume_zero_sideslip();
const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav);
if (canDeadReckon) {
// 如果可以使用除GPS以外的数据源进行推算航行，则可以等待GPS对齐检查通过
// 在EKF中使用GPS前，这在calcGpsGoodToAlign()中设置回false
waitingForGpsChecks = true;
// 强制GPS检查重启
lastPreAlignGpsCheckTime_ms = 0;
lastGpsVelFail_ms = imuSampleTime_ms;
lastGpsVelPass_ms = 0;
gpsGoodToAlign = false;
} else {
waitingForGpsChecks = false;
}
}
// 在对齐前后监测GPS速度数据的质量
calcGpsGoodToAlign();
// 对齐后检查
calcGpsGoodForFlight();
// 读取WGS-84纬度、经度、高度坐标的GPS位置
const Location &gpsloc = gps.location(selected_gps);
// 如果未先前设置并且GPS检查通过，则设置EKF原点和磁场偏角
if (gpsGoodToAlign && !validOrigin) {
Location gpsloc_fieldelevation = gpsloc;
// 如果在飞行中，则校正起飞高度的变化以使原点在地面高度
if (inFlight) {
gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
}
if (!setOrigin(gpsloc_fieldelevation)) {
// 设置一个错误，因为尝试多次设置原点
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
return;
}
// 使用已发布的偏角设置NE地磁场状态
// 设置相应的方差和协方差
alignMagStateDeclination();
// 设置NED原点的高度
ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;
// 设置GPS原点高度的不确定性
ekfOriginHgtVar = sq(gpsHgtAccuracy);
}
if (gpsGoodToAlign && !have_table_earth_field) {
setEarthFieldFromLocation(gpsloc);
}
// 如果有有效原点且不在等待GPS检查通过，则将GPS测量值转换为本地NED并保存到缓冲区以供以后融合
if (validOrigin && !waitingForGpsChecks) {
gpsDataNew.lat = gpsloc.lat;
gpsDataNew.lng = gpsloc.lng;
if ((frontend->_originHgtMode & (1<<2)) == 0) {
// 通过调整原点高度实现与GPS匹配的高度调整
gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else {
// 通过调整测量值实现与GPS匹配的高度调整
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
}
storedGPS.push(gpsDataNew);
// 声明GPS正在使用中
gpsIsInUse = true;
}
}
3.4 readGpsYawData
// 检查新的有效GPS航向数据
void NavEKF3_core::readGpsYawData()
{
const auto &gps = dal.gps();
// 如果GPS有航向数据，则将其作为欧拉航向角融合
float yaw_deg, yaw_accuracy_deg;
uint32_t yaw_time_ms;
if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D &&
dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg, yaw_time_ms) &&
yaw_time_ms != yawMeasTime_ms) {
// GPS模块对于它们的精度过于乐观。在这里设置为最小5度，以防止
// 用户不断收到关于高标准化航向创新的警告
const ftype min_yaw_accuracy_deg = 5.0f;
yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), yaw_time_ms, 2);
}
}
3.5 readDeltaAngle
// 从IMU读取角度增量和相应的时间间隔
// 如果数据不可用则返回false
bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) {
const auto &ins = dal.ins();
// 检查陀螺仪数量是否大于索引
if (ins_index < ins.get_gyro_count()) {
Vector3f dAngF;
float dAngDTF;
// 获取角度增量和时间间隔
ins.get_delta_angle(ins_index, dAngF, dAngDTF);
dAng = dAngF.toftype(); // 转换类型
dAngDT = dAngDTF;
return true; // 数据可用
}
return false; // 数据不可用
}
3.6 readBaroData
// 检查是否有新的气压高度测量数据，如果有则更新存储的测量值
void NavEKF3_core::readBaroData()
{
// 检查气压测量是否已改变，以确定是否有新的测量到达
// 限制更新频率以避免溢出FIFO缓冲区
const auto &baro = dal.baro();
if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
// 获取新的气压高度数据
baroDataNew.hgt = baro.get_altitude(selected_baro);
// 更新气压数据的时间戳，用于检查是否有新的测量值
lastBaroReceived_ms = baro.get_last_update(selected_baro);
// 估计高度测量的时间，考虑到延迟
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
// 修正由于滤波器更新频率导致的平均采样延迟
baroDataNew.time_ms -= localFilterTimeStep_ms / 2;
// 防止时间延迟超过缓冲区中最旧IMU数据的时间
baroDataNew.time_ms = MAX(baroDataNew.time_ms, imuDataDelayed.time_ms);
// 将气压测量数据保存到缓冲区，以便稍后融合
storedBaro.push(baroDataNew);
}
}
3.7 readMagData
// 检查是否有新的磁力计数据，并在数据可用时更新存储的测量值
void NavEKF3_core::readMagData()
{
const auto &compass = dal.compass();
if (!compass.available()) {
allMagSensorsFailed = true;
return;
}
// 如果我们是具有侧滑约束的车辆以帮助偏航估计，并且我们在最后一个可用磁力计上超时，
// 则声明这些磁力计在这次飞行中失败
const uint8_t maxCount = compass.get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
if (compass.learn_offsets_enabled()) {
wasLearningCompass_ms = imuSampleTime_ms;
} else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
// 允许旧数据清除缓冲区，然后再通知其他代码可以使用磁力计数据
wasLearningCompass_ms = 0;
}
// 如果磁力计超时（被拒绝的时间过长），我们会寻找其他可用的磁力计
// 如果我们在地面上不要这样做，因为可能会有磁场干扰，我们需要在起飞前知道是否存在问题
// 不要在启动后的前30秒内这样做，因为偏航误差可能是由于较大的偏航陀螺偏置
// 如果超时是由于传感器故障，则无论是否在地面上都声明超时
if (maxCount > 1) {
bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000 && !(frontend->_affinity & EKF_AFFINITY_MAG);
bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
if (fusionTimeout || sensorTimeout) {
tryChangeCompass();
}
}
// 限制磁力计更新速率，以防止高处理器负载，因为磁力计融合是一个耗时的步骤，我们可能会溢出FIFO缓冲区
if (use_compass() &&
compass.healthy(magSelectIndex) &&
((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
// 检测磁力计偏移参数的变化并重置状态
Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) {
// 清零已学习的磁力计偏置状态
stateStruct.body_magfield.zero();
// 清空测量缓冲区
storedMag.reset();
// 在下次协方差预测中重置机体磁场方差，以应对新偏移中的可能错误
needMagBodyVarReset = true;
}
lastMagOffsets = nowMagOffsets;
lastMagOffsetsValid = true;
// 保存上次测量更新的时间
lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
// 当前时间范围内的磁力计数据
mag_elements magDataNew;
// 估算磁力计测量的时间，考虑到延迟
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// 纠正由于滤波器更新率造成的平均采样延迟
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// 读取磁力计数据并缩放以改善数值条件
magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype();
// 检查磁力计之间的数据是否一致
consistentMagData = compass.consistent();
// 将磁力计测量值保存到缓冲区，以便后续融合
storedMag.push(magDataNew);
// 记住我们读取磁力计的时间，以检测磁力计传感器故障
lastMagRead_ms = imuSampleTime_ms;
}
}
3.8 readAirSpdData
// 检查是否有新的空气速度数据，如果有则更新存储的测量值
void NavEKF3_core::readAirSpdData()
{
const float EAS2TAS = dal.get_EAS2TAS(); // 获取从等效空气速度 (EAS) 到真实空气速度 (TAS) 的转换系数
// 如果空气速度读取有效，且用户设置为使用，并且数据已经更新，则
// 获取新的读取值，从 EAS 转换为 TAS，并设置标志以通知其他函数
// 新的测量值已经可用
if (useAirspeed()) {
const auto *airspeed = dal.airspeed(); // 获取空气速度数据
if (airspeed &&
(airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
if (tasDataNew.allowFusion) {
tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS; // 从 EAS 转换为 TAS
timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed); // 更新接收时间
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms; // 计算 TAS 数据时间
tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f)); // 计算 TAS 数据的方差
// 由于滤波器更新速率，修正平均采样延迟
tasDataNew.time_ms -= localFilterTimeStep_ms / 2;
// 将数据保存到缓冲区，以便在融合时间窗口赶上时进行融合
storedTAS.push(tasDataNew);
}
}
// 检查缓冲区中是否有测量值已经被融合时间窗口超越并需要融合
tasDataToFuse = storedTAS.recall(tasDataDelayed, imuDataDelayed.time_ms);
} else {
if (is_positive(defaultAirSpeed)) {
// 这是首选方法，使用自动驾驶仪提供的基于模型的空气速度估计
if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) {
tasDataDelayed.tas = defaultAirSpeed * EAS2TAS; // 将默认空气速度转换为 TAS
tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f))); // 计算 TAS 数据的方差
tasDataToFuse = true;
tasDataDelayed.allowFusion = true;
tasDataDelayed.time_ms = imuDataDelayed.time_ms;
} else {
tasDataToFuse = false;
tasDataDelayed.allowFusion = false;
}
} else if (lastAspdEstIsValid && !windStateIsObservable) {
// 使用在死推断开始之前估计的最后一个空气速度
// 风状态变得不可观察
if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) {
tasDataDelayed.tas = lastAirspeedEstimate; // 使用最后的空气速度估计
// 这个空气速度估计有很大的不确定性
tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate));
tasDataToFuse = true;
tasDataDelayed.allowFusion = true;
tasDataDelayed.time_ms = imuDataDelayed.time_ms;
} else {
tasDataToFuse = false;
tasDataDelayed.allowFusion = false;
}
}
}
}
3.9 readRngBcnData
// 检查是否有新的范围信标数据，并将其推送到数据缓冲区（如果有）
void NavEKF3_core::readRngBcnData()
{
// 确保数组足够大
static_assert(ARRAY_SIZE(rngBcn.lastTime_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms 应该至少有 AP_BEACON_MAX_BEACONS 个元素");
// 获取信标数据的位置
const AP_DAL_Beacon *beacon = dal.beacon();
// 如果没有信标对象，则立即退出
if (beacon == nullptr) {
return;
}
// 获取正在使用的信标数量
rngBcn.N = MIN(beacon->count(), ARRAY_SIZE(rngBcn.lastTime_ms));
// 遍历所有信标，查找新数据，如果找到新数据，则停止搜索并将数据推送到观测缓冲区
bool newDataPushed = false;
uint8_t numRngBcnsChecked = 0;
// 从上次停止的位置的下一个索引开始搜索
uint8_t index = rngBcn.lastChecked;
while (!newDataPushed && (numRngBcnsChecked < rngBcn.N)) {
// 记录检查的信标数量
numRngBcnsChecked++;
// 移动到下一个信标，必要时环绕索引
index++;
if (index >= rngBcn.N) {
index = 0;
}
// 检查信标是否健康且有新数据
if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != rngBcn.lastTime_ms[index]) {
rng_bcn_elements rngBcnDataNew = {};
// 设置时间戳，修正测量延迟和由于滤波器更新速率造成的平均采样间隔延迟
rngBcn.lastTime_ms[index] = beacon->beacon_last_update_ms(index);
rngBcnDataNew.time_ms = rngBcn.lastTime_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms / 2;
// 设置范围噪声
// TODO 范围库应提供每个信标的噪声/精度估计
rngBcnDataNew.rngErr = frontend->_rngBcnNoise;
// 设置范围测量值
rngBcnDataNew.rng = beacon->beacon_distance(index);
// 设置信标位置
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
// 标识信标标识符
rngBcnDataNew.beacon_ID = index;
// 表示我们有新数据要推送到缓冲区
newDataPushed = true;
// 更新最后检查的索引
rngBcn.lastChecked = index;
// 将数据保存到缓冲区，以便在融合时间范围与之匹配时融合
rngBcn.storedRange.push(rngBcnDataNew);
}
}
// 检查信标系统是否返回了 3D 固定点
Vector3f bp;
float bperr;
if (beacon->get_vehicle_position_ned(bp, bperr)) {
rngBcn.last3DmeasTime_ms = imuSampleTime_ms;
}
rngBcn.vehiclePosNED = bp.toftype();
rngBcn.vehiclePosErr = bperr;
// 检查范围信标数据是否可以用来对齐车辆位置
if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250) && (rngBcn.vehiclePosErr < 1.0f) && rngBcn.alignmentCompleted) {
// 检查信标报告的位置与 3 状态对齐滤波器的位置之间的一致性
const ftype posDiffSq = sq(rngBcn.receiverPos.x - rngBcn.vehiclePosNED.x) + sq(rngBcn.receiverPos.y - rngBcn.vehiclePosNED.y);
const ftype posDiffVar = sq(rngBcn.vehiclePosErr) + rngBcn.receiverPosCov[0][0] + rngBcn.receiverPosCov[1][1];
if (posDiffSq < 9.0f * posDiffVar) {
rngBcn.goodToAlign = true;
// 如果尚未设置 EKF 原点和磁场倾角
if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
// 从信标系统获取原点
Location origin_loc;
if (beacon->get_origin(origin_loc)) {
setOriginLLH(origin_loc);
// 使用发布的倾角设置 NE 地球磁场状态
// 并设置相应的方差和协方差
alignMagStateDeclination();
// 设置原点高度的不确定性
ekfOriginHgtVar = sq(rngBcn.vehiclePosErr);
}
}
} else {
rngBcn.goodToAlign = false;
}
} else {
rngBcn.goodToAlign = false;
}
// 检查缓冲区中是否有被融合时间范围超越的数据，需要进行融合
rngBcn.dataToFuse = rngBcn.storedRange.recall(rngBcn.dataDelayed, imuDataDelayed.time_ms);
// 修正范围信标地球框架的原点相对于 EKF 地球框架原点的估计偏移
if (rngBcn.dataToFuse) {
rngBcn.dataDelayed.beacon_posNED.x += rngBcn.posOffsetNED.x;
rngBcn.dataDelayed.beacon_posNED.y += rngBcn.posOffsetNED.y;
}
}
3.10 readRangeFinder
// 读取测距传感器数据，如果有新的测量则进行处理
// 应用中位数滤波
void NavEKF3_core::readRangeFinder(void)
{
uint8_t midIndex;
uint8_t maxIndex;
uint8_t minIndex;
// 获取理论上车辆在地面上的正确距离
// 不允许距离低于5厘米，因为这可能会导致光学流处理出现问题
const auto *_rng = dal.rangefinder();
if (_rng == nullptr) {
return;
}
rngOnGnd = MAX(_rng->ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
// 限制更新频率到数据缓冲区允许的最大值
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
// 重置用于控制测量速率的定时器
lastRngMeasTime_ms = imuSampleTime_ms;
// 如果有效，将样本和样本时间存入环形缓冲区
// 如果有两个测距传感器则使用它们的数据
for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
const auto *sensor = _rng->get_backend(sensorIndex);
if (sensor == nullptr) {
continue;
}
if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
rngMeasIndex[sensorIndex]++;
if (rngMeasIndex[sensorIndex] > 2) {
rngMeasIndex[sensorIndex] = 0;
}
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance_cm() * 0.01f;
} else {
continue;
}
// 检查是否有三个新鲜样本
bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
for (uint8_t index = 0; index <= 2; index++) {
sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
}
// 如果有三个新鲜样本，则找出中位数值
if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
minIndex = 1;
maxIndex = 0;
} else {
minIndex = 0;
maxIndex = 1;
}
if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
midIndex = maxIndex;
} else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
midIndex = minIndex;
} else {
midIndex = 2;
}
// 不允许时间向后退
if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
}
// 将测量范围限制为不小于地面上的范围
rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex], rngOnGnd);
// 获取当前传感器在机体框架中的位置
rangeDataNew.sensor_idx = sensorIndex;
// 将数据写入缓冲区，带有时间戳，以便在融合时间范围赶上时融合
storedRange.push(rangeDataNew);
// 表示我们已更新测量
rngValidMeaTime_ms = imuSampleTime_ms;
} else if (onGround && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
// 起飞前，如果没有数据，则假设使用地面范围值
rangeDataNew.time_ms = imuSampleTime_ms;
rangeDataNew.rng = rngOnGnd;
// 将数据写入缓冲区，带有时间戳，以便在融合时间范围赶上时融合
storedRange.push(rangeDataNew);
// 表示我们已更新测量
rngValidMeaTime_ms = imuSampleTime_ms;
}
}
}
}
4. 总结
NavEKF3_core类含有很多滤波、数据融合等提高数据有效性的方法。
具体提高数据有效性的方法值得深入研究，后续作为单点进行介绍，如果对此感兴趣的朋友，也请评论留言，谢谢！
5. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
2565
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
