fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: innovation sequence monitoring for all aid sources

Browse Source 
- add new 'innovation_filtered' and 'test_ratio_filtered' fields to
   estimator_aid_source topics
This commit is contained in:
Daniel Agar 2023-08-05 20:46:27 -04:00
parent 1c657a59b1
commit 206488b844
31 changed files with 540 additions and 549 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
msg/EstimatorAidSource1d.msg
View File

@ -11,8 +11,12 @@ float32 observation
float32 observation_variance
float32 innovation
float32 innovation_filtered
float32 innovation_variance
float32 test_ratio
float32 test_ratio # normalized innovation squared
float32 test_ratio_filtered # signed filtered test ratio
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

6
msg/EstimatorAidSource2d.msg
View File

@ -11,8 +11,12 @@ float32[2] observation
float32[2] observation_variance
float32[2] innovation
float32[2] innovation_filtered
float32[2] innovation_variance
float32[2] test_ratio
float32[2] test_ratio # normalized innovation squared
float32[2] test_ratio_filtered # signed filtered test ratio
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

6
msg/EstimatorAidSource3d.msg
View File

@ -11,8 +11,12 @@ float32[3] observation
float32[3] observation_variance
float32[3] innovation
float32[3] innovation_filtered
float32[3] innovation_variance
float32[3] test_ratio
float32[3] test_ratio # normalized innovation squared
float32[3] test_ratio_filtered # signed filtered test ratio
bool innovation_rejected # true if the observation has been rejected
bool fused # true if the sample was successfully fused

41
src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp
View File

@ -63,6 +63,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
}
#if defined(CONFIG_EKF2_GNSS)
// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
if (_control_status.flags.fixed_wing) {
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
@ -74,6 +75,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_yawEstimator.setTrueAirspeed(0.f);
}
}
#endif // CONFIG_EKF2_GNSS
if (_params.arsp_thr <= 0.f) {
@ -89,10 +91,14 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos;
const bool continuing_conditions_passing = _control_status.flags.in_air
&& _control_status.flags.fixed_wing
&& !_control_status.flags.fake_pos;
const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
const bool starting_conditions_passing = continuing_conditions_passing
&& is_airspeed_significant
&& (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);
if (_control_status.flags.fuse_aspd) {
@ -139,26 +145,22 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
{
// reset flags
resetEstimatorAidStatus(aid_src);
// Variance for true airspeed measurement - (m/sec)^2
const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
float innov = 0.f;
float innov_var = 0.f;
sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON,
&innov, &innov_var);
aid_src.observation = airspeed_sample.true_airspeed;
aid_src.observation_variance = R;
aid_src.innovation = innov;
aid_src.innovation_variance = innov_var;
aid_src.timestamp_sample = airspeed_sample.time_us;
const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
updateAidSourceStatus(aid_src,
airspeed_sample.time_us, // sample timestamp
airspeed_sample.true_airspeed, // observation
R, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.tas_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@ -221,7 +223,6 @@ void Ekf::stopAirspeedFusion()
{
if (_control_status.flags.fuse_aspd) {
ECL_INFO("stopping airspeed fusion");
resetEstimatorAidStatus(_aid_src_airspeed);
_control_status.flags.fuse_aspd = false;
#if defined(CONFIG_EKF2_GNSS)
@ -235,16 +236,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
constexpr float sideslip_var = sq(math::radians(15.0f));
const float euler_yaw = getEulerYaw(_R_to_earth);
const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
* math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;
sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind);
sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(),
getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind);
resetStateCovariance<State::wind_vel>(P_wind);
ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
_aid_src_airspeed.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(_aid_src_airspeed);
}
void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample)

20
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View File

@ -80,14 +80,17 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
position = ekf.global_origin().project(sample.latitude, sample.longitude);
//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get());
float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
const float pos_var = sq(pos_noise);
const Vector2f pos_obs_var(pos_var, pos_var);
ekf.updateHorizontalPositionAidSrcStatus(sample.time_us,
ekf.updateAidSourceStatus(aid_src,
sample.time_us, // sample timestamp
position, // observation
pos_obs_var, // observation variance
math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate
aid_src);
Vector2f(ekf.state().pos) - position, // innovation
Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate
}
const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
@ -97,6 +100,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
switch (_state) {
case State::stopped:
/* FALLTHROUGH */
case State::starting:
if (starting_conditions) {
@ -116,6 +120,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
}
}
}
break;
case State::active:
@ -124,8 +129,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
|| (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
aid_src.time_last_fuse = imu_delayed.time_us;
ekf.resetAidSourceStatusZeroInnovation(aid_src);
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
}
@ -133,6 +141,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
ekf.disableControlStatusAuxGpos();
_state = State::stopped;
}
break;
default:
@ -143,6 +152,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
aid_src.timestamp = hrt_absolute_time();
_estimator_aid_src_aux_global_position_pub.publish(aid_src);
#endif // MODULE_NAME
} else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
ekf.disableControlStatusAuxGpos();
_state = State::stopped;

15
src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp
View File

@ -36,13 +36,17 @@
void Ekf::controlAuxVelFusion()
{
if (_auxvel_buffer) {
auxVelSample auxvel_sample_delayed;
auxVelSample sample;
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
resetEstimatorAidStatus(_aid_src_aux_vel);
updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
updateAidSourceStatus(_aid_src_aux_vel,
sample.time_us, // sample timestamp
sample.vel, // observation
sample.velVar, // observation variance
Vector2f(_state.vel.xy()) - sample.vel, // innovation
Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
math::max(_params.auxvel_gate, 1.f)); // innovation gate
if (isHorizontalAidingActive()) {
fuseHorizontalVelocity(_aid_src_aux_vel);
@ -55,5 +59,4 @@ void Ekf::stopAuxVelFusion()
{
ECL_INFO("stopping aux vel fusion");
//_control_status.flags.aux_vel = false;
resetEstimatorAidStatus(_aid_src_aux_vel);
}

13
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View File

@ -59,8 +59,6 @@ void Ekf::controlBaroHeightFusion()
const float measurement_var = sq(_params.baro_noise);
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
if (measurement_valid) {
@ -80,11 +78,11 @@ void Ekf::controlBaroHeightFusion()
}
// vertical position innovation - baro measurement has opposite sign to earth z axis
updateVerticalPositionAidSrcStatus(baro_sample.time_us,
-(measurement - bias_est.getBias()),
measurement_var + bias_est.getBiasVar(),
innov_gate,
aid_src);
updateVerticalPositionAidStatus(aid_src,
baro_sample.time_us,
-(measurement - bias_est.getBias()), // observation
measurement_var + bias_est.getBiasVar(), // observation variance
math::max(_params.baro_innov_gate, 1.f)); // innovation gate
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@ -191,7 +189,6 @@ void Ekf::stopBaroHgtFusion()
}
_baro_b_est.setFusionInactive();
resetEstimatorAidStatus(_aid_src_baro_hgt);
_control_status.flags.baro_hgt = false;
}

48
src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp
View File

@ -105,11 +105,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
bcoef_inv(1) = bcoef_inv(0);
}
_aid_src_drag.timestamp_sample = drag_sample.time_us;
_aid_src_drag.fused = false;
bool fused[] {false, false};
Vector2f observation{};
Vector2f observation_variance{R_ACC, R_ACC};
Vector2f innovation{};
Vector2f innovation_variance{};
// Apply an innovation consistency check with a 5 Sigma threshold
const float innov_gate = 5.f;
VectorState H;
// perform sequential fusion of XY specific forces
@ -120,16 +125,15 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
// parallel to the rotor disc and mass flow through the rotor disc.
const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed
- rel_wind_body(axis_index) * mcoef_corrrected;
_aid_src_drag.observation[axis_index] = mea_acc;
_aid_src_drag.observation_variance[axis_index] = R_ACC;
_aid_src_drag.innovation[axis_index] = pred_acc - mea_acc;
_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
observation(axis_index) = mea_acc;
innovation(axis_index) = pred_acc - mea_acc;
if (axis_index == 0) {
sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
&_aid_src_drag.innovation_variance[axis_index], &H);
&innovation_variance(axis_index), &H);
if (!using_bcoef_x && !using_mcoef) {
continue;
@ -137,35 +141,41 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
} else if (axis_index == 1) {
sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
&_aid_src_drag.innovation_variance[axis_index], &H);
&innovation_variance(axis_index), &H);
if (!using_bcoef_y && !using_mcoef) {
continue;
}
}
if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) {
if (innovation_variance(axis_index) < R_ACC) {
// calculation is badly conditioned
return;
break;
}
// Apply an innovation consistency check with a 5 Sigma threshold
const float innov_gate = 5.f;
setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index));
if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos
&& PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
&& (_aid_src_drag.test_ratio[axis_index] < 1.f)
&& PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index))
&& (test_ratio < 1.f)
) {
VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
VectorState K = P * H / innovation_variance(axis_index);
if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) {
fused[axis_index] = true;
}
}
}
updateAidSourceStatus(_aid_src_drag,
drag_sample.time_us, // sample timestamp
observation, // observation
observation_variance, // observation variance
innovation, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
if (fused[0] && fused[1]) {
_aid_src_drag.fused = true;
_aid_src_drag.time_last_fuse = _time_delayed_us;

3
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
View File

@ -69,9 +69,6 @@ void Ekf::controlExternalVisionFusion()
_ev_sample_prev = ev_sample;
}
// record corresponding yaw state for future EV delta heading innovation (logging only)
_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
|| _control_status.flags.ev_hgt)
&& isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {

11
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
View File

@ -85,11 +85,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
updateVerticalPositionAidSrcStatus(ev_sample.time_us,
measurement - bias_est.getBias(),
measurement_var + bias_est.getBiasVar(),
math::max(_params.ev_pos_innov_gate, 1.f),
aid_src);
updateVerticalPositionAidStatus(aid_src,
ev_sample.time_us,
measurement - bias_est.getBias(), // observation
measurement_var + bias_est.getBiasVar(), // observation variance
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
@ -220,7 +220,6 @@ void Ekf::stopEvHgtFusion()
}
_ev_hgt_b_est.setFusionInactive();
resetEstimatorAidStatus(_aid_src_ev_hgt);
_control_status.flags.ev_hgt = false;
}

22
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
View File

@ -126,12 +126,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
}
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
}
}
#endif // CONFIG_EKF2_GNSS
const Vector2f measurement{pos(0), pos(1)};
@ -155,18 +157,24 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
}
}
updateHorizontalPositionAidSrcStatus(ev_sample.time_us,
measurement - _ev_pos_b_est.getBias(), // observation
measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance
math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate
aid_src);
const Vector2f position = measurement - _ev_pos_b_est.getBias();
const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar();
updateAidSourceStatus(aid_src,
ev_sample.time_us, // sample timestamp
position, // observation
pos_obs_var, // observation variance
Vector2f(_state.pos) - position, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid && quality_sufficient) {
_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance<State::pos>()));
_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
measurement_var + Vector2f(getStateVariance<State::pos>()));
}
if (!measurement_valid) {
@ -297,8 +305,6 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
void Ekf::stopEvPosFusion()
{
if (_control_status.flags.ev_pos) {
resetEstimatorAidStatus(_aid_src_ev_pos);
_control_status.flags.ev_pos = false;
}
}

35
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
View File

@ -107,12 +107,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
}
#if defined(CONFIG_EKF2_GNSS)
// increase minimum variance if GPS active (position reference)
if (_control_status.flags.gps) {
for (int i = 0; i < 2; i++) {
vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
}
}
#endif // CONFIG_EKF2_GNSS
const Vector3f measurement{vel};
@ -125,11 +127,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
updateVelocityAidSrcStatus(ev_sample.time_us,
updateAidSourceStatus(aid_src,
ev_sample.time_us, // sample timestamp
measurement, // observation
measurement_var, // observation variance
math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
aid_src);
_state.vel - measurement, // innovation
getVelocityVariance() + measurement_var, // innovation variance
math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
if (!measurement_valid) {
continuing_conditions_passing = false;
@ -149,7 +153,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
ECL_INFO("reset to %s", AID_SRC_NAME);
_information_events.flags.reset_vel_to_vision = true;
resetVelocityTo(measurement, measurement_var);
aid_src.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(aid_src);
} else {
// EV has reset, but quality isn't sufficient
@ -174,7 +178,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
_information_events.flags.reset_vel_to_vision = true;
ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
resetVelocityTo(measurement, measurement_var);
aid_src.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(aid_src);
if (_control_status.flags.in_air) {
_nb_ev_vel_reset_available--;
@ -205,19 +209,25 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
if (starting_conditions_passing) {
// activate fusion, only reset if necessary
if (!isHorizontalAidingActive() || yaw_alignment_changed) {
ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2));
_information_events.flags.reset_vel_to_vision = true;
resetVelocityTo(measurement, measurement_var);
ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
(double)measurement(0), (double)measurement(1), (double)measurement(2));
} else {
_information_events.flags.reset_vel_to_vision = true;
resetVelocityTo(measurement, measurement_var);
resetAidSourceStatusZeroInnovation(aid_src);
_control_status.flags.ev_vel = true;
} else if (fuseVelocity(aid_src)) {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
_control_status.flags.ev_vel = true;
}
aid_src.time_last_fuse = _time_delayed_us;
if (_control_status.flags.ev_vel) {
_nb_ev_vel_reset_available = 5;
_information_events.flags.starting_vision_vel_fusion = true;
_control_status.flags.ev_vel = true;
}
}
}
}
@ -225,7 +235,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
void Ekf::stopEvVelFusion()
{
if (_control_status.flags.ev_vel) {
resetEstimatorAidStatus(_aid_src_ev_vel);
_control_status.flags.ev_vel = false;
}

41
src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp
View File

@ -43,11 +43,22 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
{
static constexpr const char *AID_SRC_NAME = "EV yaw";
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = ev_sample.time_us;
aid_src.observation = getEulerYaw(ev_sample.quat);
aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
float obs = getEulerYaw(ev_sample.quat);
float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs);
float innov_var = 0.f;
VectorState H_YAW;
computeYawInnovVarAndH(obs_var, innov_var, H_YAW);
updateAidSourceStatus(aid_src,
ev_sample.time_us, // sample timestamp
obs, // observation
obs_var, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
if (ev_reset) {
_control_status.flags.ev_yaw_fault = false;
@ -65,10 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
&& (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
) {
continuing_conditions_passing = false;
// use delta yaw for innovation logging
aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev)
- wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
}
const bool starting_conditions_passing = common_starting_conditions_passing
@ -94,7 +101,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
}
} else if (quality_sufficient) {
fuseYaw(aid_src);
fuseYaw(aid_src, H_YAW);
} else {
aid_src.innovation_rejected = true;
@ -141,18 +148,25 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) {
if (_control_status.flags.yaw_align) {
if (fuseYaw(aid_src, H_YAW)) {
ECL_INFO("starting %s fusion", AID_SRC_NAME);
_information_events.flags.starting_vision_yaw_fusion = true;
_control_status.flags.ev_yaw = true;
}
} else {
// reset yaw to EV and set yaw_align
ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
_information_events.flags.starting_vision_yaw_fusion = true;
resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
_control_status.flags.yaw_align = true;
}
_control_status.flags.ev_yaw = true;
aid_src.time_last_fuse = _time_delayed_us;
_information_events.flags.starting_vision_yaw_fusion = true;
_control_status.flags.ev_yaw = true;
}
} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
// turn on fusion of external vision yaw measurements
@ -178,7 +192,6 @@ void Ekf::stopEvYawFusion()
{
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_yaw) {
resetEstimatorAidStatus(_aid_src_ev_yaw);
_control_status.flags.ev_yaw = false;
}

5
src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp
View File

@ -51,8 +51,7 @@ void Ekf::controlFakeHgtFusion()
const float obs_var = sq(_params.pos_noaid_noise);
const float innov_gate = 3.f;
updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src);
updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
const bool continuing_conditions_passing = !isVerticalAidingActive();
const bool starting_conditions_passing = continuing_conditions_passing
@ -113,7 +112,5 @@ void Ekf::stopFakeHgtFusion()
if (_control_status.flags.fake_hgt) {
ECL_INFO("stop fake height fusion");
_control_status.flags.fake_hgt = false;
resetEstimatorAidStatus(_aid_src_fake_hgt);
}
}

13
src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
View File

@ -63,10 +63,17 @@ void Ekf::controlFakePosFusion()
obs_var(0) = obs_var(1) = sq(0.5f);
}
const Vector2f position(_last_known_pos);
const float innov_gate = 3.f;
updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
updateAidSourceStatus(aid_src,
_time_delayed_us,
position, // observation
obs_var, // observation variance
Vector2f(_state.pos) - position, // innovation
Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
innov_gate); // innovation gate
const bool continuing_conditions_passing = !isHorizontalAidingActive()
&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
@ -131,7 +138,5 @@ void Ekf::stopFakePosFusion()
if (_control_status.flags.fake_pos) {
ECL_INFO("stop fake position fusion");
_control_status.flags.fake_pos = false;
resetEstimatorAidStatus(_aid_src_fake_pos);
}
}

9
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

@ -67,16 +67,14 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
const float measurement_var = sq(noise);
const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
// GNSS position, vertical position GNSS measurement has opposite sign to earth z axis
updateVerticalPositionAidSrcStatus(gps_sample.time_us,
updateVerticalPositionAidStatus(aid_src,
gps_sample.time_us,
-(measurement - bias_est.getBias()),
measurement_var + bias_est.getBiasVar(),
innov_gate,
aid_src);
math::max(_params.gps_pos_innov_gate, 1.f));
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
@ -171,7 +169,6 @@ void Ekf::stopGpsHgtFusion()
}
_gps_hgt_b_est.setFusionInactive();
resetEstimatorAidStatus(_aid_src_gnss_hgt);
_control_status.flags.gps_hgt = false;
}

50
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -183,13 +183,26 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise));
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
updateVelocityAidSrcStatus(gnss_sample.time_us,
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
updateAidSourceStatus(aid_src,
gnss_sample.time_us, // sample timestamp
velocity, // observation
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
aid_src);
_state.vel - velocity, // innovation
getVelocityVariance() + vel_obs_var, // innovation variance
innovation_gate); // innovation gate
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
// but limit innovation to prevent spikes that could destabilise the filter
if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) {
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
}
void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
@ -210,13 +223,16 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
}
}
const float pos_var = sq(pos_noise);
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
const Vector2f pos_obs_var(pos_var, pos_var);
updateHorizontalPositionAidSrcStatus(gnss_sample.time_us,
updateAidSourceStatus(aid_src,
gnss_sample.time_us, // sample timestamp
position, // observation
pos_obs_var, // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
aid_src);
Vector2f(_state.pos) - position, // innovation
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
}
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
@ -289,7 +305,8 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
{
_information_events.flags.reset_vel_to_gps = true;
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
aid_src.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(aid_src);
}
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
@ -297,7 +314,8 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
_information_events.flags.reset_pos_to_gps = true;
resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
aid_src.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(aid_src);
}
bool Ekf::shouldResetGpsFusion() const
@ -342,12 +360,12 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
return;
}
updateGpsYaw(gps_sample);
const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
if (is_new_data_available) {
updateGpsYaw(gps_sample);
const bool continuing_conditions_passing = _control_status.flags.tilt_align;
const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
@ -392,7 +410,9 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
// Reset before starting the fusion
if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
_control_status.flags.gps_yaw = true;
_control_status.flags.yaw_align = true;
}
@ -422,7 +442,6 @@ void Ekf::stopGpsYawFusion()
if (_control_status.flags.gps_yaw) {
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
ECL_INFO("stopping GPS yaw fusion");
}
@ -433,8 +452,7 @@ void Ekf::stopGpsFusion()
{
if (_control_status.flags.gps) {
ECL_INFO("stopping GPS position and velocity fusion");
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
_last_gps_fail_us = 0;
_last_gps_pass_us = 0;

62
src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp
View File

@ -48,13 +48,6 @@
void Ekf::updateGpsYaw(const gnssSample &gps_sample)
{
if (PX4_ISFINITE(gps_sample.yaw)) {
auto &gnss_yaw = _aid_src_gnss_yaw;
resetEstimatorAidStatus(gnss_yaw);
// initially populate for estimator_aid_src_gnss_yaw logging
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset);
@ -64,28 +57,24 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
float heading_pred;
float heading_innov_var;
{
VectorState H;
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
}
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON,
&heading_pred, &heading_innov_var, &H);
gnss_yaw.observation = measured_hdg;
gnss_yaw.observation_variance = R_YAW;
gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
gnss_yaw.innovation_variance = heading_innov_var;
gnss_yaw.timestamp_sample = gps_sample.time_us;
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate);
}
updateAidSourceStatus(_aid_src_gnss_yaw,
gps_sample.time_us, // sample timestamp
measured_hdg, // observation
R_YAW, // observation variance
wrap_pi(heading_pred - measured_hdg), // innovation
heading_innov_var, // innovation variance
math::max(_params.heading_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseGpsYaw(float antenna_yaw_offset)
{
auto &gnss_yaw = _aid_src_gnss_yaw;
auto &aid_src = _aid_src_gnss_yaw;
if (gnss_yaw.innovation_rejected) {
if (aid_src.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
return;
}
@ -94,19 +83,17 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
antenna_yaw_offset = 0.f;
}
VectorState H;
{
float heading_pred;
float heading_innov_var;
VectorState H;
// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
// making a separate function just for H uses more flash space without reducing CPU load significantly
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
}
sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, aid_src.observation_variance, FLT_EPSILON,
&heading_pred, &heading_innov_var, &H);
// check if the innovation variance calculation is badly conditioned
if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) {
if (aid_src.innovation_variance < aid_src.observation_variance) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_hdg = true;
@ -119,11 +106,9 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
_fault_status.flags.bad_hdg = false;
_innov_check_fail_status.flags.reject_yaw = false;
_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio);
if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
&& !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) {
if ((fabsf(aid_src.test_ratio_filtered) > 0.2f)
&& !_control_status.flags.in_air && isTimedOut(aid_src.time_last_fuse, (uint64_t)1e6)
) {
// A constant large signed test ratio is a sign of wrong gyro bias
// Reset the yaw gyro variance to converge faster and avoid
// being stuck on a previous bad estimate
@ -132,15 +117,15 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
// calculate the Kalman gains
// only calculate gains for states we are using
VectorState Kfusion = P * H / gnss_yaw.innovation_variance;
VectorState Kfusion = P * H / aid_src.innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation);
const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
_fault_status.flags.bad_hdg = !is_fused;
gnss_yaw.fused = is_fused;
aid_src.fused = is_fused;
if (is_fused) {
_time_last_heading_fuse = _time_delayed_us;
gnss_yaw.time_last_fuse = _time_delayed_us;
aid_src.time_last_fuse = _time_delayed_us;
}
}
@ -161,8 +146,5 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance);
_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
return true;
}

20
src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp
View File

@ -69,19 +69,13 @@ void Ekf::controlGravityFusion(const imuSample &imu)
sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H);
// fill estimator aid source status
resetEstimatorAidStatus(_aid_src_gravity);
_aid_src_gravity.timestamp_sample = imu.time_us;
measurement.copyTo(_aid_src_gravity.observation);
for (auto &var : _aid_src_gravity.observation_variance) {
var = measurement_var;
}
innovation.copyTo(_aid_src_gravity.innovation);
innovation_variance.copyTo(_aid_src_gravity.innovation_variance);
float innovation_gate = 0.25f;
setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
updateAidSourceStatus(_aid_src_gravity,
imu.time_us, // sample timestamp
measurement, // observation
Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance
innovation, // innovation
innovation_variance, // innovation variance
0.25f); // innovation gate
bool fused[3] {false, false, false};

20
src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp
View File

@ -97,10 +97,6 @@ void Ekf::controlMagFusion()
_fault_status.flags.bad_mag_y = false;
_fault_status.flags.bad_mag_z = false;
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = mag_sample.time_us;
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
@ -112,15 +108,13 @@ void Ekf::controlMagFusion()
VectorState H;
sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
for (int i = 0; i < 3; i++) {
aid_src.observation[i] = mag_sample.mag(i);
aid_src.observation_variance[i] = R_MAG;
aid_src.innovation[i] = mag_innov(i);
aid_src.innovation_variance[i] = innov_var(i);
}
const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
updateAidSourceStatus(aid_src,
mag_sample.time_us, // sample timestamp
mag_sample.mag, // observation
Vector3f(R_MAG, R_MAG, R_MAG), // observation variance
mag_innov, // innovation
innov_var, // innovation variance
math::max(_params.mag_innov_gate, 1.f)); // innovation gate
// Perform an innovation consistency check and report the result
_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);

2
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
View File

@ -164,8 +164,6 @@ void Ekf::stopFlowFusion()
if (_control_status.flags.opt_flow) {
ECL_INFO("stopping optical flow fusion");
_control_status.flags.opt_flow = false;
resetEstimatorAidStatus(_aid_src_optical_flow);
}
}

29
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
View File

@ -50,9 +50,6 @@
void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
{
resetEstimatorAidStatus(aid_src);
aid_src.timestamp_sample = _flow_sample_delayed.time_us;
const Vector2f vel_body = predictFlowVelBody();
const float range = predictFlowRange();
@ -66,24 +63,26 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
_flow_vel_body(1) = opt_flow_rate(0) * range;
_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
aid_src.innovation[0] = (vel_body(1) / range) - aid_src.observation[0];
aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
Vector2f innovation{
(vel_body(1) / range) - opt_flow_rate(0),
(-vel_body(0) / range) - opt_flow_rate(1)
};
// calculate the optical flow observation variance
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
aid_src.observation_variance[0] = R_LOS;
aid_src.observation_variance[1] = R_LOS;
Vector2f innov_var;
VectorState H;
sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(aid_src.innovation_variance);
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
updateAidSourceStatus(aid_src,
_flow_sample_delayed.time_us, // sample timestamp
opt_flow_rate, // observation
Vector2f{R_LOS, R_LOS}, // observation variance
innovation, // innovation
innov_var, // innovation variance
math::max(_params.flow_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseOptFlow()
@ -101,17 +100,13 @@ void Ekf::fuseOptFlow()
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
|| (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
// we need to reinitialise the covariance matrix and abort this fusion step
ECL_ERR("Opt flow error - covariance reset");
initialiseCovariance();
return;
}
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);

13
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -102,16 +102,14 @@ void Ekf::controlRangeHeightFusion()
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
// vertical position innovation - baro measurement has opposite sign to earth z axis
updateVerticalPositionAidSrcStatus(_range_sensor.getSampleAddress()->time_us,
-(measurement - bias_est.getBias()),
measurement_var + bias_est.getBiasVar(),
innov_gate,
aid_src);
updateVerticalPositionAidStatus(aid_src,
_range_sensor.getSampleAddress()->time_us, // sample timestamp
-(measurement - bias_est.getBias()), // observation
measurement_var + bias_est.getBiasVar(), // observation variance
math::max(_params.range_innov_gate, 1.f)); // innovation gate
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
@ -259,7 +257,6 @@ void Ekf::stopRngHgtFusion()
}
_rng_hgt_b_est.setFusionInactive();
resetEstimatorAidStatus(_aid_src_rng_hgt);
_control_status.flags.rng_hgt = false;
}

35
src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp
View File

@ -49,8 +49,10 @@
void Ekf::controlBetaFusion(const imuSample &imu_delayed)
{
_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
_control_status.flags.fuse_beta = _params.beta_fusion_enabled
&& _control_status.flags.fixed_wing
&& _control_status.flags.in_air
&& !_control_status.flags.fake_pos;
if (_control_status.flags.fuse_beta) {
@ -76,26 +78,22 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
}
}
void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
{
// reset flags
resetEstimatorAidStatus(sideslip);
float observation = 0.f;
const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
const float R = sq(_params.beta_noise); // observation noise variance
float innov = 0.f;
float innov_var = 0.f;
float innov;
float innov_var;
sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var);
sideslip.observation = 0.f;
sideslip.observation_variance = R;
sideslip.innovation = innov;
sideslip.innovation_variance = innov_var;
sideslip.timestamp_sample = _time_delayed_us;
const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);
setEstimatorAidStatusTestRatio(sideslip, innov_gate);
updateAidSourceStatus(aid_src,
_time_delayed_us, // sample timestamp
observation, // observation
R, // observation variance
innov, // innovation
innov_var, // innovation variance
math::max(_params.beta_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
@ -103,6 +101,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
if (sideslip.innovation_rejected) {
return;
}
// determine if we need the sideslip fusion to correct states other than wind
bool update_wind_only = !_control_status.flags.wind_dead_reckoning;

53
src/modules/ekf2/EKF/ekf.cpp
View File

@ -124,53 +124,6 @@ void Ekf::reset()
_time_good_vert_accel = 0;
_clip_counter = 0;
#if defined(CONFIG_EKF2_BAROMETER)
resetEstimatorAidStatus(_aid_src_baro_hgt);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_AIRSPEED)
resetEstimatorAidStatus(_aid_src_airspeed);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_SIDESLIP)
resetEstimatorAidStatus(_aid_src_sideslip);
#endif // CONFIG_EKF2_SIDESLIP
resetEstimatorAidStatus(_aid_src_fake_pos);
resetEstimatorAidStatus(_aid_src_fake_hgt);
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
resetEstimatorAidStatus(_aid_src_ev_hgt);
resetEstimatorAidStatus(_aid_src_ev_pos);
resetEstimatorAidStatus(_aid_src_ev_vel);
resetEstimatorAidStatus(_aid_src_ev_yaw);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
resetEstimatorAidStatus(_aid_src_gnss_hgt);
resetEstimatorAidStatus(_aid_src_gnss_pos);
resetEstimatorAidStatus(_aid_src_gnss_vel);
# if defined(CONFIG_EKF2_GNSS_YAW)
resetEstimatorAidStatus(_aid_src_gnss_yaw);
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
resetEstimatorAidStatus(_aid_src_mag);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_AUXVEL)
resetEstimatorAidStatus(_aid_src_aux_vel);
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
resetEstimatorAidStatus(_aid_src_optical_flow);
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
resetEstimatorAidStatus(_aid_src_rng_hgt);
#endif // CONFIG_EKF2_RANGE_FINDER
_zero_velocity_update.reset();
}
@ -332,7 +285,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
{
if (!_pos_ref.isInitialized()) {
return;
}
@ -344,7 +296,10 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
ECL_INFO("reset position to external observation");
_information_events.flags.reset_pos_to_ext_obs = true;
}
void Ekf::updateParameters()

259
src/modules/ekf2/EKF/ekf.h
View File

@ -641,8 +641,6 @@ private:
estimator_aid_source3d_s _aid_src_ev_vel{};
estimator_aid_source1d_s _aid_src_ev_yaw{};
float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
uint8_t _nb_ev_pos_reset_available{0};
uint8_t _nb_ev_vel_reset_available{0};
uint8_t _nb_ev_yaw_reset_available{0};
@ -747,7 +745,6 @@ private:
}
// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
@ -802,8 +799,9 @@ private:
void resetHorizontalVelocityToZero();
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
void resetHorizontalPositionToLastKnown();
void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var);
void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); }
@ -815,20 +813,15 @@ private:
void resetVerticalVelocityToZero();
// horizontal and vertical position aid source
void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
// 2d & 3d velocity aid source
void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate = 1.f) const;
// horizontal and vertical position fusion
void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
bool fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
// 2d & 3d velocity fusion
void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
bool fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
#if defined(CONFIG_EKF2_TERRAIN)
// terrain vertical position estimator
@ -1130,88 +1123,200 @@ private:
bool _ev_q_error_initialized{false};
#endif // CONFIG_EKF2_EXTERNAL_VISION
void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
// state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc)
void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const
{
// only bother resetting if timestamp_sample is set
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
status.time_last_fuse = _time_delayed_us;
// preserve status.time_last_fuse
status.innovation = 0.f;
status.innovation_filtered = 0.f;
status.innovation_variance = status.observation_variance;
status.observation = 0;
status.observation_variance = 0;
status.test_ratio = 0.f;
status.test_ratio_filtered = 0.f;
status.innovation = 0;
status.innovation_variance = 0;
status.test_ratio = INFINITY;
status.innovation_rejected = true;
status.fused = false;
}
status.innovation_rejected = false;
status.fused = true;
}
template <typename T>
void resetEstimatorAidStatus(T &status) const
{
// only bother resetting if timestamp_sample is set
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
// preserve status.time_last_fuse
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
status.observation[i] = 0;
status.observation_variance[i] = 0;
status.innovation[i] = 0;
status.innovation_variance[i] = 0;
status.test_ratio[i] = INFINITY;
}
status.innovation_rejected = true;
status.fused = false;
}
}
void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
{
if (PX4_ISFINITE(status.innovation)
&& PX4_ISFINITE(status.innovation_variance)
&& (status.innovation_variance > 0.f)
) {
status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
status.innovation_rejected = (status.test_ratio > 1.f);
} else {
status.test_ratio = INFINITY;
status.innovation_rejected = true;
}
}
template <typename T>
void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
// helper used for populating and filtering estimator aid source struct for logging
void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
const float &observation, const float &observation_variance,
const float &innovation, const float &innovation_variance,
float innovation_gate = 1.f) const
{
bool innovation_rejected = false;
for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
if (PX4_ISFINITE(status.innovation[i])
&& PX4_ISFINITE(status.innovation_variance[i])
&& (status.innovation_variance[i] > 0.f)
) {
status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
if (status.test_ratio[i] > 1.f) {
innovation_rejected = true;
if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
static constexpr float tau = 0.5f;
const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
// test_ratio_filtered
if (PX4_ISFINITE(status.test_ratio_filtered)) {
status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
} else {
// otherwise, init the filtered test ratio
status.test_ratio_filtered = test_ratio;
}
// innovation_filtered
if (PX4_ISFINITE(status.innovation_filtered)) {
status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
} else {
// otherwise, init the filtered innovation
status.innovation_filtered = innovation;
}
// limit extremes in filtered values
static constexpr float kNormalizedInnovationLimit = 2.f;
static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
if (test_ratio > kTestRatioLimit) {
status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
}
} else {
status.test_ratio[i] = INFINITY;
// invalid timestamp_sample, reset
status.test_ratio_filtered = test_ratio;
status.innovation_filtered = innovation;
}
status.test_ratio = test_ratio;
status.observation = observation;
status.observation_variance = observation_variance;
status.innovation = innovation;
status.innovation_variance = innovation_variance;
if ((test_ratio > 1.f)
|| !PX4_ISFINITE(test_ratio)
|| !PX4_ISFINITE(status.innovation)
|| !PX4_ISFINITE(status.innovation_variance)
) {
innovation_rejected = true;
}
status.timestamp_sample = timestamp_sample;
// if any of the innovations are rejected, then the overall innovation is rejected
status.innovation_rejected = innovation_rejected;
// reset
status.fused = false;
}
// state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc)
template <typename T>
void resetAidSourceStatusZeroInnovation(T &status) const
{
status.time_last_fuse = _time_delayed_us;
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
status.innovation[i] = 0.f;
status.innovation_filtered[i] = 0.f;
status.innovation_variance[i] = status.observation_variance[i];
status.test_ratio[i] = 0.f;
status.test_ratio_filtered[i] = 0.f;
}
status.innovation_rejected = false;
status.fused = true;
}
// helper used for populating and filtering estimator aid source struct for logging
template <typename T, typename S>
void updateAidSourceStatus(T &status, const uint64_t &timestamp_sample,
const S &observation, const S &observation_variance,
const S &innovation, const S &innovation_variance,
float innovation_gate = 1.f) const
{
bool innovation_rejected = false;
const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
static constexpr float tau = 0.5f;
const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
const float test_ratio = sq(innovation(i)) / (sq(innovation_gate) * innovation_variance(i));
if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
// test_ratio_filtered
if (PX4_ISFINITE(status.test_ratio_filtered[i])) {
status.test_ratio_filtered[i] += alpha * (matrix::sign(innovation(i)) * test_ratio - status.test_ratio_filtered[i]);
} else {
// otherwise, init the filtered test ratio
status.test_ratio_filtered[i] = test_ratio;
}
// innovation_filtered
if (PX4_ISFINITE(status.innovation_filtered[i])) {
status.innovation_filtered[i] += alpha * (innovation(i) - status.innovation_filtered[i]);
} else {
// otherwise, init the filtered innovation
status.innovation_filtered[i] = innovation(i);
}
// limit extremes in filtered values
static constexpr float kNormalizedInnovationLimit = 2.f;
static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
if (test_ratio > kTestRatioLimit) {
status.test_ratio_filtered[i] = math::constrain(status.test_ratio_filtered[i], -kTestRatioLimit, kTestRatioLimit);
const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance(i));
status.innovation_filtered[i] = math::constrain(status.innovation_filtered[i], -innov_limit, innov_limit);
}
} else {
// invalid timestamp_sample, reset
status.test_ratio_filtered[i] = test_ratio;
status.innovation_filtered[i] = innovation(i);
}
status.test_ratio[i] = test_ratio;
status.observation[i] = observation(i);
status.observation_variance[i] = observation_variance(i);
status.innovation[i] = innovation(i);
status.innovation_variance[i] = innovation_variance(i);
if ((test_ratio > 1.f)
|| !PX4_ISFINITE(test_ratio)
|| !PX4_ISFINITE(status.innovation[i])
|| !PX4_ISFINITE(status.innovation_variance[i])
) {
innovation_rejected = true;
}
}
status.timestamp_sample = timestamp_sample;
// if any of the innovations are rejected, then the overall innovation is rejected
status.innovation_rejected = innovation_rejected;
// reset
status.fused = false;
}
ZeroGyroUpdate _zero_gyro_update{};

1
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -401,7 +401,6 @@ protected:
# if defined(CONFIG_EKF2_GNSS_YAW)
// innovation consistency check monitoring ratios
AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
uint64_t _time_last_gps_yaw_buffer_push{0};
# endif // CONFIG_EKF2_GNSS_YAW
#endif // CONFIG_EKF2_GNSS

57
src/modules/ekf2/EKF/position_fusion.cpp
View File

@ -33,50 +33,27 @@
#include "ekf.h"
void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us,
const float observation, const float observation_variance, const float innovation_gate) const
{
resetEstimatorAidStatus(aid_src);
float innovation = _state.pos(2) - observation;
float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
for (int i = 0; i < 2; i++) {
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::pos.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
const float innov_gate, estimator_aid_source1d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
aid_src.observation = obs;
aid_src.innovation = _state.pos(2) - aid_src.observation;
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
updateAidSourceStatus(aid_src, time_us,
observation, observation_variance,
innovation, innovation_variance,
innovation_gate);
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected
@ -91,9 +68,11 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
} else {
aid_src.fused = false;
}
return aid_src.fused;
}
void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected
@ -107,6 +86,8 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
} else {
aid_src.fused = false;
}
return aid_src.fused;
}
void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
@ -200,11 +181,3 @@ void Ekf::resetHorizontalPositionToLastKnown()
// Used when falling back to non-aiding mode of operation
resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise));
}
void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy)
{
ECL_INFO("reset position to external observation");
_information_events.flags.reset_pos_to_ext_obs = true;
resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
}

31
src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp
View File

@ -205,8 +205,6 @@ void Ekf::stopHaglRngFusion()
ECL_INFO("stopping HAGL range fusion");
// reset flags
resetEstimatorAidStatus(_aid_src_terrain_range_finder);
_innov_check_fail_status.flags.reject_hagl = false;
_hagl_sensor_status.flags.range_finder = false;
@ -221,30 +219,19 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
// predict the hagl from the vehicle position and terrain height
const float pred_hagl = _terrain_vpos - _state.pos(2);
// calculate the innovation
const float hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty
const float obs_variance = getRngVar();
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
// perform an innovation consistency check and only fuse data if it passes
const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f);
aid_src.timestamp_sample = _time_delayed_us; // TODO
aid_src.observation = meas_hagl;
aid_src.observation_variance = obs_variance;
aid_src.innovation = hagl_innov;
aid_src.innovation_variance = hagl_innov_var;
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.fused = false;
updateAidSourceStatus(aid_src,
_time_delayed_us, // sample timestamp
meas_hagl, // observation
obs_variance, // observation variance
pred_hagl - meas_hagl, // innovation
hagl_innov_var, // innovation variance
math::max(_params.range_innov_gate, 1.f)); // innovation gate
}
void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
@ -329,7 +316,6 @@ void Ekf::stopHaglFlowFusion()
ECL_INFO("stopping HAGL flow fusion");
_hagl_sensor_status.flags.flow = false;
resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
}
}
@ -367,9 +353,6 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
return;
}
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);

54
src/modules/ekf2/EKF/velocity_fusion.cpp
View File

@ -33,53 +33,7 @@
#include "ekf.h"
void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
const float innov_gate, estimator_aid_source2d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
for (int i = 0; i < 2; i++) {
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::vel.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
aid_src.timestamp_sample = time_us;
}
void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
const float innov_gate, estimator_aid_source3d_s &aid_src) const
{
resetEstimatorAidStatus(aid_src);
for (int i = 0; i < 3; i++) {
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
const int state_index = State::vel.idx + i;
aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
}
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
// vz special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
aid_src.timestamp_sample = time_us;
}
void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
{
// vx, vy
if (!aid_src.innovation_rejected
@ -94,9 +48,11 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
} else {
aid_src.fused = false;
}
return aid_src.fused;
}
void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
// vx, vy, vz
if (!aid_src.innovation_rejected
@ -113,6 +69,8 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
} else {
aid_src.fused = false;
}
return aid_src.fused;
}
void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)

25
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -37,23 +37,8 @@
#include <mathlib/mathlib.h>
// update quaternion states and covariances using the yaw innovation and yaw observation variance
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
{
VectorState H_YAW;
sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW);
return fuseYaw(aid_src_status, H_YAW);
}
bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW)
{
// define the innovation gate size
float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
// innovation test ratio
setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
// check if the innovation variance calculation is badly conditioned
if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
// the innovation variance contribution from the state covariances is not negative, no fault
@ -83,7 +68,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
Kfusion(row) *= heading_innov_var_inv;
}
// set the magnetometer unhealthy if the test fails
// set the heading unhealthy if the test fails
if (aid_src_status.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
@ -97,7 +82,8 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
// constrain the innovation to the maximum set by the gate
// we need to delay this forced fusion to avoid starting it
// immediately after touchdown, when the drone is still armed
float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
const float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
const float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
// also reset the yaw gyro variance to converge faster and avoid
@ -122,13 +108,10 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
_fault_status.flags.bad_hdg = false;
return true;
} else {
_fault_status.flags.bad_hdg = true;
}
// otherwise
aid_src_status.fused = false;
_fault_status.flags.bad_hdg = true;
return false;
}