From 86b9079bdc093f45c4f68b875c33aaeae9251f70 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 15 Oct 2019 14:49:05 +0200 Subject: [PATCH] Refactoring velPos fusion --- EKF/common.h | 6 + EKF/control.cpp | 286 +++++++++++++++++---------- EKF/ekf.h | 59 +++--- EKF/ekf_helper.cpp | 53 +++-- EKF/estimator_interface.h | 30 +-- EKF/vel_pos_fusion.cpp | 407 ++++++++++++++------------------------ 6 files changed, 423 insertions(+), 418 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 310817f23c..e8f4ab4b94 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -210,6 +210,12 @@ struct auxVelSample { #define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) #define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec) +// VelPos measurement bundles indices +#define HVEL 0 ///< x and y velocity bundle index +#define VVEL 1 ///< z velocity bundle index +#define HPOS 2 ///< x and y position bundle index +#define VPOS 3 ///< z position bundle index + // bad accelerometer detection and mitigation #define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) #define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) diff --git a/EKF/control.cpp b/EKF/control.cpp index 00ae5e38b0..0b797f9287 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -157,16 +157,15 @@ void Ekf::controlFusionModes() controlDragFusion(); controlHeightFusion(); - // For efficiency, fusion of direct state observations for position and velocity is performed sequentially - // in a single function using sensor data from multiple sources (GPS, baro, range finder, etc) - controlVelPosFusion(); - - // Additional data from an external vision pose estimator can be fused. + // Additional data odoemtery data from an external estimator can be fused. controlExternalVisionFusion(); - // Additional NE velocity data from an auxiliary sensor can be fused + // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(); + // Fake position measurement for constraining drift when no other velocity or position measurements + controlFakePosFusion(); + // check if we are no longer fusing measurements that directly constrain velocity drift update_deadreckoning_status(); } @@ -266,23 +265,13 @@ void Ekf::controlExternalVisionFusion() } } - // determine if we should start using the height observations - if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { - // don't start using EV data unless data is arriving frequently - if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) { - setControlEVHeight(); - resetHeight(); - } - } - - // determine if we should use the vertical position observation - if (_control_status.flags.ev_hgt) { - _fuse_height = true; - } + bool ev_fuse_mask[4]{}; + float ev_obs_var[6]{}; + float ev_innov_gate[4]{}; // determine if we should use the horizontal position observations if (_control_status.flags.ev_pos) { - _fuse_pos = true; + ev_fuse_mask[HPOS] = true; // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; @@ -312,11 +301,11 @@ void Ekf::controlExternalVisionFusion() ev_delta_pos = _ev_rot_mat * ev_delta_pos; // use the change in position since the last measurement - _vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); - _vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); + _ev_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); + _ev_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); // observation 1-STD error, incremental pos observation is expected to have more uncertainty - _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f); + ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.5f); } // record observation and estimate for use next time @@ -330,10 +319,10 @@ void Ekf::controlExternalVisionFusion() if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _ev_rot_mat * ev_pos_meas; } - _vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); - _vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); + _ev_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); + _ev_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); // observation 1-STD error - _posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f); + ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.01f); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) { @@ -347,16 +336,12 @@ void Ekf::controlExternalVisionFusion() } // innovation gate size - _posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f); - }else{ - _vel_pos_innov[3] = 0.0f; - _vel_pos_innov[4] = 0.0f; + ev_innov_gate[HPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f); } // determine if we should use the velocity observations if (_control_status.flags.ev_vel) { - _fuse_hor_vel = true; - _fuse_vert_vel = true; + ev_fuse_mask[HVEL] = ev_fuse_mask[VVEL] = true; Vector3f vel_aligned{_ev_sample_delayed.vel}; @@ -372,9 +357,9 @@ void Ekf::controlExternalVisionFusion() Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; vel_aligned -= vel_offset_earth; - _vel_pos_innov[0] = _state.vel(0) - vel_aligned(0); - _vel_pos_innov[1] = _state.vel(1) - vel_aligned(1); - _vel_pos_innov[2] = _state.vel(2) - vel_aligned(2); + _ev_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0); + _ev_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1); + _ev_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2); // check if we have been deadreckoning too long if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) { @@ -385,19 +370,15 @@ void Ekf::controlExternalVisionFusion() } // observation 1-STD error - _velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f); + ev_obs_var[0] = ev_obs_var[1] = ev_obs_var[2] = fmaxf(_ev_sample_delayed.velErr, 0.01f); // innovation gate size - _vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f); + ev_innov_gate[HVEL] = ev_innov_gate[VVEL] = fmaxf(_params.ev_vel_innov_gate, 1.0f); } // Fuse available NED position data into the main filter - if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { - fuseVelPosHeight(); - _fuse_vert_vel = _fuse_hor_vel = false; - _fuse_pos = _fuse_height = false; - _fuse_hpos_as_odom = false; - + if (ev_fuse_mask[HVEL] || ev_fuse_mask[VVEL] || ev_fuse_mask[HPOS]) { + fuseVelPosHeightSeq(_ev_vel_pos_innov, ev_innov_gate, ev_obs_var , ev_fuse_mask, _ev_vel_pos_innov_var, _ev_vel_pos_test_ratio); } // determine if we should use the yaw observation @@ -702,9 +683,14 @@ void Ekf::controlGpsFusion() // Only use GPS data for position and velocity aiding if enabled if (_control_status.flags.gps) { - _fuse_pos = true; - _fuse_vert_vel = true; - _fuse_hor_vel = true; + + bool gps_fuse_mask[4]{}; + float gps_obs_var[6]{}; + float gps_innov_gate[4]{}; + + // Enable full velocity and horizontal position fusion + gps_fuse_mask[HVEL] = gps_fuse_mask[VVEL] = true; + gps_fuse_mask[HPOS] = true; // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); @@ -725,27 +711,32 @@ void Ekf::controlGpsFusion() if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { // if we are using other sources of aiding, then relax the upper observation // noise limit which prevents bad GPS perturbing the position estimate - _posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit); + gps_obs_var[3] = gps_obs_var[4] = fmaxf(_gps_sample_delayed.hacc, lower_limit); } else { // if we are not using another source of aiding, then we are reliant on the GPS // observations to constrain attitude errors and must limit the observation noise value. float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - _posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); + gps_obs_var[3] = gps_obs_var[4] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); } - _velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); + gps_obs_var[0] = gps_obs_var[1] = gps_obs_var[2] = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); // calculate innovations - _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); - _vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); - _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); - _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); - _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); + _gps_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); + _gps_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); + _gps_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); + _gps_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); + _gps_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); // set innovation gate size - _posInnovGateNE = fmaxf(_params.gps_pos_innov_gate, 1.0f); - _hvelInnovGate = _vvelInnovGate = fmaxf(_params.gps_vel_innov_gate, 1.0f); + gps_innov_gate[HPOS] = fmaxf(_params.gps_pos_innov_gate, 1.0f); + gps_innov_gate[HVEL] = gps_innov_gate[VVEL] = fmaxf(_params.gps_vel_innov_gate, 1.0f); + + // fuse GPS measurement + fuseVelPosHeightSeq(_gps_vel_pos_innov,gps_innov_gate, + gps_obs_var,gps_fuse_mask, + _gps_vel_pos_innov_var,_gps_vel_pos_test_ratio); } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { @@ -995,7 +986,12 @@ void Ekf::controlHeightSensorTimeouts() void Ekf::controlHeightFusion() { - // set control flags for the desired primary height source + bool height_fuse_mask[4]{}; + float height_innov_gate[4]{}; + float height_test_ratio[4]{}; + float height_obs_var[6]{}; + float height_innov_var[6]{}; + float height_innov[6]{}; checkRangeAidSuitability(); _range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable(); @@ -1004,7 +1000,7 @@ void Ekf::controlHeightFusion() if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { setControlRangeHeight(); - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1019,7 +1015,7 @@ void Ekf::controlHeightFusion() } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -1037,7 +1033,7 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) { // switch to gps if there was a reset to gps - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using gps height, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1050,7 +1046,7 @@ void Ekf::controlHeightFusion() // set the height data source to range if requested if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) { setControlRangeHeight(); - _fuse_height = _range_data_ready; + height_fuse_mask[VPOS] = _range_data_ready; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1072,7 +1068,7 @@ void Ekf::controlHeightFusion() } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -1086,7 +1082,7 @@ void Ekf::controlHeightFusion() if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { setControlRangeHeight(); - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1101,7 +1097,7 @@ void Ekf::controlHeightFusion() } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { setControlGPSHeight(); - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using gps height, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1111,7 +1107,7 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -1123,9 +1119,17 @@ void Ekf::controlHeightFusion() // Determine if we rely on EV height but switched to baro if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { + + // don't start using EV data unless data is arriving frequently + if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) { + height_fuse_mask[VPOS] = true; + setControlEVHeight(); + resetHeight(); + } + if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro - _fuse_height = true; + height_fuse_mask[VPOS] = true; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -1133,6 +1137,11 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = 0.0f; } } + // TODO: Add EV normal case here + // determine if we should use the vertical position observation + if (_control_status.flags.ev_hgt) { + height_fuse_mask[VPOS] = true; + } } // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference @@ -1157,9 +1166,73 @@ void Ekf::controlHeightFusion() } - _fuse_height = true; + height_fuse_mask[VPOS] = true; } + if (height_fuse_mask[VPOS]) { + if (_control_status.flags.baro_hgt) { + // vertical position innovation - baro measurement has opposite sign to earth z axis + height_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; + // observation variance - user parameter defined + height_obs_var[5] = sq(fmaxf(_params.baro_noise, 0.01f)); + + // innovation gate size + height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f); + + // Compensate for positive static pressure transients (negative vertical position innovations) + // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. + float deadzone_start = 0.0f; + float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; + + if (_control_status.flags.gnd_effect) { + if (height_innov[5] < -deadzone_start) { + if (height_innov[5] <= -deadzone_end) { + height_innov[5] += deadzone_end; + + } else { + height_innov[5] = -deadzone_start; + } + } + } + + } else if (_control_status.flags.gps_hgt) { + // vertical position innovation - gps measurement has opposite sign to earth z axis + height_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; + // observation variance - receiver defined and parameter limited + // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP + float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + height_obs_var[5] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit)); + // innovation gate size + height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f); + + } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { + // use range finder with tilt correction + height_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, + _params.rng_gnd_clearance)) - _hgt_sensor_offset; + // observation variance - user parameter defined + height_obs_var[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); + // innovation gate size + height_innov_gate[VPOS] = fmaxf(_params.range_innov_gate, 1.0f); + + } else if (_control_status.flags.ev_hgt) { + // calculate the innovation assuming the external vision observation is in local NED frame + height_innov[5] = _state.pos(2) - _ev_sample_delayed.pos(2); + // observation variance - defined externally + height_obs_var[5] = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); + // innovation gate size + height_innov_gate[VPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f); + } + // fuse height inforamtion + fuseVelPosHeightSeq(height_innov,height_innov_gate, + height_obs_var,height_fuse_mask, + height_innov_var,height_test_ratio); + + // This is a temporary hack until we do proper height sensor fusion + _gps_vel_pos_innov[5] = height_innov[5]; + _gps_vel_pos_innov_var[5] = height_innov_var[5]; + _gps_vel_pos_test_ratio[VPOS] = height_test_ratio[VPOS]; + } } @@ -1296,13 +1369,13 @@ void Ekf::controlDragFusion() } } -void Ekf::controlVelPosFusion() +void Ekf::controlFakePosFusion() { - // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift + // if we aren't doing any aiding, fake position measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz - if (!(_params.fusion_mode & MASK_USE_GPS)) { - _control_status.flags.gps = false; - } + bool fake_gps_fuse_mask[4]{}; + float fake_gps_obs_var[6]{}; + float fake_gps_innov_gate[4]{}; if (!_control_status.flags.gps && !_control_status.flags.opt_flow && @@ -1314,51 +1387,45 @@ void Ekf::controlVelPosFusion() _using_synthetic_position = true; // Fuse synthetic position observations every 200msec - if (((_time_last_imu - _time_last_fake_gps) > (uint64_t)2e5) || _fuse_height) { + if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) { // Reset position and velocity states if we re-commence this aiding method - if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) { + if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) { resetPosition(); resetVelocity(); _fuse_hpos_as_odom = false; - if (_time_last_fake_gps != 0) { + if (_time_last_fake_pos != 0) { ECL_WARN_TIMESTAMPED("EKF stopping navigation"); } } + // Fuse horizontal position + fake_gps_fuse_mask[HPOS] = true; - _fuse_pos = true; - _fuse_hor_vel = false; - _fuse_vert_vel = false; - _time_last_fake_gps = _time_last_imu; + _time_last_fake_pos = _time_last_imu; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - _posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + fake_gps_obs_var[3] = fake_gps_obs_var[4] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); } else { - _posObsNoiseNE = 0.5f; + fake_gps_obs_var[3] = fake_gps_obs_var[4] = 0.5f; } - _vel_pos_innov[0] = 0.0f; - _vel_pos_innov[1] = 0.0f; - _vel_pos_innov[2] = 0.0f; - _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); - _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); + _gps_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); + _gps_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); // glitch protection is not required so set gate to a large value - _posInnovGateNE = 100.0f; + fake_gps_innov_gate[HPOS] = 100.0f; + fuseVelPosHeightSeq(_gps_vel_pos_innov,fake_gps_innov_gate, + fake_gps_obs_var,fake_gps_fuse_mask, + _gps_vel_pos_innov_var,_gps_vel_pos_test_ratio); } } else { _using_synthetic_position = false; } - // Fuse available NED velocity and position data into the main filter - if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { - fuseVelPosHeight(); - - } } void Ekf::controlAuxVelFusion() @@ -1367,13 +1434,32 @@ void Ekf::controlAuxVelFusion() bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow; if (data_ready && primary_aiding) { - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; - _fuse_hor_vel_aux = true; - _aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); - _aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); - _velObsVarNED(0) = _auxvel_sample_delayed.velVarNE(0); - _velObsVarNED(1) = _auxvel_sample_delayed.velVarNE(1); - _hvelInnovGate = _params.auxvel_gate; - fuseVelPosHeight(); + + bool auxvel_fuse_mask[4]{}; + float auxvel_innov[6]{}; + float auxvel_innov_gate[4]{}; + float auxvel_obs_var[4]{}; + float auxvel_innov_var[6]{}; + float auxvel_test_ratio[4]{}; + + auxvel_fuse_mask[HVEL] = true; + auxvel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); + auxvel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); + auxvel_innov_gate[HVEL] = _params.auxvel_gate; + auxvel_obs_var[0] = _auxvel_sample_delayed.velVarNE(0); + auxvel_obs_var[1] = _auxvel_sample_delayed.velVarNE(1); + + fuseVelPosHeightSeq(auxvel_innov,auxvel_innov_gate, + auxvel_obs_var,auxvel_fuse_mask, + auxvel_innov_var,auxvel_test_ratio); + + _aux_vel_innov[0] = auxvel_innov[0]; + _aux_vel_innov[1] = auxvel_innov[1]; + + _aux_vel_innov_var[0] = auxvel_innov_var[0]; + _aux_vel_innov_var[1] = auxvel_innov_var[1]; + + _aux_vel_test_ratio = auxvel_test_ratio[HVEL]; + } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 1aa0e2da9e..062e97aabc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -63,31 +63,24 @@ public: // should be called every time new data is pushed into the filter bool update() override; - // gets the innovations of velocity and position measurements - // 0-2 vel, 3-5 pos - void get_vel_pos_innov(float vel_pos_innov[6]) override; + void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos); - // gets the innovations for of the NE auxiliary velocity measurement - void get_aux_vel_innov(float aux_vel_innov[2]) override; + void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos); // gets the innovations of the earth magnetic field measurements void get_mag_innov(float mag_innov[3]) override; - // gets the innovations of the heading measurement - void get_heading_innov(float *heading_innov) override; + void getEvVelPosInnov(float hvel[2], float& vvel, float hpos[2], float& vpos); + void get_heading_innov(float *heading_innov); - // gets the innovation variances of velocity and position measurements - // 0-2 vel, 3-5 pos - void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override; + void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos); // gets the innovation variances of the earth magnetic field measurements void get_mag_innov_var(float mag_innov_var[3]) override; - // gets the innovations of airspeed measurement - void get_airspeed_innov(float *airspeed_innov) override; + void getAuxVelInnov(float aux_vel_innov[2]); - // gets the innovation variance of the airspeed measurement - void get_airspeed_innov_var(float *airspeed_innov_var) override; + void getAuxVelInnovVar(float aux_vel_innov[2]); // gets the innovations of synthetic sideslip measurement void get_beta_innov(float *beta_innov) override; @@ -296,13 +289,6 @@ private: bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused bool _fuse_hor_vel_aux{false}; ///< true when auxiliary horizontal velocity measurement should be fused - float _posObsNoiseNE{0.0f}; ///< 1-STD observation noise used for the fusion of NE position data (m) - float _posInnovGateNE{1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check - - Vector3f _velObsVarNED; ///< 1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 - float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check - float _vvelInnovGate{1.0f}; ///< Number of standard deviations used for the vertical velocity fusion innovation consistency check - // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m) @@ -323,7 +309,7 @@ private: bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator - uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec) + uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift @@ -335,7 +321,7 @@ private: uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec) - Vector2f _last_known_posNE; ///< last known local NE position vector (m) + Vector2f _last_known_posNE{}; ///< last known local NE position vector (m) float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -369,10 +355,18 @@ private: Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance - float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) - float _vel_pos_innov_var[6] {}; ///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) - float _aux_vel_innov[2] {}; ///< NE auxiliary velocity innovations: (m/sec) + float _vel_pos_innov[6] {}; ///< velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) + float _vel_pos_innov_var[6] {}; ///< velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) + + float _gps_vel_pos_innov[6] {}; ///< GPS velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) + float _gps_vel_pos_innov_var[6] {}; ///< GPS velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) + + float _ev_vel_pos_innov[6] {}; ///< external vision velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m) + float _ev_vel_pos_innov_var[6] {}; ///< external vision velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) + + float _aux_vel_innov[2] {}; ///< horizontal auxiliary velocity innovations: (m/sec) + float _aux_vel_innov_var[2] {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) float _mag_innov[3] {}; ///< earth magnetic field innovations (Gauss) float _mag_innov_var[3] {}; ///< earth magnetic field innovation variance (Gauss**2) @@ -533,8 +527,13 @@ private: // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(); - // fuse velocity and position measurements (also barometer height) - void fuseVelPosHeight(); + // fuse velocity and position measurements sequentially + void fuseVelPosHeightSeq(const float (&innov)[6], const float (&innov_gate)[4], + const float (obs_var)[6], bool (&fuse_mask)[4], + float (&innov_var)[6], float (&test_ratio)[4]); + + // fuse single velocity and position measurement + void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); // reset velocity states of the ekf bool resetVelocity(); @@ -669,8 +668,8 @@ private: // control fusion of pressure altitude observations void controlBaroFusion(); - // control fusion of velocity and position observations - void controlVelPosFusion(); + // control fusion of fake position observations to constrain drift + void controlFakePosFusion(); // control fusion of auxiliary velocity observations void controlAuxVelFusion(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 60420ca3cb..e9e5fd311b 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -408,8 +408,8 @@ bool Ekf::realignYawGPS() float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1))); if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { - // check for excessive GPS velocity innovations - bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps; + // check for excessive horizontal GPS velocity innovations + bool badVelInnov = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) && _control_status.flags.gps; // calculate GPS course over ground angle float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); @@ -824,17 +824,20 @@ void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad); } -// gets the innovations of velocity and position measurements -// 0-2 vel, 3-5 pos -void Ekf::get_vel_pos_innov(float vel_pos_innov[6]) +void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) { - memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6); + memcpy(hvel, _gps_vel_pos_innov+0, sizeof(float) * 2); + memcpy(&vvel, _gps_vel_pos_innov+2, sizeof(float) * 1); + memcpy(hpos, _gps_vel_pos_innov+3, sizeof(float) * 2); + memcpy(&vpos, _gps_vel_pos_innov+5, sizeof(float) * 1); } -// gets the innovations for of the NE auxiliary velocity measurement -void Ekf::get_aux_vel_innov(float aux_vel_innov[2]) +void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) { - memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2); + memcpy(hvel, _gps_vel_pos_innov_var+0, sizeof(float) * 2); + memcpy(&vvel, _gps_vel_pos_innov_var+2, sizeof(float) * 1); + memcpy(hpos, _gps_vel_pos_innov_var+3, sizeof(float) * 2); + memcpy(&vpos, _gps_vel_pos_innov_var+5, sizeof(float) * 1); } // writes the innovations of the earth magnetic field measurements @@ -843,22 +846,34 @@ void Ekf::get_mag_innov(float mag_innov[3]) memcpy(mag_innov, _mag_innov, 3 * sizeof(float)); } -// gets the innovations of the airspeed measurement -void Ekf::get_airspeed_innov(float *airspeed_innov) +void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) { - memcpy(airspeed_innov, &_airspeed_innov, sizeof(float)); + memcpy(hvel, _ev_vel_pos_innov+0, sizeof(float) * 2); + memcpy(&vvel, _ev_vel_pos_innov+2, sizeof(float) * 1); + memcpy(hpos, _ev_vel_pos_innov+3, sizeof(float) * 2); + memcpy(&vpos, _ev_vel_pos_innov+5, sizeof(float) * 1); } -// gets the innovations of the synthetic sideslip measurements -void Ekf::get_beta_innov(float *beta_innov) +void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) +{ + memcpy(hvel, _ev_vel_pos_innov_var+0, sizeof(float) * 2); + memcpy(&vvel, _ev_vel_pos_innov_var+2, sizeof(float) * 1); + memcpy(hpos, _ev_vel_pos_innov_var+3, sizeof(float) * 2); + memcpy(&vpos, _ev_vel_pos_innov_var+5, sizeof(float) * 1); +} { memcpy(beta_innov, &_beta_innov, sizeof(float)); } -// gets the innovations of the heading measurement -void Ekf::get_heading_innov(float *heading_innov) +void Ekf::getAuxVelInnov(float aux_vel_innov[2]) { - memcpy(heading_innov, &_heading_innov, sizeof(float)); + memcpy(aux_vel_innov, _aux_vel_innov, sizeof(_aux_vel_innov)); +} + +void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) +{ + memcpy(aux_vel_innov_var, _aux_vel_innov_var, sizeof(_aux_vel_innov_var)); +} } // gets the innovation variances of velocity and position measurements @@ -1189,8 +1204,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; - bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f); - bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f); + bool gps_vel_innov_bad = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) || (_gps_vel_pos_test_ratio[VVEL] > 1.0f); + bool gps_pos_innov_bad = (_gps_vel_pos_test_ratio[HPOS] > 1.0f); bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f); soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; soln_status.flags.accel_error = _bad_vert_accel_detected; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index f1a53c2882..af8fb0c45c 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -67,28 +67,28 @@ public: virtual void resetStatesAndCovariances() = 0; virtual bool update() = 0; - // gets the innovations of velocity and position measurements - // 0-2 vel, 3-5 pos - virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0; + // gets the GPS innovations of velocity and position measurements + virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; + // gets the GPS innovation variances of velocity and position measurements + virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; - // gets the innovations for of the NE auxiliary velocity measurement - virtual void get_aux_vel_innov(float aux_vel_innov[2]) = 0; - // gets the innovations of the earth magnetic field measurements - virtual void get_mag_innov(float mag_innov[3]) = 0; + // gets the external vision innovations of velocity and position measurements + virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; + // gets the external vision innovation variances of velocity and position measurements + virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; // gets the innovation of airspeed measurement virtual void get_airspeed_innov(float *airspeed_innov) = 0; - // gets the innovation of the synthetic sideslip measurement - virtual void get_beta_innov(float *beta_innov) = 0; + // gets the innovations for of the horizontal auxiliary velocity measurement + virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0; + // gets the innovation variances for of the horizontal auxiliary velocity measurement + virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0; // gets the innovations of the heading measurement virtual void get_heading_innov(float *heading_innov) = 0; - // gets the innovation variances of velocity and position measurements - // 0-2 vel, 3-5 pos - virtual void get_vel_pos_innov_var(float vel_pos_innov_var[6]) = 0; // gets the innovation variances of the earth magnetic field measurements virtual void get_mag_innov_var(float mag_innov_var[3]) = 0; @@ -486,9 +486,9 @@ protected: // innovation consistency check monitoring ratios float _yaw_test_ratio{0.0f}; // yaw innovation consistency check ratio float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios - float _vel_pos_test_ratio[6] {}; // velocity and position innovation consistency check ratios - float _tas_test_ratio{0.0f}; // tas innovation consistency check ratio - float _terr_test_ratio{0.0f}; // height above terrain measurement innovation consistency check ratio + float _gps_vel_pos_test_ratio[4] {}; // GPS velocity and position innovation consistency check ratios + float _ev_vel_pos_test_ratio[4] {}; // EV velocity and position innovation consistency check ratios + float _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio float _beta_test_ratio{0.0f}; // sideslip innovation consistency check ratio float _drag_test_ratio[2] {}; // drag innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ac63e96395..752be5c8e8 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -45,276 +45,175 @@ #include #include -void Ekf::fuseVelPosHeight() +/** + * Update the EKF state with velocity and position measurements sequentially. [(vx vy) (vz) (x y) (z)] + * + * @param innov Input [vx vy vz x y z] + * @param innov_gate Input [vxy vz xy z] + * @param obs_var Input [vx vy vz x y z] + * @param fuse_mask Input/Output [vxy vz xy z] + * Specify which innovation components should be fused, + * components that do not pass innovations checks will be set to zero + * @param innov_var Ouput [vx vy vz x y z] + * @param test_ratio Output [vxy vz xy z] + */ +void Ekf::fuseVelPosHeightSeq(const float (&innov)[6], const float (&innov_gate)[4], + const float (obs_var)[6], bool (&fuse_mask)[4], + float (&innov_var)[6], float (&test_ratio)[4]) { - bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available - bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations - float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD] - float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations - float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used - float innovation[6]; // local copy of innovations for [VN,VE,VD,PN,PE,PD] - memcpy(innovation, _vel_pos_innov, sizeof(_vel_pos_innov)); + // check position, velocity and height innovations sequentially and if checks are passed fuse it + // treat 2D horizintal velocity, vertical velocity, 2D horizontal position and vertical height as separate sensors + // At the moment we still fuse velocity as 3D measurement, but this should be split in the future - // calculate innovations, innovations gate sizes and observation variances - if (_fuse_hor_vel || _fuse_hor_vel_aux) { - // enable fusion for NE velocity axes - fuse_map[0] = fuse_map[1] = true; + // horizontal and vertical velocity + if(fuse_mask[HVEL] && fuse_mask[VVEL]){ + innov_var[0] = P[4][4] + obs_var[0]; + innov_var[1] = P[5][5] + obs_var[1]; + test_ratio[HVEL] = fmaxf( sq(innov[0]) / (sq(innov_gate[HVEL]) * innov_var[0]), + sq(innov[1]) / (sq(innov_gate[HVEL]) * innov_var[1])); - // handle special case where we are getting velocity observations from an auxiliary source - if (!_fuse_hor_vel) { - innovation[0] = _aux_vel_innov[0]; - innovation[1] = _aux_vel_innov[1]; - } + innov_var[2] = P[6][6] + obs_var[2]; + test_ratio[VVEL] = sq(innov[2]) / (sq(innov_gate[VVEL]) * innov_var[2]); - // Set observation noise variance and innovation consistency check gate size for the NE position observations - R[0] = _velObsVarNED(0); - R[1] = _velObsVarNED(1); - gate_size[1] = gate_size[0] = _hvelInnovGate; + bool innov_check_pass = (test_ratio[HVEL] <= 1.0f) && (test_ratio[VVEL] <= 1.0f); + if (innov_check_pass) { + _time_last_vel_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_vel_NED = false; - } + // fuse the horizontal and vertical velocity measurements + fuseVelPosHeight(innov[0],innov_var[0],0); + fuseVelPosHeight(innov[1],innov_var[1],1); + fuseVelPosHeight(innov[2],innov_var[2],2); - if (_fuse_vert_vel) { - fuse_map[2] = true; - // observation variance - use receiver reported accuracy with parameter setting the minimum value - R[2] = _velObsVarNED(2); - // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP - R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc); - R[2] = R[2] * R[2]; - // innovation gate size - gate_size[2] = _vvelInnovGate; - } - - if (_fuse_pos) { - // enable fusion for the NE position axes - fuse_map[3] = fuse_map[4] = true; - - // Set observation noise variance and innovation consistency check gate size for the NE position observations - R[4] = R[3] = sq(_posObsNoiseNE); - gate_size[4] = gate_size[3] = _posInnovGateNE; - - } - - if (_fuse_height) { - if (_control_status.flags.baro_hgt) { - fuse_map[5] = true; - // vertical position innovation - baro measurement has opposite sign to earth z axis - innovation[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; - // observation variance - user parameter defined - R[5] = fmaxf(_params.baro_noise, 0.01f); - R[5] = R[5] * R[5]; - // innovation gate size - gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); - - // Compensate for positive static pressure transients (negative vertical position innovations) - // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. - float deadzone_start = 0.0f; - float deadzone_end = deadzone_start + _params.gnd_effect_deadzone; - - if (_control_status.flags.gnd_effect) { - if (innovation[5] < -deadzone_start) { - if (innovation[5] <= -deadzone_end) { - innovation[5] += deadzone_end; - - } else { - innovation[5] = -deadzone_start; - } - } - } - - } else if (_control_status.flags.gps_hgt) { - fuse_map[5] = true; - // vertical position innovation - gps measurement has opposite sign to earth z axis - innovation[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; - // observation variance - receiver defined and parameter limited - // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP - float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit); - R[5] = R[5] * R[5]; - // innovation gate size - gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); - - } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) { - fuse_map[5] = true; - // use range finder with tilt correction - innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, - _params.rng_gnd_clearance)) - _hgt_sensor_offset; - // observation variance - user parameter defined - R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); - // innovation gate size - gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); - - } else if (_control_status.flags.ev_hgt) { - fuse_map[5] = true; - // calculate the innovation assuming the external vision observation is in local NED frame - innovation[5] = _state.pos(2) - _ev_sample_delayed.pos(2); - // observation variance - defined externally - R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f); - R[5] = R[5] * R[5]; - // innovation gate size - gate_size[5] = fmaxf(_params.ev_pos_innov_gate, 1.0f); - } - - // update innovation class variable for logging purposes - _vel_pos_innov[5] = innovation[5]; - } - - // calculate innovation test ratios - for (unsigned obs_index = 0; obs_index < 6; obs_index++) { - if (fuse_map[obs_index]) { - // compute the innovation variance SK = HPH + R - unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state - _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index]; - // Compute the ratio of innovation to gate size - _vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) * - _vel_pos_innov_var[obs_index]); }else{ - _vel_pos_test_ratio[obs_index] = 0; + fuse_mask[HVEL] = fuse_mask[VVEL] = false; + _innov_check_fail_status.flags.reject_vel_NED = true; } } - // check position, velocity and height innovations - // treat 3D velocity, 2D position and height as separate sensors - // always pass position checks if using synthetic position measurements or yet to complete tilt alignment - // always pass height checks if yet to complete tilt alignment - bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) - && (_vel_pos_test_ratio[2] <= 1.0f); - innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; - bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align; - innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; - innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; + // horizontal position + if(fuse_mask[HPOS]){ + innov_var[3] = P[7][7] + obs_var[3]; + innov_var[4] = P[8][8] + obs_var[4]; + test_ratio[HPOS] = fmaxf( sq(innov[3]) / (sq(innov_gate[HPOS]) * innov_var[3]), + sq(innov[4]) / (sq(innov_gate[HPOS]) * innov_var[4])); - // record the successful velocity fusion event - if ((_fuse_hor_vel || _fuse_hor_vel_aux || _fuse_vert_vel) && vel_check_pass) { - _time_last_vel_fuse = _time_last_imu; - _innov_check_fail_status.flags.reject_vel_NED = false; - - } else if (!vel_check_pass) { - _innov_check_fail_status.flags.reject_vel_NED = true; - } - - _fuse_hor_vel = _fuse_hor_vel_aux = _fuse_vert_vel = false; - - // record the successful position fusion event - if (pos_check_pass && _fuse_pos) { - if (!_fuse_hpos_as_odom) { - _time_last_pos_fuse = _time_last_imu; - - } else { - _time_last_delpos_fuse = _time_last_imu; - } - - _innov_check_fail_status.flags.reject_pos_NE = false; - - } else if (!pos_check_pass) { - _innov_check_fail_status.flags.reject_pos_NE = true; - } - - _fuse_pos = false; - - // record the successful height fusion event - if (innov_check_pass_map[5] && _fuse_height) { - _time_last_hgt_fuse = _time_last_imu; - _innov_check_fail_status.flags.reject_pos_D = false; - - } else if (!innov_check_pass_map[5]) { - _innov_check_fail_status.flags.reject_pos_D = true; - } - - _fuse_height = false; - - for (unsigned obs_index = 0; obs_index < 6; obs_index++) { - // skip fusion if not requested or checks have failed - if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) { - continue; - } - - unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < _k_num_states; row++) { - Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index]; - } - - // update covariance matrix via Pnew = (I - KH)P - float KHP[_k_num_states][_k_num_states]; - - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - KHP[row][column] = Kfusion[row] * P[state_index][column]; - } - } - - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - - for (int i = 0; i < _k_num_states; i++) { - if (P[i][i] < KHP[i][i]) { - // zero rows and columns - zeroRows(P, i, i); - zeroCols(P, i, i); - - //flag as unhealthy - healthy = false; - - // update individual measurement health status - if (obs_index == 0) { - _fault_status.flags.bad_vel_N = true; - - } else if (obs_index == 1) { - _fault_status.flags.bad_vel_E = true; - - } else if (obs_index == 2) { - _fault_status.flags.bad_vel_D = true; - - } else if (obs_index == 3) { - _fault_status.flags.bad_pos_N = true; - - } else if (obs_index == 4) { - _fault_status.flags.bad_pos_E = true; - - } else if (obs_index == 5) { - _fault_status.flags.bad_pos_D = true; - } + bool innov_check_pass = (test_ratio[HPOS] <= 1.0f) || !_control_status.flags.tilt_align; + if (innov_check_pass) { + if (!_fuse_hpos_as_odom) { + _time_last_pos_fuse = _time_last_imu; } else { - // update individual measurement health status - if (obs_index == 0) { - _fault_status.flags.bad_vel_N = false; - - } else if (obs_index == 1) { - _fault_status.flags.bad_vel_E = false; - - } else if (obs_index == 2) { - _fault_status.flags.bad_vel_D = false; - - } else if (obs_index == 3) { - _fault_status.flags.bad_pos_N = false; - - } else if (obs_index == 4) { - _fault_status.flags.bad_pos_E = false; - - } else if (obs_index == 5) { - _fault_status.flags.bad_pos_D = false; - } + _time_last_delpos_fuse = _time_last_imu; } - } + _innov_check_fail_status.flags.reject_pos_NE = false; - // only apply covariance and state corrections if healthy - if (healthy) { - // apply the covariance corrections - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { - P[row][column] = P[row][column] - KHP[row][column]; - } - } + // fuse the horizontal position measurements + fuseVelPosHeight(innov[3],innov_var[3],3); + fuseVelPosHeight(innov[4],innov_var[4],4); - // correct the covariance matrix for gross errors - fixCovarianceErrors(); - - // apply the state corrections - fuse(Kfusion, innovation[obs_index]); + }else{ + fuse_mask[HPOS] = false; + _innov_check_fail_status.flags.reject_pos_NE = true; } } + + // vertical position + if(fuse_mask[VPOS]){ + innov_var[5] = P[9][9] + obs_var[5]; + test_ratio[VPOS] = sq(innov[5]) / (sq(innov_gate[VPOS]) * innov_var[5]); + + bool innov_check_pass = (test_ratio[VPOS] <= 1.0f) || !_control_status.flags.tilt_align; + if (innov_check_pass) { + _time_last_hgt_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_pos_D = false; + + // fuse the horizontal position measurements + fuseVelPosHeight(innov[5],innov_var[5],5); + + }else{ + fuse_mask[VPOS] = false; + _innov_check_fail_status.flags.reject_pos_D = true; + } + } + +} + +// Helper function that fuses a single velocity or position measurement +void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +{ + float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used. + unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < _k_num_states; row++) { + Kfusion[row] = P[row][state_index] / innov_var; + } + + float KHP[_k_num_states][_k_num_states]; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + KHP[row][column] = Kfusion[row] * P[state_index][column]; + } + } + + // if the covariance correction will result in a negative variance, then + // the covariance matrix is unhealthy and must be corrected + bool healthy = true; + + for (int i = 0; i < _k_num_states; i++) { + if (P[i][i] < KHP[i][i]) { + // zero rows and columns + zeroRows(P, i, i); + zeroCols(P, i, i); + + healthy = false; + + setVelPosFaultStatus(obs_index,true); + + } else { + setVelPosFaultStatus(obs_index,false); + } + } + + + // only apply covariance and state corrections if healthy + if (healthy) { + // apply the covariance corrections + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] = P[row][column] - KHP[row][column]; + } + } + + // correct the covariance matrix for gross errors + fixCovarianceErrors(); + + // apply the state corrections + fuse(Kfusion, innov); + + } +} + +void Ekf::setVelPosFaultStatus(const int index, const bool status) +{ + if (index == 0) { + _fault_status.flags.bad_vel_N = status; + + } else if (index == 1) { + _fault_status.flags.bad_vel_E = status; + + } else if (index == 2) { + _fault_status.flags.bad_vel_D = status; + + } else if (index == 3) { + _fault_status.flags.bad_pos_N = status; + + } else if (index == 4) { + _fault_status.flags.bad_pos_E = status; + + } else if (index == 5) { + _fault_status.flags.bad_pos_D = status; + } }