From da9d894441400df67dcc305b94d3aba4813c46c9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 7 May 2016 12:52:45 +1000 Subject: [PATCH] EKF: Improvements to covariance reset --- EKF/control.cpp | 12 +++--------- EKF/covariance.cpp | 16 ++++++++-------- EKF/ekf.cpp | 7 +++++-- EKF/ekf_helper.cpp | 4 ++-- EKF/optflow_fusion.cpp | 2 +- 5 files changed, 19 insertions(+), 22 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 12869a7ab3..3d2acd9178 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -291,18 +291,12 @@ void Ekf::controlFusionModes() // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); - // check if baro data is consistent - float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); - // reset to baro if data is available and we have no range data + // reset to baro if we have no range data and baro data is available bool reset_to_baro = !rng_data_available && baro_data_available; - // reset to baro if data is acceptable - reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty); - - // reset to range data if it is available and we cannot switch to baro - bool reset_to_rng = !reset_to_baro && rng_data_available; + // reset to range data if it is available + bool reset_to_rng = rng_data_available; if (reset_to_baro) { // set height sensor health diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 5febf3f49b..948bed6ccd 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -732,42 +732,42 @@ void Ekf::limitCov() for (int i = 0; i <= 3; i++) { - math::constrain(P[i][i], 0.0f, P_lim[0]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]); } for (int i = 4; i <= 6; i++) { - math::constrain(P[i][i], 0.0f, P_lim[1]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]); } for (int i = 7; i <= 9; i++) { - math::constrain(P[i][i], 0.0f, P_lim[2]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]); } for (int i = 10; i <= 12; i++) { - math::constrain(P[i][i], 0.0f, P_lim[3]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]); } for (int i = 13; i <= 15; i++) { - math::constrain(P[i][i], 0.0f, P_lim[4]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); } for (int i = 16; i <= 18; i++) { - math::constrain(P[i][i], 0.0f, P_lim[5]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]); } for (int i = 19; i <= 21; i++) { - math::constrain(P[i][i], 0.0f, P_lim[6]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]); } for (int i = 22; i <= 23; i++) { - math::constrain(P[i][i], 0.0f, P_lim[7]); + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]); } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 0e02296d73..3c8de0ae87 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -263,14 +263,17 @@ bool Ekf::update() } // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled + uint64_t last_baro_time_us = _baro_sample_delayed.time_us; if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_control_status.flags.baro_hgt) { _fuse_height = true; } else { // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference - float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset; - _baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f); + float dt = math::constrain(1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us),0.0f,1.0f); + last_baro_time_us = _baro_sample_delayed.time_us; + float raw_offset_error = (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; + _baro_hgt_offset += dt * math::constrain(0.1f * raw_offset_error, -0.1f, 0.1f); } } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ea98974464..0aca1f2ff1 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -135,7 +135,7 @@ void Ekf::resetHeight() zeroRows(P, 9, 9); zeroCols(P, 9, 9); - // the state variance is th esame as the observation + // the state variance is the same as the observation P[9][9] = sq(_params.baro_noise); vert_pos_reset = true; @@ -185,7 +185,7 @@ void Ekf::resetHeight() // Set the variance to a value large enough to allow the state to converge quickly // that does not destabilise the filter - P[6][6] = fminf(sq(_state.vel(2)),100.0f); + P[6][6] = 10.0f; } vert_vel_reset = true; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 1dae4d5c6c..5345f18310 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -438,7 +438,7 @@ void Ekf::fuseOptFlow() // first calculate expression for KHP // then calculate P - KHP for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column <= 5; column++) { + for (unsigned column = 0; column <= 6; column++) { KH[row][column] = gain[row] * H_LOS[obs_index][column]; } }