diff --git a/EKF/common.h b/EKF/common.h index e8f4ab4b94..5737f1d840 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -210,12 +210,6 @@ 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) @@ -407,18 +401,19 @@ union fault_status_u { // define structure used to communicate innovation test failures union innovation_fault_status_u { struct { - bool reject_vel_NED: 1; ///< 0 - true if velocity observations have been rejected - bool reject_pos_NE: 1; ///< 1 - true if horizontal position observations have been rejected - bool reject_pos_D: 1; ///< 2 - true if true if vertical position observations have been rejected - bool reject_mag_x: 1; ///< 3 - true if the X magnetometer observation has been rejected - bool reject_mag_y: 1; ///< 4 - true if the Y magnetometer observation has been rejected - bool reject_mag_z: 1; ///< 5 - true if the Z magnetometer observation has been rejected - bool reject_yaw: 1; ///< 6 - true if the yaw observation has been rejected - bool reject_airspeed: 1; ///< 7 - true if the airspeed observation has been rejected - bool reject_sideslip: 1; ///< 8 - true if the synthetic sideslip observation has been rejected - bool reject_hagl: 1; ///< 9 - true if the height above ground observation has been rejected - bool reject_optflow_X: 1; ///< 10 - true if the X optical flow observation has been rejected - bool reject_optflow_Y: 1; ///< 11 - true if the Y optical flow observation has been rejected + bool reject_hor_vel: 1; ///< 0 - true if horizontal velocity observations have been rejected + bool reject_ver_vel: 1; ///< 1 - true if vertical velocity observations have been rejected + bool reject_hor_pos: 1; ///< 2 - true if horizontal position observations have been rejected + bool reject_ver_pos: 1; ///< 3 - true if true if vertical position observations have been rejected + bool reject_mag_x: 1; ///< 4 - true if the X magnetometer observation has been rejected + bool reject_mag_y: 1; ///< 5 - true if the Y magnetometer observation has been rejected + bool reject_mag_z: 1; ///< 6 - true if the Z magnetometer observation has been rejected + bool reject_yaw: 1; ///< 7 - true if the yaw observation has been rejected + bool reject_airspeed: 1; ///< 8 - true if the airspeed observation has been rejected + bool reject_sideslip: 1; ///< 9 - true if the synthetic sideslip observation has been rejected + bool reject_hagl: 1; ///< 10 - true if the height above ground observation has been rejected + bool reject_optflow_X: 1; ///< 11 - true if the X optical flow observation has been rejected + bool reject_optflow_Y: 1; ///< 12 - true if the Y optical flow observation has been rejected } flags; uint16_t value; diff --git a/EKF/control.cpp b/EKF/control.cpp index c2988b9995..bc77d664c3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -265,13 +265,13 @@ void Ekf::controlExternalVisionFusion() } } - 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) { - ev_fuse_mask[HPOS] = true; + + Vector3f ev_pos_obs_var{}; + Vector2f ev_pos_innov_gates{}; // correct position and height for offset relative to IMU Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; @@ -301,11 +301,11 @@ void Ekf::controlExternalVisionFusion() ev_delta_pos = _ev_rot_mat * ev_delta_pos; // use the change in position since the last measurement - _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); + _ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); + _ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); // observation 1-STD error, incremental pos observation is expected to have more uncertainty - ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.5f); + ev_pos_obs_var(0) = ev_pos_obs_var(1) = fmaxf(_ev_sample_delayed.posErr, 0.5f); } // record observation and estimate for use next time @@ -319,15 +319,15 @@ void Ekf::controlExternalVisionFusion() if (_params.fusion_mode & MASK_ROTATE_EV) { ev_pos_meas = _ev_rot_mat * ev_pos_meas; } - _ev_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); - _ev_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); + _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); + _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); // observation 1-STD error - ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.01f); + ev_pos_obs_var(0) = ev_pos_obs_var(1) = 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) { + if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it - if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) { + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_vel_fuse) > (uint64_t)1E6)) { resetVelocity(); } @@ -336,12 +336,16 @@ void Ekf::controlExternalVisionFusion() } // innovation gate size - ev_innov_gate[HPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f); + ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f); + + fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); } // determine if we should use the velocity observations if (_control_status.flags.ev_vel) { - ev_fuse_mask[HVEL] = ev_fuse_mask[VVEL] = true; + + Vector3f ev_vel_obs_var{}; + Vector2f ev_vel_innov_gates{}; Vector3f vel_aligned{_ev_sample_delayed.vel}; @@ -357,34 +361,31 @@ void Ekf::controlExternalVisionFusion() Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; vel_aligned -= vel_offset_earth; - _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); + _ev_vel_innov(0) = _state.vel(0) - vel_aligned(0); + _ev_vel_innov(1) = _state.vel(1) - vel_aligned(1); + _ev_vel_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) { + if ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it - if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) { + if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_pos_fuse) > (uint64_t)1E6)) { resetVelocity(); } } // observation 1-STD error - ev_obs_var[0] = ev_obs_var[1] = ev_obs_var[2] = fmaxf(_ev_sample_delayed.velErr, 0.01f); + ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = fmaxf(_ev_sample_delayed.velErr, 0.01f); // innovation gate size - ev_innov_gate[HVEL] = ev_innov_gate[VVEL] = fmaxf(_params.ev_vel_innov_gate, 1.0f); - } + ev_vel_innov_gates(0) = ev_vel_innov_gates(1) = fmaxf(_params.ev_vel_innov_gate, 1.0f); - // Fuse available NED position data into the main filter - 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); + fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); + fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); } // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { fuseHeading(); - } } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel) @@ -652,13 +653,13 @@ void Ekf::controlGpsFusion() if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time // with no aiding we need to do something - bool do_reset = ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) + bool do_reset = ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) && ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max) - && ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) + && ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) && ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max); // We haven't had an absolute position fix for a longer time so need to do something - do_reset = do_reset || ((_time_last_imu - _time_last_pos_fuse) > (2 * _params.reset_timeout_max)); + do_reset = do_reset || ((_time_last_imu - _time_last_hor_pos_fuse) > (2 * _params.reset_timeout_max)); if (do_reset) { // use GPS velocity data to check and correct yaw angle if a FW vehicle @@ -673,8 +674,8 @@ void Ekf::controlGpsFusion() ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS"); // Reset the timeout counters - _time_last_pos_fuse = _time_last_imu; - _time_last_vel_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _time_last_imu; } } @@ -682,13 +683,11 @@ void Ekf::controlGpsFusion() // Only use GPS data for position and velocity aiding if enabled if (_control_status.flags.gps) { - 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; + Vector2f gps_vel_innov_gates{}; // [horizontal vertical] + Vector2f gps_pos_innov_gates{}; // [horizontal vertical] + Vector3f gps_vel_obs_var{}; + Vector3f gps_pos_obs_var{}; // correct velocity for offset relative to IMU Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); @@ -709,32 +708,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 - gps_obs_var[3] = gps_obs_var[4] = fmaxf(_gps_sample_delayed.hacc, lower_limit); + gps_pos_obs_var(0) = gps_pos_obs_var(1) = 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); - gps_obs_var[3] = gps_obs_var[4] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); + gps_pos_obs_var(0) = gps_pos_obs_var(1) = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); } - gps_obs_var[0] = gps_obs_var[1] = gps_obs_var[2] = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); + gps_vel_obs_var(0) = gps_vel_obs_var(1) = gps_vel_obs_var(2) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)); // calculate innovations - _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); + _gps_vel_innov(0) = _state.vel(0) - _gps_sample_delayed.vel(0); + _gps_vel_innov(1) = _state.vel(1) - _gps_sample_delayed.vel(1); + _gps_vel_innov(2) = _state.vel(2) - _gps_sample_delayed.vel(2); + _gps_pos_innov(0) = _state.pos(0) - _gps_sample_delayed.pos(0); + _gps_pos_innov(1) = _state.pos(1) - _gps_sample_delayed.pos(1); // set innovation gate size - 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); + gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f); + gps_pos_innov_gates(1) = 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); + fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); + fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); + fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { @@ -761,8 +760,8 @@ void Ekf::controlHeightSensorTimeouts() // Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim); bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are independent - (sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error - (_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling + (sq(_gps_pos_innov(2) * fmaxf(fabsf(_gps_vel_innov(2)),fabsf(_ev_vel_innov(2)))) > var_product_lim * (_gps_pos_innov_var(2) * fmaxf(fabsf(_gps_vel_innov_var(2)),fabsf(_ev_vel_innov_var(2))))) && // vertical position and velocity sensors are in agreement that we have a significant error + (_gps_vel_innov(2) > 0.0f || _ev_vel_innov(2) > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh @@ -984,12 +983,6 @@ void Ekf::controlHeightSensorTimeouts() void Ekf::controlHeightFusion() { - 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(); @@ -998,7 +991,7 @@ void Ekf::controlHeightFusion() if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { setControlRangeHeight(); - height_fuse_mask[VPOS] = true; + _fuse_height = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1013,7 +1006,7 @@ void Ekf::controlHeightFusion() } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); - height_fuse_mask[VPOS] = true; + _fuse_height = 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 @@ -1031,7 +1024,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 - height_fuse_mask[VPOS] = true; + _fuse_height = true; // we have just switched to using gps height, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1044,7 +1037,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(); - height_fuse_mask[VPOS] = _range_data_ready; + _fuse_height = _range_data_ready; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1066,7 +1059,7 @@ void Ekf::controlHeightFusion() } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); - height_fuse_mask[VPOS] = true; + _fuse_height = 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 @@ -1080,7 +1073,7 @@ void Ekf::controlHeightFusion() if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) { setControlRangeHeight(); - height_fuse_mask[VPOS] = true; + _fuse_height = true; // we have just switched to using range finder, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1095,7 +1088,7 @@ void Ekf::controlHeightFusion() } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) { setControlGPSHeight(); - height_fuse_mask[VPOS] = true; + _fuse_height = true; // we have just switched to using gps height, calculate height sensor offset such that current // measurement matches our current height estimate @@ -1105,7 +1098,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 - height_fuse_mask[VPOS] = true; + _fuse_height = 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 @@ -1120,14 +1113,14 @@ void Ekf::controlHeightFusion() // 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; + _fuse_height = 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 - height_fuse_mask[VPOS] = true; + _fuse_height = 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 @@ -1138,7 +1131,7 @@ void Ekf::controlHeightFusion() // 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; + _fuse_height = true; } } @@ -1164,18 +1157,25 @@ void Ekf::controlHeightFusion() } - height_fuse_mask[VPOS] = true; + _fuse_height = true; } - if (height_fuse_mask[VPOS]) { + if (_fuse_height) { + + Vector2f height_innov_gate{}; + Vector2f height_test_ratio{}; + Vector3f height_obs_var{}; + Vector3f height_innov_var{}; + Vector3f height_innov{}; + 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; + height_innov(2) = _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)); + height_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f)); // innovation gate size - height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f); + height_innov_gate(1) = 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. @@ -1183,53 +1183,52 @@ void Ekf::controlHeightFusion() 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; + if (height_innov(2) < -deadzone_start) { + if (height_innov(2) <= -deadzone_end) { + height_innov(2) += deadzone_end; } else { - height_innov[5] = -deadzone_start; + height_innov(2) = -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; + height_innov(2) = _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)); + height_obs_var(2) = 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); + height_innov_gate(1) = 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, + height_innov(2) = _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); + height_obs_var(2) = 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); + height_innov_gate(1) = 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); + height_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); // observation variance - defined externally - height_obs_var[5] = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); + height_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f)); // innovation gate size - height_innov_gate[VPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f); + height_innov_gate(1) = 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); + fuseVerticalPosition(height_innov,height_innov_gate, + height_obs_var, 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]; + _gps_pos_innov(2) = height_innov(2); + _gps_pos_innov_var(2) = height_innov_var(2); + _gps_pos_test_ratio(1) = height_test_ratio(1); } } @@ -1371,9 +1370,6 @@ void Ekf::controlFakePosFusion() { // 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 - 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 && @@ -1386,6 +1382,11 @@ void Ekf::controlFakePosFusion() // Fuse synthetic position observations every 200msec if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) { + + Vector3f fake_gps_pos_obs_var{}; + Vector2f fake_gps_pos_innov_gate{}; + + // Reset position and velocity states if we re-commence this aiding method if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) { resetPosition(); @@ -1397,27 +1398,23 @@ void Ekf::controlFakePosFusion() } } - // Fuse horizontal position - fake_gps_fuse_mask[HPOS] = true; - _time_last_fake_pos = _time_last_imu; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - fake_gps_obs_var[3] = fake_gps_obs_var[4] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); + fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); } else { - fake_gps_obs_var[3] = fake_gps_obs_var[4] = 0.5f; + fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = 0.5f; } - _gps_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); - _gps_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); + _gps_pos_innov(0) = _state.pos(0) - _last_known_posNE(0); + _gps_pos_innov(1) = _state.pos(1) - _last_known_posNE(1); // glitch protection is not required so set gate to a large value - fake_gps_innov_gate[HPOS] = 100.0f; + fake_gps_pos_innov_gate(0) = 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); + fuseHorizontalPosition(_gps_pos_innov, fake_gps_pos_innov_gate, fake_gps_pos_obs_var, + _gps_pos_innov_var, _gps_pos_test_ratio); } } else { @@ -1433,31 +1430,17 @@ void Ekf::controlAuxVelFusion() if (data_ready && primary_aiding) { - 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]{}; + Vector2f aux_vel_innov_gate{}; + Vector3f aux_vel_obs_var{}; - 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); + _aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0); + _aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1); + aux_vel_innov_gate(0) = _params.auxvel_gate; + aux_vel_obs_var(0) = _auxvel_sample_delayed.velVarNE(0); + aux_vel_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]; + fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var, + _aux_vel_innov_var, _aux_vel_test_ratio); } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 1cfa93165d..cc749e7b01 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -302,9 +302,9 @@ bool Ekf::initialiseFilter() // reset the essential fusion timeout counters _time_last_hgt_fuse = _time_last_imu; - _time_last_pos_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _time_last_imu; _time_last_delpos_fuse = _time_last_imu; - _time_last_vel_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _time_last_imu; _time_last_hagl_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu; diff --git a/EKF/ekf.h b/EKF/ekf.h index bf3b8499d9..64b726981c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -330,10 +330,12 @@ private: 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 - uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) + // TODO: Split those into sensor and measurement specifics + uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) + uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) + uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) + uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec) - uint64_t _time_last_vel_fuse{0}; ///< time the last fusion of velocity measurements was performed (uSec) - uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of height measurements was performed (uSec) uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) 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) @@ -373,17 +375,20 @@ private: Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance - 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) + Vector3f _gps_vel_innov {}; ///< GPS velocity innovations (m/sec) + Vector3f _gps_vel_innov_var {}; ///< GPS velocity innovation variances ((m/sec)**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) + Vector3f _gps_pos_innov {}; ///< GPS position innovations (m) + Vector3f _gps_pos_innov_var {}; ///< GPS position innovation variances (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) + Vector3f _ev_vel_innov {}; ///< external vision velocity innovations (m/sec) + Vector3f _ev_vel_innov_var {}; ///< external vision velocity innovation variances ((m/sec)**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) + Vector3f _ev_pos_innov {}; ///< external vision position innovations (m) + Vector3f _ev_pos_innov_var {}; ///< external vision position 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) float _heading_innov{0.0f}; ///< heading measurement innovation (rad) float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) @@ -546,11 +551,6 @@ private: // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(); - // 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); @@ -560,6 +560,18 @@ private: // fuse optical flow line of sight rate measurements void fuseOptFlow(); + bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); + + bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); + + bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); + + bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); + // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable bool calcOptFlowBodyRateComp(); @@ -789,37 +801,26 @@ private: // sensor measurement float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted); - // stop GPS fusion void stopGpsFusion(); - // stop GPS position fusion void stopGpsPosFusion(); - // stop GPS velocity fusion void stopGpsVelFusion(); - // stop GPS yaw fusion void stopGpsYawFusion(); - // stop external vision fusion void stopEvFusion(); - // stop external vision position fusion void stopEvPosFusion(); - // stop external vision velocity fusion void stopEvVelFusion(); - // stop external vision yaw fusion void stopEvYawFusion(); - // stop auxiliary horizontal velocity fusion void stopAuxVelFusion(); - // stop optical flow fusion void stopFlowFusion(); - // Helper function to set velPos fault status void setVelPosFaultStatus(const int index, const bool status); }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 1401bf2d36..0a086dfacc 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -418,7 +418,7 @@ bool Ekf::realignYawGPS() if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { // check for excessive horizontal GPS velocity innovations - bool badVelInnov = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) && _control_status.flags.gps; + bool badVelInnov = (_gps_vel_test_ratio(0) > 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)); @@ -835,65 +835,75 @@ void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) { - 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); + hvel[0] = _gps_vel_innov(0); + hvel[1] = _gps_vel_innov(1); + vvel = _gps_vel_innov(2); + hpos[0] = _gps_pos_innov(0); + hpos[1] = _gps_pos_innov(1); + vpos = _gps_pos_innov(2); } void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) { - 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); + hvel[0] = _gps_vel_innov_var(0); + hvel[1] = _gps_vel_innov_var(1); + vvel = _gps_vel_innov_var(2); + hpos[0] = _gps_pos_innov_var(0); + hpos[1] = _gps_pos_innov_var(1); + vpos = _gps_pos_innov_var(2); } void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) { - memcpy(&hvel, _gps_vel_pos_test_ratio+HVEL, sizeof(float)); - memcpy(&vvel, _gps_vel_pos_test_ratio+VVEL, sizeof(float)); - memcpy(&hpos, _gps_vel_pos_test_ratio+HPOS, sizeof(float)); - memcpy(&vpos, _gps_vel_pos_test_ratio+VPOS, sizeof(float)); + hvel = _gps_vel_test_ratio(0); + vvel = _gps_vel_test_ratio(1); + hpos = _gps_pos_test_ratio(0); + vpos = _gps_pos_test_ratio(1); } void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) { - 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); + hvel[0] = _ev_vel_innov(0); + hvel[1] = _ev_vel_innov(1); + vvel = _ev_vel_innov(2); + hpos[0] = _ev_pos_innov(0); + hpos[1] = _ev_pos_innov(1); + vpos = _ev_pos_innov(2); } 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); + hvel[0] = _ev_vel_innov_var(0); + hvel[1] = _ev_vel_innov_var(1); + vvel = _ev_vel_innov_var(2); + hpos[0] = _ev_pos_innov_var(0); + hpos[1] = _ev_pos_innov_var(1); + vpos = _ev_pos_innov_var(2); } void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) { - memcpy(&hvel, _ev_vel_pos_test_ratio+HVEL, sizeof(float)); - memcpy(&vvel, _ev_vel_pos_test_ratio+VVEL, sizeof(float)); - memcpy(&hpos, _ev_vel_pos_test_ratio+HPOS, sizeof(float)); - memcpy(&vpos, _ev_vel_pos_test_ratio+VPOS, sizeof(float)); + hvel = _ev_vel_test_ratio(0); + vvel = _ev_vel_test_ratio(1); + hpos = _ev_pos_test_ratio(0); + vpos = _ev_pos_test_ratio(1); } void Ekf::getAuxVelInnov(float aux_vel_innov[2]) { - memcpy(aux_vel_innov, _aux_vel_innov, sizeof(_aux_vel_innov)); + aux_vel_innov[0] = _aux_vel_innov(0); + aux_vel_innov[1] = _aux_vel_innov(1); } void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) { - memcpy(aux_vel_innov_var, _aux_vel_innov_var, sizeof(_aux_vel_innov_var)); + aux_vel_innov_var[0] = _aux_vel_innov_var(0); + aux_vel_innov_var[1] = _aux_vel_innov_var(1); } void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) { - memcpy(&aux_vel_innov_ratio, &_aux_vel_test_ratio, sizeof(_aux_vel_test_ratio)); + aux_vel_innov_ratio = _aux_vel_test_ratio(0); } void Ekf::getFlowInnov(float flow_innov[2]) @@ -908,22 +918,22 @@ void Ekf::getFlowInnovVar(float flow_innov_var[2]) void Ekf::getFlowInnovRatio(float &flow_innov_ratio) { - memcpy(&flow_innov_ratio, &_optflow_test_ratio, sizeof(_optflow_test_ratio)); + flow_innov_ratio = _optflow_test_ratio; } void Ekf::getHeadingInnov(float &heading_innov) { - memcpy(&heading_innov, &_heading_innov, sizeof(_heading_innov)); + heading_innov = _heading_innov; } void Ekf::getHeadingInnovVar(float &heading_innov_var) { - memcpy(&heading_innov_var, &_heading_innov_var, sizeof(_heading_innov_var)); + heading_innov_var = _heading_innov_var; } void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) { - memcpy(&heading_innov_ratio, &_yaw_test_ratio, sizeof(_yaw_test_ratio)); + heading_innov_ratio = _yaw_test_ratio; } void Ekf::getMagInnov(float mag_innov[3]) @@ -958,47 +968,47 @@ void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) void Ekf::getAirspeedInnov(float &airspeed_innov) { - memcpy(&airspeed_innov, &_airspeed_innov, sizeof(_airspeed_innov)); + airspeed_innov = _airspeed_innov; } void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) { - memcpy(&airspeed_innov_var, &_airspeed_innov_var, sizeof(_airspeed_innov_var)); + airspeed_innov_var = _airspeed_innov_var; } void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) { - memcpy(&airspeed_innov_ratio, &_tas_test_ratio, sizeof(_tas_test_ratio)); + airspeed_innov_ratio = _tas_test_ratio; } void Ekf::getBetaInnov(float &beta_innov) { - memcpy(&beta_innov, &_beta_innov, sizeof(_beta_innov)); + beta_innov = _beta_innov; } void Ekf::getBetaInnovVar(float &beta_innov_var) { - memcpy(&beta_innov_var, &_beta_innov_var, sizeof(_beta_innov_var)); + beta_innov_var = _beta_innov_var; } void Ekf::getBetaInnovRatio(float &beta_innov_ratio) { - memcpy(&beta_innov_ratio, &_beta_test_ratio, sizeof(_beta_test_ratio)); + beta_innov_ratio = _beta_test_ratio; } void Ekf::getHaglInnov(float &hagl_innov) { - memcpy(&hagl_innov, &_hagl_innov, sizeof(_hagl_innov)); + hagl_innov = _hagl_innov; } void Ekf::getHaglInnovVar(float &hagl_innov_var) { - memcpy(&hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var)); + hagl_innov_var = _hagl_innov_var; } void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) { - memcpy(&hagl_innov_ratio, &_hagl_test_ratio, sizeof(_hagl_test_ratio)); + hagl_innov_ratio = _hagl_test_ratio; } // get GPS check status @@ -1121,8 +1131,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) { - hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4]))); + if (_is_dead_reckoning && (_control_status.flags.gps)) { + hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); + } + else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) { + hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); } *ekf_eph = hpos_err; @@ -1138,8 +1151,8 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) { - hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4]))); + if (_is_dead_reckoning && _control_status.flags.gps) { + hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); } *ekf_eph = hpos_err; @@ -1162,15 +1175,16 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1])); } - if (_control_status.flags.gps || _control_status.flags.ev_pos) { - vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1]))); + if (_control_status.flags.gps) { + vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1)))); + } + else if (_control_status.flags.ev_pos) { + vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); } if (_control_status.flags.ev_vel) { - // What is the right thing to do here -// vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1]))); + vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1)))); } - hvel_err = math::max(hvel_err, vel_err_conservative); } @@ -1270,15 +1284,15 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &gps_ve // return the largest magnetometer innovation test ratio mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]))); // return the largest NED GPS velocity innovation test ratio - gps_vel = sqrtf(math::max(_gps_vel_pos_test_ratio[HVEL], _gps_vel_pos_test_ratio[VVEL])); + gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1))); // return the largest NE GPS position innovation test ratio - gps_pos = sqrtf(_gps_vel_pos_test_ratio[HPOS]); + gps_pos = sqrtf(_gps_pos_test_ratio(0)); // return the largest external vision velocity innovation test ratio - ev_vel = sqrtf(math::max(_ev_vel_pos_test_ratio[HVEL], _ev_vel_pos_test_ratio[VVEL])); + ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))); // return the largest horizontal external vision position innovation test ratio - ev_pos = sqrtf(_ev_vel_pos_test_ratio[HPOS]); + ev_pos = sqrtf(_ev_pos_test_ratio(0)); // return the vertical position innovation test ratio - hgt = sqrtf(_gps_vel_pos_test_ratio[VPOS]); + hgt = sqrtf(_gps_pos_test_ratio(0)); // return the airspeed fusion innovation test ratio tas = sqrtf(_tas_test_ratio); // return the terrain height innovation test ratio @@ -1302,8 +1316,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 = (_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 gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f); + bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 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; @@ -1438,8 +1452,8 @@ bool Ekf::global_position_is_valid() void Ekf::update_deadreckoning_status() { bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) - && (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max) - || ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max) + && (((_time_last_imu - _time_last_hor_pos_fuse) <= _params.no_aid_timeout_max) + || ((_time_last_imu - _time_last_hor_vel_fuse) <= _params.no_aid_timeout_max) || ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max)); bool optFlowAiding = _control_status.flags.opt_flow && ((_time_last_imu - _time_last_of_fuse) <= _params.no_aid_timeout_max); bool airDataAiding = _control_status.flags.wind && ((_time_last_imu - _time_last_arsp_fuse) <= _params.no_aid_timeout_max) && ((_time_last_imu - _time_last_beta_fuse) <= _params.no_aid_timeout_max); @@ -1892,16 +1906,16 @@ void Ekf::stopGpsPosFusion() { _control_status.flags.gps = false; _control_status.flags.gps_hgt = false; - memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3); - memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3); - memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); + _gps_pos_innov.setZero(); + _gps_pos_innov_var.setZero(); + _gps_pos_test_ratio.setZero(); } void Ekf::stopGpsVelFusion() { - memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3); - memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3); - memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2); + _gps_vel_innov.setZero(); + _gps_vel_innov_var.setZero(); + _gps_vel_test_ratio.setZero(); } void Ekf::stopGpsYawFusion() @@ -1919,27 +1933,29 @@ void Ekf::stopEvFusion() void Ekf::stopEvPosFusion() { _control_status.flags.ev_pos = false; - memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3); - memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3); - memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); + _ev_pos_innov.setZero(); + _ev_pos_innov_var.setZero(); + _ev_pos_test_ratio.setZero(); } void Ekf::stopEvVelFusion() { _control_status.flags.ev_vel = false; - memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3); - memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3); - memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);} + _ev_vel_innov.setZero(); + _ev_vel_innov_var.setZero(); + _ev_vel_test_ratio.setZero(); +} void Ekf::stopEvYawFusion() { _control_status.flags.ev_yaw = false; - } void Ekf::stopAuxVelFusion() { - // TODO: Add proper handling of auxiliar velocity fusion + _aux_vel_innov.setZero(); + _aux_vel_innov_var.setZero(); + _aux_vel_test_ratio.setZero(); } void Ekf::stopFlowFusion() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index d50439b518..8d78b31d9e 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -67,82 +67,44 @@ public: virtual void resetStatesAndCovariances() = 0; virtual bool update() = 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 GPS innovation test ratios of velocity and position measurements virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 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 external vision innovation test ratios of velocity and position measurements virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 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 innovation test ratios of the horizontal auxiliary velocity measurement virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0; - - // gets the innovation of the flow measurement virtual void getFlowInnov(float flow_innov[2]) = 0; - // gets the innovation variance of the flow measurement virtual void getFlowInnovVar(float flow_innov_var[2]) = 0; - // gets the innovation test ratios of the flow measurement virtual void getFlowInnovRatio(float &flow_innov_ratio) = 0; - - // gets the innovations of the heading measurement virtual void getHeadingInnov(float &heading_innov) = 0; - // gets the innovation variance of the heading measurement virtual void getHeadingInnovVar(float &heading_innov_var) = 0; - // gets the innovation test ratios of the heading measurement virtual void getHeadingInnovRatio(float &heading_innov_ratio) = 0; - - // gets the innovations of the earth magnetic field measurements virtual void getMagInnov(float mag_innov[3]) = 0; - // gets the innovation variances of the earth magnetic field measurements virtual void getMagInnovVar(float mag_innov_var[3]) = 0; - // gets the innovation test ratios of the earth magnetic field measurements virtual void getMagInnovRatio(float &mag_innov_ratio) = 0; - - // gets the innovation of the drag specific force measurement virtual void getDragInnov(float drag_innov[2]) = 0; - // gets the innovation variance of the drag specific force measurement virtual void getDragInnovVar(float drag_innov_var[2]) = 0; - // gets the innovation test ratios of the drag specific force measurement virtual void getDragInnovRatio(float drag_innov_ratio[2]) = 0; - // gets the innovation of airspeed measurement virtual void getAirspeedInnov(float &airspeed_innov) = 0; - // gets the innovation variance of the airspeed measurement virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) = 0; - // gets the innovation test ratios of the airspeed measurement virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) = 0; - - // gets the innovation of the synthetic sideslip measurement virtual void getBetaInnov(float &beta_innov) = 0; - // gets the innovation variance of the synthetic sideslip measurement virtual void getBetaInnovVar(float &get_beta_innov_var) = 0; - // gets the innovation test ratios of the synthetic sideslip measurement virtual void getBetaInnovRatio(float &beta_innov_ratio) = 0; - - // gets the innovation of the HAGL measurement virtual void getHaglInnov(float &hagl_innov) = 0; - // gets the innovation variance of the HAGL measurement virtual void getHaglInnovVar(float &hagl_innov_var) = 0; - // gets the innovation test ratios of the HAGL measurement virtual void getHaglInnovRatio(float &hagl_innov_ratio) = 0; @@ -512,10 +474,12 @@ protected: // innovation consistency check monitoring ratios float _yaw_test_ratio{}; // yaw innovation consistency check ratio float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios - 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 _optflow_test_ratio{}; // Optical flow innovation consistency check ratio + Vector2f _gps_vel_test_ratio {}; // GPS velocity innovation consistency check ratios + Vector2f _gps_pos_test_ratio {}; // GPS position innovation consistency check ratios + 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{}; // Auxiliray horizontal velocity innovation consistency check ratio + 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 float _beta_test_ratio{}; // sideslip innovation consistency check ratio diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 752be5c8e8..01f6ae669d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -45,98 +45,97 @@ #include #include -/** - * 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 Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) { - // 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 + innov_var(0) = P[4][4] + obs_var(0); + innov_var(1) = P[5][5] + obs_var(1); + test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), + sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); - // 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])); + bool innov_check_pass = (test_ratio(0) <= 1.0f); + if (innov_check_pass) + { + _time_last_hor_vel_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_hor_vel = false; - innov_var[2] = P[6][6] + obs_var[2]; - test_ratio[VVEL] = sq(innov[2]) / (sq(innov_gate[VVEL]) * innov_var[2]); + fuseVelPosHeight(innov(0),innov_var(0),0); + fuseVelPosHeight(innov(1),innov_var(1),1); - 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); - - }else{ - fuse_mask[HVEL] = fuse_mask[VVEL] = false; - _innov_check_fail_status.flags.reject_vel_NED = true; - } + return true; + }else{ + _innov_check_fail_status.flags.reject_hor_vel = true; + return false; } +} - // 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])); +bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) +{ + innov_var(2) = P[6][6] + obs_var(2); + test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); - bool innov_check_pass = (test_ratio[HPOS] <= 1.0f) || !_control_status.flags.tilt_align; + bool innov_check_pass = (test_ratio(1) <= 1.0f); + if (innov_check_pass) { + _time_last_ver_vel_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_ver_vel = false; + + fuseVelPosHeight(innov(2),innov_var(2),2); + + return true; + }else{ + _innov_check_fail_status.flags.reject_ver_vel = true; + return false; + } +} + +bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) +{ + innov_var(0) = P[7][7] + obs_var(0); + innov_var(1) = P[8][8] + obs_var(1); + test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)), + sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); + + bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align; if (innov_check_pass) { if (!_fuse_hpos_as_odom) { - _time_last_pos_fuse = _time_last_imu; + _time_last_hor_pos_fuse = _time_last_imu; } else { _time_last_delpos_fuse = _time_last_imu; } - _innov_check_fail_status.flags.reject_pos_NE = false; + _innov_check_fail_status.flags.reject_hor_pos = false; - // fuse the horizontal position measurements - fuseVelPosHeight(innov[3],innov_var[3],3); - fuseVelPosHeight(innov[4],innov_var[4],4); + fuseVelPosHeight(innov(0),innov_var(0),3); + fuseVelPosHeight(innov(1),innov_var(1),4); + return true; }else{ - fuse_mask[HPOS] = false; - _innov_check_fail_status.flags.reject_pos_NE = true; + _innov_check_fail_status.flags.reject_hor_pos = true; + return false; } +} + +bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, + const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio) +{ + innov_var(2) = P[9][9] + obs_var(2); + test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); + + bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align; + if (innov_check_pass) { + _time_last_hgt_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_ver_pos = false; + + fuseVelPosHeight(innov(2),innov_var(2),5); + + return true; + }else{ + _innov_check_fail_status.flags.reject_ver_pos = true; + return false; } - - // 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 @@ -170,10 +169,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o healthy = false; - setVelPosFaultStatus(obs_index,true); + setVelPosFaultStatus(obs_index, true); } else { - setVelPosFaultStatus(obs_index,false); + setVelPosFaultStatus(obs_index, false); } }