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

Group velocity and position as a 3d vector

Browse Source
This commit is contained in:
kamilritz
2019-12-05 20:09:34 +11:00
committed by Paul Riseborough
parent beedf1ce4f
commit dae8c2f8dc
7 changed files with 326 additions and 368 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+13 -18
View File
@@ -210,12 +210,6 @@ struct auxVelSample {
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec) #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) #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 // 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_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) #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 // define structure used to communicate innovation test failures
union innovation_fault_status_u { union innovation_fault_status_u {
struct { struct {
bool reject_vel_NED: 1; ///< 0 - true if velocity observations have been rejected bool reject_hor_vel: 1; ///< 0 - true if horizontal velocity observations have been rejected
bool reject_pos_NE: 1; ///< 1 - true if horizontal position observations have been rejected bool reject_ver_vel: 1; ///< 1 - true if vertical velocity observations have been rejected
bool reject_pos_D: 1; ///< 2 - true if true if vertical position observations have been rejected bool reject_hor_pos: 1; ///< 2 - true if horizontal position observations have been rejected
bool reject_mag_x: 1; ///< 3 - true if the X magnetometer observation has been rejected bool reject_ver_pos: 1; ///< 3 - true if true if vertical position observations have been rejected
bool reject_mag_y: 1; ///< 4 - true if the Y magnetometer observation has been rejected bool reject_mag_x: 1; ///< 4 - true if the X magnetometer observation has been rejected
bool reject_mag_z: 1; ///< 5 - true if the Z magnetometer observation has been rejected bool reject_mag_y: 1; ///< 5 - true if the Y magnetometer observation has been rejected
bool reject_yaw: 1; ///< 6 - true if the yaw observation has been rejected bool reject_mag_z: 1; ///< 6 - true if the Z magnetometer observation has been rejected
bool reject_airspeed: 1; ///< 7 - true if the airspeed observation has been rejected bool reject_yaw: 1; ///< 7 - true if the yaw observation has been rejected
bool reject_sideslip: 1; ///< 8 - true if the synthetic sideslip observation has been rejected bool reject_airspeed: 1; ///< 8 - true if the airspeed observation has been rejected
bool reject_hagl: 1; ///< 9 - true if the height above ground observation has been rejected bool reject_sideslip: 1; ///< 9 - true if the synthetic sideslip observation has been rejected
bool reject_optflow_X: 1; ///< 10 - true if the X optical flow observation has been rejected bool reject_hagl: 1; ///< 10 - true if the height above ground observation has been rejected
bool reject_optflow_Y: 1; ///< 11 - true if the Y optical flow 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; } flags;
uint16_t value; uint16_t value;
EKF/control.cpp
+113 -130
View File
@@ -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 // determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) { 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 // correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; 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; ev_delta_pos = _ev_rot_mat * ev_delta_pos;
// use the change in position since the last measurement // 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_pos_innov(0) = _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(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 // 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 // record observation and estimate for use next time
@@ -319,15 +319,15 @@ void Ekf::controlExternalVisionFusion()
if (_params.fusion_mode & MASK_ROTATE_EV) { if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas; ev_pos_meas = _ev_rot_mat * ev_pos_meas;
} }
_ev_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0); _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
_ev_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1); _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
// observation 1-STD error // 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 // 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 // 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(); resetVelocity();
} }
@@ -336,12 +336,16 @@ void Ekf::controlExternalVisionFusion()
} }
// innovation gate size // 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 // determine if we should use the velocity observations
if (_control_status.flags.ev_vel) { 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}; Vector3f vel_aligned{_ev_sample_delayed.vel};
@@ -357,34 +361,31 @@ void Ekf::controlExternalVisionFusion()
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
vel_aligned -= vel_offset_earth; vel_aligned -= vel_offset_earth;
_ev_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0); _ev_vel_innov(0) = _state.vel(0) - vel_aligned(0);
_ev_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1); _ev_vel_innov(1) = _state.vel(1) - vel_aligned(1);
_ev_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2); _ev_vel_innov(2) = _state.vel(2) - vel_aligned(2);
// check if we have been deadreckoning too long // 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 // 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(); resetVelocity();
} }
} }
// observation 1-STD error // 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 // 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 fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
if (ev_fuse_mask[HVEL] || ev_fuse_mask[VVEL] || ev_fuse_mask[HPOS]) { fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
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 // determine if we should use the yaw observation
if (_control_status.flags.ev_yaw) { if (_control_status.flags.ev_yaw) {
fuseHeading(); fuseHeading();
} }
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel) } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
@@ -652,13 +653,13 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time // We are relying on aiding to constrain drift so after a specified time
// with no aiding we need to do something // 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_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); && ((_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 // 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) { if (do_reset) {
// use GPS velocity data to check and correct yaw angle if a FW vehicle // 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"); ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS");
// Reset the timeout counters // Reset the timeout counters
_time_last_pos_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu;
_time_last_vel_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 // Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) { 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 Vector2f gps_vel_innov_gates{}; // [horizontal vertical]
gps_fuse_mask[HVEL] = gps_fuse_mask[VVEL] = true; Vector2f gps_pos_innov_gates{}; // [horizontal vertical]
gps_fuse_mask[HPOS] = true; Vector3f gps_vel_obs_var{};
Vector3f gps_pos_obs_var{};
// correct velocity for offset relative to IMU // correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt); 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 (_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 // if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate // 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 { } else {
// if we are not using another source of aiding, then we are reliant on the GPS // 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. // observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); 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 // calculate innovations
_gps_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); _gps_vel_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_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_innov(2) = _state.vel(2) - _gps_sample_delayed.vel(2);
_gps_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); _gps_pos_innov(0) = _state.pos(0) - _gps_sample_delayed.pos(0);
_gps_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); _gps_pos_innov(1) = _state.pos(1) - _gps_sample_delayed.pos(1);
// set innovation gate size // set innovation gate size
gps_innov_gate[HPOS] = fmaxf(_params.gps_pos_innov_gate, 1.0f); gps_pos_innov_gates(0) = 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(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f);
// fuse GPS measurement // fuse GPS measurement
fuseVelPosHeightSeq(_gps_vel_pos_innov,gps_innov_gate, fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
gps_obs_var,gps_fuse_mask, fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
_gps_vel_pos_innov_var,_gps_vel_pos_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)) { } 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 // 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); 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 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 (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
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling (_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 - _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 ((_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() 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(); checkRangeAidSuitability();
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable(); _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) { if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight(); setControlRangeHeight();
height_fuse_mask[VPOS] = true; _fuse_height = true;
// we have just switched to using range finder, calculate height sensor offset such that current // we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate // 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) { } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight(); 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 // 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 // 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) { } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
// switch to gps if there was a reset to gps // 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 // we have just switched to using gps height, calculate height sensor offset such that current
// measurement matches our current height estimate // measurement matches our current height estimate
@@ -1044,7 +1037,7 @@ void Ekf::controlHeightFusion()
// set the height data source to range if requested // set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) { if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
setControlRangeHeight(); 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 // we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate // 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) { } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight(); 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 // 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 // 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) { if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight(); setControlRangeHeight();
height_fuse_mask[VPOS] = true; _fuse_height = true;
// we have just switched to using range finder, calculate height sensor offset such that current // we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate // 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) { } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
setControlGPSHeight(); setControlGPSHeight();
height_fuse_mask[VPOS] = true; _fuse_height = true;
// we have just switched to using gps height, calculate height sensor offset such that current // we have just switched to using gps height, calculate height sensor offset such that current
// measurement matches our current height estimate // 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) { } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro // 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 // 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 // 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 // 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))) { 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(); setControlEVHeight();
resetHeight(); resetHeight();
} }
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro // 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 // 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 // since we track a separate _baro_hgt_offset
@@ -1138,7 +1131,7 @@ void Ekf::controlHeightFusion()
// TODO: Add EV normal case here // TODO: Add EV normal case here
// determine if we should use the vertical position observation // determine if we should use the vertical position observation
if (_control_status.flags.ev_hgt) { 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) { if (_control_status.flags.baro_hgt) {
// vertical position innovation - baro measurement has opposite sign to earth z axis // 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 // 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 // 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) // Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro 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; float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) { if (_control_status.flags.gnd_effect) {
if (height_innov[5] < -deadzone_start) { if (height_innov(2) < -deadzone_start) {
if (height_innov[5] <= -deadzone_end) { if (height_innov(2) <= -deadzone_end) {
height_innov[5] += deadzone_end; height_innov(2) += deadzone_end;
} else { } else {
height_innov[5] = -deadzone_start; height_innov(2) = -deadzone_start;
} }
} }
} }
} else if (_control_status.flags.gps_hgt) { } else if (_control_status.flags.gps_hgt) {
// vertical position innovation - gps measurement has opposite sign to earth z axis // 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 // observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); 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 // 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)) { } else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
// use range finder with tilt correction // 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; _params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined // 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 // 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) { } else if (_control_status.flags.ev_hgt) {
// calculate the innovation assuming the external vision observation is in local NED frame // 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 // 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 // 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 // fuse height inforamtion
fuseVelPosHeightSeq(height_innov,height_innov_gate, fuseVerticalPosition(height_innov,height_innov_gate,
height_obs_var,height_fuse_mask, height_obs_var, height_innov_var,height_test_ratio);
height_innov_var,height_test_ratio);
// This is a temporary hack until we do proper height sensor fusion // This is a temporary hack until we do proper height sensor fusion
_gps_vel_pos_innov[5] = height_innov[5]; _gps_pos_innov(2) = height_innov(2);
_gps_vel_pos_innov_var[5] = height_innov_var[5]; _gps_pos_innov_var(2) = height_innov_var(2);
_gps_vel_pos_test_ratio[VPOS] = height_test_ratio[VPOS]; _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 // 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 // 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 && if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow && !_control_status.flags.opt_flow &&
@@ -1386,6 +1382,11 @@ void Ekf::controlFakePosFusion()
// Fuse synthetic position observations every 200msec // Fuse synthetic position observations every 200msec
if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) { 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 // Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) { if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) {
resetPosition(); resetPosition();
@@ -1397,27 +1398,23 @@ void Ekf::controlFakePosFusion()
} }
} }
// Fuse horizontal position
fake_gps_fuse_mask[HPOS] = true;
_time_last_fake_pos = _time_last_imu; _time_last_fake_pos = _time_last_imu;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) { 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 { } 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_pos_innov(0) = _state.pos(0) - _last_known_posNE(0);
_gps_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); _gps_pos_innov(1) = _state.pos(1) - _last_known_posNE(1);
// glitch protection is not required so set gate to a large value // 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, fuseHorizontalPosition(_gps_pos_innov, fake_gps_pos_innov_gate, fake_gps_pos_obs_var,
fake_gps_obs_var,fake_gps_fuse_mask, _gps_pos_innov_var, _gps_pos_test_ratio);
_gps_vel_pos_innov_var,_gps_vel_pos_test_ratio);
} }
} else { } else {
@@ -1433,31 +1430,17 @@ void Ekf::controlAuxVelFusion()
if (data_ready && primary_aiding) { if (data_ready && primary_aiding) {
bool auxvel_fuse_mask[4]{}; Vector2f aux_vel_innov_gate{};
float auxvel_innov[6]{}; Vector3f aux_vel_obs_var{};
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; _aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
auxvel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0); _aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
auxvel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1); aux_vel_innov_gate(0) = _params.auxvel_gate;
auxvel_innov_gate[HVEL] = _params.auxvel_gate; aux_vel_obs_var(0) = _auxvel_sample_delayed.velVarNE(0);
auxvel_obs_var[0] = _auxvel_sample_delayed.velVarNE(0); aux_vel_obs_var(1) = _auxvel_sample_delayed.velVarNE(1);
auxvel_obs_var[1] = _auxvel_sample_delayed.velVarNE(1);
fuseVelPosHeightSeq(auxvel_innov,auxvel_innov_gate, fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var,
auxvel_obs_var,auxvel_fuse_mask, _aux_vel_innov_var, _aux_vel_test_ratio);
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];
} }
} }
EKF/ekf.cpp
+2 -2
View File
@@ -302,9 +302,9 @@ bool Ekf::initialiseFilter()
// reset the essential fusion timeout counters // reset the essential fusion timeout counters
_time_last_hgt_fuse = _time_last_imu; _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_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_hagl_fuse = _time_last_imu;
_time_last_of_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu;
EKF/ekf.h
+28 -27
View File
@@ -330,10 +330,12 @@ private:
uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (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 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_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_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_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_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 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) Vector3f _gps_vel_innov {}; ///< GPS velocity innovations (m/sec)
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_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) Vector3f _gps_pos_innov {}; ///< GPS position innovations (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_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) Vector3f _ev_vel_innov {}; ///< external vision velocity innovations (m/sec)
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_var {}; ///< external vision velocity innovation variances ((m/sec)**2)
float _aux_vel_innov[2] {}; ///< horizontal auxiliary velocity innovations: (m/sec) Vector3f _ev_pos_innov {}; ///< external vision position innovations (m)
float _aux_vel_innov_var[2] {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) 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{0.0f}; ///< heading measurement innovation (rad)
float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) 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 // fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(); 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 // fuse single velocity and position measurement
void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); 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 // fuse optical flow line of sight rate measurements
void fuseOptFlow(); 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 // calculate optical flow body angular rate compensation
// returns false if bias corrected body rate data is unavailable // returns false if bias corrected body rate data is unavailable
bool calcOptFlowBodyRateComp(); bool calcOptFlowBodyRateComp();
@@ -789,37 +801,26 @@ private:
// sensor measurement // sensor measurement
float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted); float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted);
// stop GPS fusion
void stopGpsFusion(); void stopGpsFusion();
// stop GPS position fusion
void stopGpsPosFusion(); void stopGpsPosFusion();
// stop GPS velocity fusion
void stopGpsVelFusion(); void stopGpsVelFusion();
// stop GPS yaw fusion
void stopGpsYawFusion(); void stopGpsYawFusion();
// stop external vision fusion
void stopEvFusion(); void stopEvFusion();
// stop external vision position fusion
void stopEvPosFusion(); void stopEvPosFusion();
// stop external vision velocity fusion
void stopEvVelFusion(); void stopEvVelFusion();
// stop external vision yaw fusion
void stopEvYawFusion(); void stopEvYawFusion();
// stop auxiliary horizontal velocity fusion
void stopAuxVelFusion(); void stopAuxVelFusion();
// stop optical flow fusion
void stopFlowFusion(); void stopFlowFusion();
// Helper function to set velPos fault status
void setVelPosFaultStatus(const int index, const bool status); void setVelPosFaultStatus(const int index, const bool status);
}; };
EKF/ekf_helper.cpp
+89 -73
View File
@@ -418,7 +418,7 @@ bool Ekf::realignYawGPS()
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) { if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
// check for excessive horizontal GPS velocity innovations // 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 // calculate GPS course over ground angle
float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0)); 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) void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
{ {
memcpy(hvel, _gps_vel_pos_innov+0, sizeof(float) * 2); hvel[0] = _gps_vel_innov(0);
memcpy(&vvel, _gps_vel_pos_innov+2, sizeof(float) * 1); hvel[1] = _gps_vel_innov(1);
memcpy(hpos, _gps_vel_pos_innov+3, sizeof(float) * 2); vvel = _gps_vel_innov(2);
memcpy(&vpos, _gps_vel_pos_innov+5, sizeof(float) * 1); 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) void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
{ {
memcpy(hvel, _gps_vel_pos_innov_var+0, sizeof(float) * 2); hvel[0] = _gps_vel_innov_var(0);
memcpy(&vvel, _gps_vel_pos_innov_var+2, sizeof(float) * 1); hvel[1] = _gps_vel_innov_var(1);
memcpy(hpos, _gps_vel_pos_innov_var+3, sizeof(float) * 2); vvel = _gps_vel_innov_var(2);
memcpy(&vpos, _gps_vel_pos_innov_var+5, sizeof(float) * 1); 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) void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
{ {
memcpy(&hvel, _gps_vel_pos_test_ratio+HVEL, sizeof(float)); hvel = _gps_vel_test_ratio(0);
memcpy(&vvel, _gps_vel_pos_test_ratio+VVEL, sizeof(float)); vvel = _gps_vel_test_ratio(1);
memcpy(&hpos, _gps_vel_pos_test_ratio+HPOS, sizeof(float)); hpos = _gps_pos_test_ratio(0);
memcpy(&vpos, _gps_vel_pos_test_ratio+VPOS, sizeof(float)); vpos = _gps_pos_test_ratio(1);
} }
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
{ {
memcpy(hvel, _ev_vel_pos_innov+0, sizeof(float) * 2); hvel[0] = _ev_vel_innov(0);
memcpy(&vvel, _ev_vel_pos_innov+2, sizeof(float) * 1); hvel[1] = _ev_vel_innov(1);
memcpy(hpos, _ev_vel_pos_innov+3, sizeof(float) * 2); vvel = _ev_vel_innov(2);
memcpy(&vpos, _ev_vel_pos_innov+5, sizeof(float) * 1); 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) void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
{ {
memcpy(hvel, _ev_vel_pos_innov_var+0, sizeof(float) * 2); hvel[0] = _ev_vel_innov_var(0);
memcpy(&vvel, _ev_vel_pos_innov_var+2, sizeof(float) * 1); hvel[1] = _ev_vel_innov_var(1);
memcpy(hpos, _ev_vel_pos_innov_var+3, sizeof(float) * 2); vvel = _ev_vel_innov_var(2);
memcpy(&vpos, _ev_vel_pos_innov_var+5, sizeof(float) * 1); 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) void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
{ {
memcpy(&hvel, _ev_vel_pos_test_ratio+HVEL, sizeof(float)); hvel = _ev_vel_test_ratio(0);
memcpy(&vvel, _ev_vel_pos_test_ratio+VVEL, sizeof(float)); vvel = _ev_vel_test_ratio(1);
memcpy(&hpos, _ev_vel_pos_test_ratio+HPOS, sizeof(float)); hpos = _ev_pos_test_ratio(0);
memcpy(&vpos, _ev_vel_pos_test_ratio+VPOS, sizeof(float)); vpos = _ev_pos_test_ratio(1);
} }
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) 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]) 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) 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]) 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) 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) 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) 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) 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]) 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) 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) 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) 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) 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) 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) 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) 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) 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) 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 // 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 // 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 // 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 // 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)) { if (_is_dead_reckoning && (_control_status.flags.gps)) {
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4]))); 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; *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 // 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 // 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 // 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)) { if (_is_dead_reckoning && _control_status.flags.gps) {
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4]))); hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
} }
*ekf_eph = hpos_err; *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])); 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) { if (_control_status.flags.gps) {
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(_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) { if (_control_status.flags.ev_vel) {
// What is the right thing to do here vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1))));
// vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
} }
hvel_err = math::max(hvel_err, vel_err_conservative); 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 // 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]))); 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 // 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 // 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 // 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 // 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 // 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 // return the airspeed fusion innovation test ratio
tas = sqrtf(_tas_test_ratio); tas = sqrtf(_tas_test_ratio);
// return the terrain height innovation 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.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_rel = soln_status.flags.pos_horiz_rel;
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; 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_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
bool gps_pos_innov_bad = (_gps_vel_pos_test_ratio[HPOS] > 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); 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.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
soln_status.flags.accel_error = _bad_vert_accel_detected; soln_status.flags.accel_error = _bad_vert_accel_detected;
@@ -1438,8 +1452,8 @@ bool Ekf::global_position_is_valid()
void Ekf::update_deadreckoning_status() void Ekf::update_deadreckoning_status()
{ {
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) 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_hor_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_vel_fuse) <= _params.no_aid_timeout_max)
|| ((_time_last_imu - _time_last_delpos_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 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); 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 = false;
_control_status.flags.gps_hgt = false; _control_status.flags.gps_hgt = false;
memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3); _gps_pos_innov.setZero();
memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3); _gps_pos_innov_var.setZero();
memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); _gps_pos_test_ratio.setZero();
} }
void Ekf::stopGpsVelFusion() void Ekf::stopGpsVelFusion()
{ {
memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3); _gps_vel_innov.setZero();
memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3); _gps_vel_innov_var.setZero();
memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2); _gps_vel_test_ratio.setZero();
} }
void Ekf::stopGpsYawFusion() void Ekf::stopGpsYawFusion()
@@ -1919,27 +1933,29 @@ void Ekf::stopEvFusion()
void Ekf::stopEvPosFusion() void Ekf::stopEvPosFusion()
{ {
_control_status.flags.ev_pos = false; _control_status.flags.ev_pos = false;
memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3); _ev_pos_innov.setZero();
memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3); _ev_pos_innov_var.setZero();
memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2); _ev_pos_test_ratio.setZero();
} }
void Ekf::stopEvVelFusion() void Ekf::stopEvVelFusion()
{ {
_control_status.flags.ev_vel = false; _control_status.flags.ev_vel = false;
memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3); _ev_vel_innov.setZero();
memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3); _ev_vel_innov_var.setZero();
memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);} _ev_vel_test_ratio.setZero();
}
void Ekf::stopEvYawFusion() void Ekf::stopEvYawFusion()
{ {
_control_status.flags.ev_yaw = false; _control_status.flags.ev_yaw = false;
} }
void Ekf::stopAuxVelFusion() 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() void Ekf::stopFlowFusion()
EKF/estimator_interface.h
+6 -42
View File
@@ -67,82 +67,44 @@ public:
virtual void resetStatesAndCovariances() = 0; virtual void resetStatesAndCovariances() = 0;
virtual bool update() = 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; 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; 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; 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; 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; 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; 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; 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; 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; virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0;
// gets the innovation of the flow measurement
virtual void getFlowInnov(float flow_innov[2]) = 0; 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; 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; virtual void getFlowInnovRatio(float &flow_innov_ratio) = 0;
// gets the innovations of the heading measurement
virtual void getHeadingInnov(float &heading_innov) = 0; virtual void getHeadingInnov(float &heading_innov) = 0;
// gets the innovation variance of the heading measurement
virtual void getHeadingInnovVar(float &heading_innov_var) = 0; 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; 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; 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; 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; 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; 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; 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; virtual void getDragInnovRatio(float drag_innov_ratio[2]) = 0;
// gets the innovation of airspeed measurement
virtual void getAirspeedInnov(float &airspeed_innov) = 0; virtual void getAirspeedInnov(float &airspeed_innov) = 0;
// gets the innovation variance of the airspeed measurement
virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) = 0; 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; virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) = 0;
// gets the innovation of the synthetic sideslip measurement
virtual void getBetaInnov(float &beta_innov) = 0; 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; 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; virtual void getBetaInnovRatio(float &beta_innov_ratio) = 0;
// gets the innovation of the HAGL measurement
virtual void getHaglInnov(float &hagl_innov) = 0; virtual void getHaglInnov(float &hagl_innov) = 0;
// gets the innovation variance of the HAGL measurement
virtual void getHaglInnovVar(float &hagl_innov_var) = 0; 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; virtual void getHaglInnovRatio(float &hagl_innov_ratio) = 0;
@@ -512,10 +474,12 @@ protected:
// innovation consistency check monitoring ratios // innovation consistency check monitoring ratios
float _yaw_test_ratio{}; // yaw innovation consistency check ratio float _yaw_test_ratio{}; // yaw innovation consistency check ratio
float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios 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 Vector2f _gps_vel_test_ratio {}; // GPS velocity innovation consistency check ratios
float _ev_vel_pos_test_ratio[4] {}; // EV velocity and position innovation consistency check ratios Vector2f _gps_pos_test_ratio {}; // GPS position innovation consistency check ratios
float _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio Vector2f _ev_vel_test_ratio {}; // EV velocity innovation consistency check ratios
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio 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 _tas_test_ratio{}; // tas innovation consistency check ratio
float _hagl_test_ratio{}; // height above terrain measurement 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 float _beta_test_ratio{}; // sideslip innovation consistency check ratio
EKF/vel_pos_fusion.cpp
+75 -76
View File
@@ -45,98 +45,97 @@
#include <ecl.h> #include <ecl.h>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
/**
* Update the EKF state with velocity and position measurements sequentially. [(vx vy) (vz) (x y) (z)] bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
* const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
* @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])
{ {
// check position, velocity and height innovations sequentially and if checks are passed fuse it innov_var(0) = P[4][4] + obs_var(0);
// treat 2D horizintal velocity, vertical velocity, 2D horizontal position and vertical height as separate sensors innov_var(1) = P[5][5] + obs_var(1);
// At the moment we still fuse velocity as 3D measurement, but this should be split in the future 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 bool innov_check_pass = (test_ratio(0) <= 1.0f);
if(fuse_mask[HVEL] && fuse_mask[VVEL]){ if (innov_check_pass)
innov_var[0] = P[4][4] + obs_var[0]; {
innov_var[1] = P[5][5] + obs_var[1]; _time_last_hor_vel_fuse = _time_last_imu;
test_ratio[HVEL] = fmaxf( sq(innov[0]) / (sq(innov_gate[HVEL]) * innov_var[0]), _innov_check_fail_status.flags.reject_hor_vel = false;
sq(innov[1]) / (sq(innov_gate[HVEL]) * innov_var[1]));
innov_var[2] = P[6][6] + obs_var[2]; fuseVelPosHeight(innov(0),innov_var(0),0);
test_ratio[VVEL] = sq(innov[2]) / (sq(innov_gate[VVEL]) * innov_var[2]); fuseVelPosHeight(innov(1),innov_var(1),1);
bool innov_check_pass = (test_ratio[HVEL] <= 1.0f) && (test_ratio[VVEL] <= 1.0f); return true;
if (innov_check_pass) { }else{
_time_last_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hor_vel = true;
_innov_check_fail_status.flags.reject_vel_NED = false; return 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;
}
} }
}
// horizontal position bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
if(fuse_mask[HPOS]){ const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
innov_var[3] = P[7][7] + obs_var[3]; {
innov_var[4] = P[8][8] + obs_var[4]; innov_var(2) = P[6][6] + obs_var(2);
test_ratio[HPOS] = fmaxf( sq(innov[3]) / (sq(innov_gate[HPOS]) * innov_var[3]), test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
sq(innov[4]) / (sq(innov_gate[HPOS]) * innov_var[4]));
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 (innov_check_pass) {
if (!_fuse_hpos_as_odom) { if (!_fuse_hpos_as_odom) {
_time_last_pos_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu;
} else { } else {
_time_last_delpos_fuse = _time_last_imu; _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(0),innov_var(0),3);
fuseVelPosHeight(innov[3],innov_var[3],3); fuseVelPosHeight(innov(1),innov_var(1),4);
fuseVelPosHeight(innov[4],innov_var[4],4);
return true;
}else{ }else{
fuse_mask[HPOS] = false; _innov_check_fail_status.flags.reject_hor_pos = true;
_innov_check_fail_status.flags.reject_pos_NE = 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 // 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; healthy = false;
setVelPosFaultStatus(obs_index,true); setVelPosFaultStatus(obs_index, true);
} else { } else {
setVelPosFaultStatus(obs_index,false); setVelPosFaultStatus(obs_index, false);
} }
} }