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