从SCHED_TASK(ekf_check, 10, 75) 10hz开始。
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
// 检查ekf协方差是否超过容忍限度并且触发失效保护。
void Copter::ekf_check()
{
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc;
// 若是没能获得原点位置则不记录，为啥get_origin()能知道没有记录原点？
if (!ahrs.get_origin(temp_loc)) {
return;
}
// return immediately if motors are not armed, or ekf check is disabled
// 若是马达没解锁或者ekf检查被disable则立刻退出。
// fs_ekf_thresh 可通过地面站去设置，并且此时此值为0.8
if (!motors->armed() || (g.fs_ekf_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
failsafe_ekf_off_event(); // clear failsafe
return;
}
// compare compass and velocity variance vs threshold
// 与阈值对比罗盘和速度的协方差。
if (ekf_over_threshold()) {
// if compass is not yet flagged as bad
// 若是罗盘
if (!ekf_check_state.bad_variance) {
// increase counter
ekf_check_state.fail_count++;
// if counter above max then trigger failsafe
// 若是超过10次则开始触发这个ekf_variance.
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
ekf_check_state.last_warn_time = AP_HAL::millis();
}
failsafe_ekf_event();
}
}
} else { //若是有一次则减一次
// reduce counter
if (ekf_check_state.fail_count > 0) {
ekf_check_state.fail_count--;
// if compass is flagged as bad and the counter reaches zero then clear flag
// 若是减到最后为0，则清除状态，回归正路。
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
}
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
// To-Do: add ekf variances to extended status
}
比较ekf协方差是否超过阈值。若是超过阈值则返回真。
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Copter::ekf_over_threshold()
{
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
return false;
}
// use EKF to get variance
// 用EKF去获得协方差
// 分别为位置协方差、速度协方差、高度协方差、真空速协方差。
float position_variance, vel_variance, height_variance, tas_variance;
// mag 三轴协方差。
Vector3f mag_variance;
// 补偿
Vector2f offset;
ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset);
// return true if two of compass, velocity and position variances are over the threshold
// 若是同时mag_variance.length()、vel_variance 和 position_variance中有两个大于0.8既判断ekf的协方差变大为真。
uint8_t over_thresh_count = 0;
if (mag_variance.length() >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (vel_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (position_variance >= g.fs_ekf_thresh) {
over_thresh_count++;
}
if (over_thresh_count >= 2) {
return true;
}
// either optflow relative or absolute position estimate OK
// 若是前面不触发，则开始检查ekf位置是否OK，若是不OK则为真，反之为假。
// 位置超时时候也是可以让ekf_position_ok为0；
if (optflow_position_ok() || ekf_position_ok()) {
return false;
}
return true;
}
// 返回更新一致性测试比率（协方差？？？）。
// return the innovation consistency test ratios for the velocity, position, magnetometer and true airspeed measurements
// this indicates the amount of margin available when tuning the various error traps
// also return the delta in position due to the last position reset
void NavEKF2_core::getVariances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
velVar = sqrtf(velTestRatio);
posVar = sqrtf(posTestRatio);
hgtVar = sqrtf(hgtTestRatio);
// If we are using simple compass yaw fusion, populate all three components with the yaw test ratio to provide an equivalent output
magVar.x = sqrtf(MAX(magTestRatio.x,yawTestRatio));
magVar.y = sqrtf(MAX(magTestRatio.y,yawTestRatio));
magVar.z = sqrtf(MAX(magTestRatio.z,yawTestRatio));
tasVar = sqrtf(tasTestRatio);
offset = posResetNE;
}
// 检查ekf位置估计是否正确。
// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set
bool Copter::ekf_position_ok()
{
if (!ahrs.have_inertial_nav()) {
// do not allow navigation with dcm position
return false;
}
// with EKF use filter status and ekf check
nav_filter_status filt_status = inertial_nav.get_filter_status();
// Disarmed(锁定) Auto Armed(自动解锁)
// if disarmed we accept a predicted horizontal position
if (!motors->armed()) {
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
} else {
// once armed we require a good absolute position and EKF must not be in const_pos_mode
// horiz_pos_abs 如果绝对水平位置估计有效，则为真
// filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy;
// bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
/* enum AidingMode {AID_ABSOLUTE=0, // GPS or some other form of absolute position reference aiding is being used (optical flow may also be used in parallel) so position estimates are absolute.
AID_NONE=1, // no aiding is being used so only attitude and height estimates are available. Either constVelMode or constPosMode must be used to constrain tilt drift.
AID_RELATIVE=2 // only optical flow aiding is being used so position estimates will be relative }; */
// const_pos_mode 如果我们处于常量位置模式，则为真。
// filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy;
return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
}
}
接下来去查下大多数协方差过大都是由于哪一项造成的？
