fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 13:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: don't use Vectors for height innovations (baro, rng, etc)

Browse Source
This commit is contained in:
Daniel Agar
2021-12-22 09:58:14 -05:00
parent eac92ec671
commit 35502c249d
6 changed files with 69 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/covariance.cpp
+2 -2
View File
@@ -993,8 +993,8 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
&& ((down_dvel_bias * _gps_vel_innov(2) < 0.0f && _control_status.flags.gps)
|| (down_dvel_bias * _ev_vel_innov(2) < 0.0f && _control_status.flags.ev_vel))
&& ((down_dvel_bias * _gps_pos_innov(2) < 0.0f && _control_status.flags.gps_hgt)
|| (down_dvel_bias * _baro_hgt_innov(2) < 0.0f && _control_status.flags.baro_hgt)
|| (down_dvel_bias * _rng_hgt_innov(2) < 0.0f && _control_status.flags.rng_hgt)
|| (down_dvel_bias * _baro_hgt_innov < 0.0f && _control_status.flags.baro_hgt)
|| (down_dvel_bias * _rng_hgt_innov < 0.0f && _control_status.flags.rng_hgt)
|| (down_dvel_bias * _ev_pos_innov(2) < 0.0f && _control_status.flags.ev_hgt)));
// record the pass/fail
src/modules/ekf2/EKF/ekf.h
+12 -12
View File
@@ -78,13 +78,13 @@ public:
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); }
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov; }
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var; }
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio; }
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); }
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov; }
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var; }
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio; }
void getAuxVelInnov(float aux_vel_innov[2]) const;
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
@@ -440,11 +440,11 @@ private:
Vector3f _ev_pos_innov{}; ///< external vision position innovations (m)
Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2)
Vector3f _baro_hgt_innov{}; ///< baro hgt innovations (m)
Vector3f _baro_hgt_innov_var{}; ///< baro hgt innovation variances (m**2)
float _baro_hgt_innov{}; ///< baro hgt innovations (m)
float _baro_hgt_innov_var{}; ///< baro hgt innovation variances (m**2)
Vector3f _rng_hgt_innov{}; ///< range hgt innovations (m)
Vector3f _rng_hgt_innov_var{}; ///< range hgt innovation variances (m**2)
float _rng_hgt_innov{}; ///< range hgt innovations (m)
float _rng_hgt_innov_var{}; ///< range hgt innovation variances (m**2)
Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec)
Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
@@ -672,8 +672,8 @@ private:
bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio);
bool fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
float &innov_var, float &test_ratio);
void fuseGpsVelPos();
src/modules/ekf2/EKF/ekf_helper.cpp
+2 -2
View File
@@ -965,13 +965,13 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
// return the vertical position innovation test ratio
if (_control_status.flags.baro_hgt) {
hgt = math::max(sqrtf(_baro_hgt_test_ratio(1)), FLT_MIN);
hgt = math::max(sqrtf(_baro_hgt_test_ratio), FLT_MIN);
} else if (_control_status.flags.gps_hgt) {
hgt = math::max(sqrtf(_gps_pos_test_ratio(1)), FLT_MIN);
} else if (_control_status.flags.rng_hgt) {
hgt = math::max(sqrtf(_rng_hgt_test_ratio(1)), FLT_MIN);
hgt = math::max(sqrtf(_rng_hgt_test_ratio), FLT_MIN);
} else if (_control_status.flags.ev_hgt) {
hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN);
src/modules/ekf2/EKF/estimator_interface.h
+2 -2
View File
@@ -338,8 +338,8 @@ protected:
Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios
Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios
Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio
Vector2f _baro_hgt_test_ratio{}; // baro height innovation consistency check ratios
Vector2f _rng_hgt_test_ratio{}; // range finder height innovation consistency check ratios
float _baro_hgt_test_ratio{}; // baro height innovation consistency check ratios
float _rng_hgt_test_ratio{}; // range finder height innovation consistency check ratios
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
float _tas_test_ratio{}; // tas innovation consistency check ratio
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
src/modules/ekf2/EKF/height_fusion.cpp
+41 -40
View File
@@ -40,16 +40,10 @@
void Ekf::fuseBaroHgt()
{
Vector2f baro_hgt_innov_gate;
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
_baro_hgt_innov(2) = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// observation variance - user parameter defined
baro_hgt_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
baro_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
_baro_hgt_innov = _state.pos(2) + unbiased_baro - _baro_hgt_offset;
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -57,62 +51,69 @@ void Ekf::fuseBaroHgt()
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (_baro_hgt_innov(2) < -deadzone_start) {
if (_baro_hgt_innov(2) <= -deadzone_end) {
_baro_hgt_innov(2) += deadzone_end;
if (_baro_hgt_innov < -deadzone_start) {
if (_baro_hgt_innov <= -deadzone_end) {
_baro_hgt_innov += deadzone_end;
} else {
_baro_hgt_innov(2) = -deadzone_start;
_baro_hgt_innov = -deadzone_start;
}
}
}
fuseVerticalPosition(_baro_hgt_innov, baro_hgt_innov_gate,
baro_hgt_obs_var, _baro_hgt_innov_var, _baro_hgt_test_ratio);
// innovation gate size
float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
// observation variance - user parameter defined
float obs_var = sq(fmaxf(_params.baro_noise, 0.01f));
fuseVerticalPosition(_baro_hgt_innov, innov_gate, obs_var,
_baro_hgt_innov_var, _baro_hgt_test_ratio);
}
void Ekf::fuseGpsHgt()
{
Vector2f gps_hgt_innov_gate;
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
gps_hgt_obs_var(2) = getGpsHeightVariance();
// innovation gate size
gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
fuseVerticalPosition(_gps_pos_innov, gps_hgt_innov_gate,
gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
// innovation gate size
float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
float obs_var = getGpsHeightVariance();
// _gps_pos_test_ratio(1) is the vertical test ratio
fuseVerticalPosition(_gps_pos_innov(2), innov_gate, obs_var,
_gps_pos_innov_var(2), _gps_pos_test_ratio(1));
}
void Ekf::fuseRngHgt()
{
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
rng_hgt_obs_var(2) = fmaxf(sq(_params.range_noise)
+ sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
// innovation gate size
rng_hgt_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
_rng_hgt_innov = _state.pos(2) - (-math::max(_range_sensor.getDistBottom(),
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
fuseVerticalPosition(_rng_hgt_innov, rng_hgt_innov_gate,
rng_hgt_obs_var, _rng_hgt_innov_var, _rng_hgt_test_ratio);
// innovation gate size
float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
// observation variance - user parameter defined
float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
fuseVerticalPosition(_rng_hgt_innov, innov_gate, obs_var,
_rng_hgt_innov_var, _rng_hgt_test_ratio);
}
void Ekf::fuseEvHgt()
{
Vector2f ev_hgt_innov_gate;
Vector3f ev_hgt_obs_var;
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
ev_hgt_obs_var(2) = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// innovation gate size
ev_hgt_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
fuseVerticalPosition(_ev_pos_innov, ev_hgt_innov_gate,
ev_hgt_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
// innovation gate size
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
// observation variance - defined externally
float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
// _ev_pos_test_ratio(1) is the vertical test ratio
fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var,
_ev_pos_innov_var(2), _ev_pos_test_ratio(1));
}
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+10 -11
View File
@@ -146,33 +146,32 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio)
bool Ekf::fuseVerticalPosition(const float innov, const float innov_gate, const float obs_var,
float &innov_var, float &test_ratio)
{
innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
innov_var = P(9, 9) + obs_var;
test_ratio = sq(innov) / (sq(innov_gate) * innov_var);
_vert_pos_innov_ratio = innov / sqrtf(innov_var);
_vert_pos_fuse_attempt_time_us = _time_last_imu;
bool innov_check_pass = test_ratio(1) <= 1.0f;
bool innov_check_pass = test_ratio <= 1.0f;
// if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
float innovation;
if (_fault_status.flags.bad_acc_vertical && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit);
const float innov_limit = innov_gate * sqrtf(innov_var);
innovation = math::constrain(innov, -innov_limit, innov_limit);
innov_check_pass = true;
} else {
innovation = innov(2);
innovation = innov;
}
if (innov_check_pass) {
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innovation, innov_var(2), 5);
fuseVelPosHeight(innovation, innov_var, 5);
return true;