mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:04:07 +08:00
Group velocity and position as a 3d vector
This commit is contained in:
kamilritz
Paul Riseborough
parent
beedf1ce4f
commit
dae8c2f8dc
31
EKF/common.h
31
EKF/common.h
@ -210,12 +210,6 @@ struct auxVelSample {
|
||||
#define RNG_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between range finder measurements (uSec)
|
||||
#define EV_MAX_INTERVAL (uint64_t)2e5 ///< Maximum allowable time interval between external vision system measurements (uSec)
|
||||
|
||||
// VelPos measurement bundles indices
|
||||
#define HVEL 0 ///< x and y velocity bundle index
|
||||
#define VVEL 1 ///< z velocity bundle index
|
||||
#define HPOS 2 ///< x and y position bundle index
|
||||
#define VPOS 3 ///< z position bundle index
|
||||
|
||||
// bad accelerometer detection and mitigation
|
||||
#define BADACC_PROBATION (uint64_t)10e6 ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
|
||||
#define BADACC_BIAS_PNOISE 4.9f ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
|
||||
@ -407,18 +401,19 @@ union fault_status_u {
|
||||
// define structure used to communicate innovation test failures
|
||||
union innovation_fault_status_u {
|
||||
struct {
|
||||
bool reject_vel_NED: 1; ///< 0 - true if velocity observations have been rejected
|
||||
bool reject_pos_NE: 1; ///< 1 - true if horizontal position observations have been rejected
|
||||
bool reject_pos_D: 1; ///< 2 - true if true if vertical position observations have been rejected
|
||||
bool reject_mag_x: 1; ///< 3 - true if the X magnetometer observation has been rejected
|
||||
bool reject_mag_y: 1; ///< 4 - true if the Y magnetometer observation has been rejected
|
||||
bool reject_mag_z: 1; ///< 5 - true if the Z magnetometer observation has been rejected
|
||||
bool reject_yaw: 1; ///< 6 - true if the yaw observation has been rejected
|
||||
bool reject_airspeed: 1; ///< 7 - true if the airspeed observation has been rejected
|
||||
bool reject_sideslip: 1; ///< 8 - true if the synthetic sideslip observation has been rejected
|
||||
bool reject_hagl: 1; ///< 9 - true if the height above ground observation has been rejected
|
||||
bool reject_optflow_X: 1; ///< 10 - true if the X optical flow observation has been rejected
|
||||
bool reject_optflow_Y: 1; ///< 11 - true if the Y optical flow observation has been rejected
|
||||
bool reject_hor_vel: 1; ///< 0 - true if horizontal velocity observations have been rejected
|
||||
bool reject_ver_vel: 1; ///< 1 - true if vertical velocity observations have been rejected
|
||||
bool reject_hor_pos: 1; ///< 2 - true if horizontal position observations have been rejected
|
||||
bool reject_ver_pos: 1; ///< 3 - true if true if vertical position observations have been rejected
|
||||
bool reject_mag_x: 1; ///< 4 - true if the X magnetometer observation has been rejected
|
||||
bool reject_mag_y: 1; ///< 5 - true if the Y magnetometer observation has been rejected
|
||||
bool reject_mag_z: 1; ///< 6 - true if the Z magnetometer observation has been rejected
|
||||
bool reject_yaw: 1; ///< 7 - true if the yaw observation has been rejected
|
||||
bool reject_airspeed: 1; ///< 8 - true if the airspeed observation has been rejected
|
||||
bool reject_sideslip: 1; ///< 9 - true if the synthetic sideslip observation has been rejected
|
||||
bool reject_hagl: 1; ///< 10 - true if the height above ground observation has been rejected
|
||||
bool reject_optflow_X: 1; ///< 11 - true if the X optical flow observation has been rejected
|
||||
bool reject_optflow_Y: 1; ///< 12 - true if the Y optical flow observation has been rejected
|
||||
} flags;
|
||||
uint16_t value;
|
||||
|
||||
|
||||
243
EKF/control.cpp
243
EKF/control.cpp
@ -265,13 +265,13 @@ void Ekf::controlExternalVisionFusion()
|
||||
}
|
||||
}
|
||||
|
||||
bool ev_fuse_mask[4]{};
|
||||
float ev_obs_var[6]{};
|
||||
float ev_innov_gate[4]{};
|
||||
|
||||
|
||||
// determine if we should use the horizontal position observations
|
||||
if (_control_status.flags.ev_pos) {
|
||||
ev_fuse_mask[HPOS] = true;
|
||||
|
||||
Vector3f ev_pos_obs_var{};
|
||||
Vector2f ev_pos_innov_gates{};
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
|
||||
@ -301,11 +301,11 @@ void Ekf::controlExternalVisionFusion()
|
||||
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
|
||||
|
||||
// use the change in position since the last measurement
|
||||
_ev_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
||||
_ev_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
|
||||
_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
|
||||
_ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
|
||||
|
||||
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
|
||||
ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.5f);
|
||||
ev_pos_obs_var(0) = ev_pos_obs_var(1) = fmaxf(_ev_sample_delayed.posErr, 0.5f);
|
||||
}
|
||||
|
||||
// record observation and estimate for use next time
|
||||
@ -319,15 +319,15 @@ void Ekf::controlExternalVisionFusion()
|
||||
if (_params.fusion_mode & MASK_ROTATE_EV) {
|
||||
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
|
||||
}
|
||||
_ev_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
|
||||
_ev_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
|
||||
_ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0);
|
||||
_ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1);
|
||||
// observation 1-STD error
|
||||
ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
ev_pos_obs_var(0) = ev_pos_obs_var(1) = fmaxf(_ev_sample_delayed.posErr, 0.01f);
|
||||
|
||||
// check if we have been deadreckoning too long
|
||||
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
|
||||
if ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max) {
|
||||
// don't reset velocity if we have another source of aiding constraining it
|
||||
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_vel_fuse) > (uint64_t)1E6)) {
|
||||
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_vel_fuse) > (uint64_t)1E6)) {
|
||||
resetVelocity();
|
||||
}
|
||||
|
||||
@ -336,12 +336,16 @@ void Ekf::controlExternalVisionFusion()
|
||||
}
|
||||
|
||||
// innovation gate size
|
||||
ev_innov_gate[HPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
ev_pos_innov_gates(0) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
|
||||
fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gates, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio);
|
||||
}
|
||||
|
||||
// determine if we should use the velocity observations
|
||||
if (_control_status.flags.ev_vel) {
|
||||
ev_fuse_mask[HVEL] = ev_fuse_mask[VVEL] = true;
|
||||
|
||||
Vector3f ev_vel_obs_var{};
|
||||
Vector2f ev_vel_innov_gates{};
|
||||
|
||||
Vector3f vel_aligned{_ev_sample_delayed.vel};
|
||||
|
||||
@ -357,34 +361,31 @@ void Ekf::controlExternalVisionFusion()
|
||||
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
vel_aligned -= vel_offset_earth;
|
||||
|
||||
_ev_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
|
||||
_ev_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
|
||||
_ev_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
|
||||
_ev_vel_innov(0) = _state.vel(0) - vel_aligned(0);
|
||||
_ev_vel_innov(1) = _state.vel(1) - vel_aligned(1);
|
||||
_ev_vel_innov(2) = _state.vel(2) - vel_aligned(2);
|
||||
|
||||
// check if we have been deadreckoning too long
|
||||
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {
|
||||
if ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max) {
|
||||
// don't reset velocity if we have another source of aiding constraining it
|
||||
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_pos_fuse) > (uint64_t)1E6)) {
|
||||
if (((_time_last_imu - _time_last_of_fuse) > (uint64_t)1E6) && ((_time_last_imu - _time_last_hor_pos_fuse) > (uint64_t)1E6)) {
|
||||
resetVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
// observation 1-STD error
|
||||
ev_obs_var[0] = ev_obs_var[1] = ev_obs_var[2] = fmaxf(_ev_sample_delayed.velErr, 0.01f);
|
||||
ev_vel_obs_var(0) = ev_vel_obs_var(1) = ev_vel_obs_var(2) = fmaxf(_ev_sample_delayed.velErr, 0.01f);
|
||||
|
||||
// innovation gate size
|
||||
ev_innov_gate[HVEL] = ev_innov_gate[VVEL] = fmaxf(_params.ev_vel_innov_gate, 1.0f);
|
||||
}
|
||||
ev_vel_innov_gates(0) = ev_vel_innov_gates(1) = fmaxf(_params.ev_vel_innov_gate, 1.0f);
|
||||
|
||||
// Fuse available NED position data into the main filter
|
||||
if (ev_fuse_mask[HVEL] || ev_fuse_mask[VVEL] || ev_fuse_mask[HPOS]) {
|
||||
fuseVelPosHeightSeq(_ev_vel_pos_innov, ev_innov_gate, ev_obs_var , ev_fuse_mask, _ev_vel_pos_innov_var, _ev_vel_pos_test_ratio);
|
||||
fuseHorizontalVelocity(_ev_vel_innov, ev_vel_innov_gates,ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
|
||||
fuseVerticalVelocity(_ev_vel_innov, ev_vel_innov_gates, ev_vel_obs_var, _ev_vel_innov_var, _ev_vel_test_ratio);
|
||||
}
|
||||
|
||||
// determine if we should use the yaw observation
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
fuseHeading();
|
||||
|
||||
}
|
||||
|
||||
} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
||||
@ -652,13 +653,13 @@ void Ekf::controlGpsFusion()
|
||||
if (_control_status.flags.gps) {
|
||||
// We are relying on aiding to constrain drift so after a specified time
|
||||
// with no aiding we need to do something
|
||||
bool do_reset = ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max)
|
||||
bool do_reset = ((_time_last_imu - _time_last_hor_pos_fuse) > _params.reset_timeout_max)
|
||||
&& ((_time_last_imu - _time_last_delpos_fuse) > _params.reset_timeout_max)
|
||||
&& ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max)
|
||||
&& ((_time_last_imu - _time_last_hor_vel_fuse) > _params.reset_timeout_max)
|
||||
&& ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max);
|
||||
|
||||
// We haven't had an absolute position fix for a longer time so need to do something
|
||||
do_reset = do_reset || ((_time_last_imu - _time_last_pos_fuse) > (2 * _params.reset_timeout_max));
|
||||
do_reset = do_reset || ((_time_last_imu - _time_last_hor_pos_fuse) > (2 * _params.reset_timeout_max));
|
||||
|
||||
if (do_reset) {
|
||||
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
||||
@ -673,8 +674,8 @@ void Ekf::controlGpsFusion()
|
||||
ECL_WARN_TIMESTAMPED("EKF GPS fusion timeout - reset to GPS");
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
|
||||
}
|
||||
}
|
||||
@ -682,13 +683,11 @@ void Ekf::controlGpsFusion()
|
||||
// Only use GPS data for position and velocity aiding if enabled
|
||||
if (_control_status.flags.gps) {
|
||||
|
||||
bool gps_fuse_mask[4]{};
|
||||
float gps_obs_var[6]{};
|
||||
float gps_innov_gate[4]{};
|
||||
|
||||
// Enable full velocity and horizontal position fusion
|
||||
gps_fuse_mask[HVEL] = gps_fuse_mask[VVEL] = true;
|
||||
gps_fuse_mask[HPOS] = true;
|
||||
Vector2f gps_vel_innov_gates{}; // [horizontal vertical]
|
||||
Vector2f gps_pos_innov_gates{}; // [horizontal vertical]
|
||||
Vector3f gps_vel_obs_var{};
|
||||
Vector3f gps_pos_obs_var{};
|
||||
|
||||
// correct velocity for offset relative to IMU
|
||||
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
|
||||
@ -709,32 +708,32 @@ void Ekf::controlGpsFusion()
|
||||
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
|
||||
// if we are using other sources of aiding, then relax the upper observation
|
||||
// noise limit which prevents bad GPS perturbing the position estimate
|
||||
gps_obs_var[3] = gps_obs_var[4] = fmaxf(_gps_sample_delayed.hacc, lower_limit);
|
||||
gps_pos_obs_var(0) = gps_pos_obs_var(1) = fmaxf(_gps_sample_delayed.hacc, lower_limit);
|
||||
|
||||
} else {
|
||||
// if we are not using another source of aiding, then we are reliant on the GPS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
gps_obs_var[3] = gps_obs_var[4] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
||||
gps_pos_obs_var(0) = gps_pos_obs_var(1) = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
|
||||
}
|
||||
|
||||
gps_obs_var[0] = gps_obs_var[1] = gps_obs_var[2] = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
|
||||
gps_vel_obs_var(0) = gps_vel_obs_var(1) = gps_vel_obs_var(2) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
|
||||
|
||||
// calculate innovations
|
||||
_gps_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
|
||||
_gps_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
|
||||
_gps_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
|
||||
_gps_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_gps_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
_gps_vel_innov(0) = _state.vel(0) - _gps_sample_delayed.vel(0);
|
||||
_gps_vel_innov(1) = _state.vel(1) - _gps_sample_delayed.vel(1);
|
||||
_gps_vel_innov(2) = _state.vel(2) - _gps_sample_delayed.vel(2);
|
||||
_gps_pos_innov(0) = _state.pos(0) - _gps_sample_delayed.pos(0);
|
||||
_gps_pos_innov(1) = _state.pos(1) - _gps_sample_delayed.pos(1);
|
||||
|
||||
// set innovation gate size
|
||||
gps_innov_gate[HPOS] = fmaxf(_params.gps_pos_innov_gate, 1.0f);
|
||||
gps_innov_gate[HVEL] = gps_innov_gate[VVEL] = fmaxf(_params.gps_vel_innov_gate, 1.0f);
|
||||
gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f);
|
||||
gps_pos_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f);
|
||||
|
||||
// fuse GPS measurement
|
||||
fuseVelPosHeightSeq(_gps_vel_pos_innov,gps_innov_gate,
|
||||
gps_obs_var,gps_fuse_mask,
|
||||
_gps_vel_pos_innov_var,_gps_vel_pos_test_ratio);
|
||||
fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates,gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
|
||||
fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, gps_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio);
|
||||
fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio);
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
|
||||
@ -761,8 +760,8 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
// Clipping causes the average accel reading to move towards zero which makes the INS think it is falling and produces positive vertical innovations
|
||||
float var_product_lim = sq(_params.vert_innov_test_lim) * sq(_params.vert_innov_test_lim);
|
||||
bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are independent
|
||||
(sq(_vel_pos_innov[5] * _vel_pos_innov[2]) > var_product_lim * (_vel_pos_innov_var[5] * _vel_pos_innov_var[2])) && // vertical position and velocity sensors are in agreement that we have a significant error
|
||||
(_vel_pos_innov[2] > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
|
||||
(sq(_gps_pos_innov(2) * fmaxf(fabsf(_gps_vel_innov(2)),fabsf(_ev_vel_innov(2)))) > var_product_lim * (_gps_pos_innov_var(2) * fmaxf(fabsf(_gps_vel_innov_var(2)),fabsf(_ev_vel_innov_var(2))))) && // vertical position and velocity sensors are in agreement that we have a significant error
|
||||
(_gps_vel_innov(2) > 0.0f || _ev_vel_innov(2) > 0.0f) && // positive innovation indicates that the inertial nav thinks it is falling
|
||||
((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh
|
||||
((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); // vertical velocity data is fresh
|
||||
|
||||
@ -984,12 +983,6 @@ void Ekf::controlHeightSensorTimeouts()
|
||||
|
||||
void Ekf::controlHeightFusion()
|
||||
{
|
||||
bool height_fuse_mask[4]{};
|
||||
float height_innov_gate[4]{};
|
||||
float height_test_ratio[4]{};
|
||||
float height_obs_var[6]{};
|
||||
float height_innov_var[6]{};
|
||||
float height_innov[6]{};
|
||||
|
||||
checkRangeAidSuitability();
|
||||
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
|
||||
@ -998,7 +991,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
|
||||
setControlRangeHeight();
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
@ -1013,7 +1006,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
|
||||
setControlBaroHeight();
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using baro height, we don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
@ -1031,7 +1024,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
|
||||
// switch to gps if there was a reset to gps
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
@ -1044,7 +1037,7 @@ void Ekf::controlHeightFusion()
|
||||
// set the height data source to range if requested
|
||||
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
|
||||
setControlRangeHeight();
|
||||
height_fuse_mask[VPOS] = _range_data_ready;
|
||||
_fuse_height = _range_data_ready;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
@ -1066,7 +1059,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
|
||||
setControlBaroHeight();
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using baro height, we don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
@ -1080,7 +1073,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
|
||||
setControlRangeHeight();
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using range finder, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
@ -1095,7 +1088,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
|
||||
setControlGPSHeight();
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using gps height, calculate height sensor offset such that current
|
||||
// measurement matches our current height estimate
|
||||
@ -1105,7 +1098,7 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
|
||||
// switch to baro if there was a reset to baro
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using baro height, we don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
@ -1120,14 +1113,14 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
setControlEVHeight();
|
||||
resetHeight();
|
||||
}
|
||||
|
||||
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
|
||||
// switch to baro if there was a reset to baro
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
|
||||
// we have just switched to using baro height, we don't need to set a height sensor offset
|
||||
// since we track a separate _baro_hgt_offset
|
||||
@ -1138,7 +1131,7 @@ void Ekf::controlHeightFusion()
|
||||
// TODO: Add EV normal case here
|
||||
// determine if we should use the vertical position observation
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1164,18 +1157,25 @@ void Ekf::controlHeightFusion()
|
||||
|
||||
}
|
||||
|
||||
height_fuse_mask[VPOS] = true;
|
||||
_fuse_height = true;
|
||||
}
|
||||
|
||||
if (height_fuse_mask[VPOS]) {
|
||||
if (_fuse_height) {
|
||||
|
||||
Vector2f height_innov_gate{};
|
||||
Vector2f height_test_ratio{};
|
||||
Vector3f height_obs_var{};
|
||||
Vector3f height_innov_var{};
|
||||
Vector3f height_innov{};
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
// vertical position innovation - baro measurement has opposite sign to earth z axis
|
||||
height_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
|
||||
height_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
|
||||
// observation variance - user parameter defined
|
||||
height_obs_var[5] = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
height_obs_var(2) = sq(fmaxf(_params.baro_noise, 0.01f));
|
||||
|
||||
// innovation gate size
|
||||
height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
|
||||
// Compensate for positive static pressure transients (negative vertical position innovations)
|
||||
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
|
||||
@ -1183,53 +1183,52 @@ void Ekf::controlHeightFusion()
|
||||
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
|
||||
|
||||
if (_control_status.flags.gnd_effect) {
|
||||
if (height_innov[5] < -deadzone_start) {
|
||||
if (height_innov[5] <= -deadzone_end) {
|
||||
height_innov[5] += deadzone_end;
|
||||
if (height_innov(2) < -deadzone_start) {
|
||||
if (height_innov(2) <= -deadzone_end) {
|
||||
height_innov(2) += deadzone_end;
|
||||
|
||||
} else {
|
||||
height_innov[5] = -deadzone_start;
|
||||
height_innov(2) = -deadzone_start;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
// vertical position innovation - gps measurement has opposite sign to earth z axis
|
||||
height_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
|
||||
height_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
|
||||
// observation variance - receiver defined and parameter limited
|
||||
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
|
||||
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
|
||||
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
|
||||
height_obs_var[5] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
|
||||
height_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
|
||||
// innovation gate size
|
||||
height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
height_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
|
||||
|
||||
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
|
||||
// use range finder with tilt correction
|
||||
height_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
|
||||
height_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
|
||||
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
|
||||
// observation variance - user parameter defined
|
||||
height_obs_var[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
|
||||
height_obs_var(2) = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
|
||||
// innovation gate size
|
||||
height_innov_gate[VPOS] = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
height_innov_gate(1) = fmaxf(_params.range_innov_gate, 1.0f);
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
// calculate the innovation assuming the external vision observation is in local NED frame
|
||||
height_innov[5] = _state.pos(2) - _ev_sample_delayed.pos(2);
|
||||
height_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
|
||||
// observation variance - defined externally
|
||||
height_obs_var[5] = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
|
||||
height_obs_var(2) = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
|
||||
// innovation gate size
|
||||
height_innov_gate[VPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
height_innov_gate(1) = fmaxf(_params.ev_pos_innov_gate, 1.0f);
|
||||
}
|
||||
// fuse height inforamtion
|
||||
fuseVelPosHeightSeq(height_innov,height_innov_gate,
|
||||
height_obs_var,height_fuse_mask,
|
||||
height_innov_var,height_test_ratio);
|
||||
fuseVerticalPosition(height_innov,height_innov_gate,
|
||||
height_obs_var, height_innov_var,height_test_ratio);
|
||||
|
||||
// This is a temporary hack until we do proper height sensor fusion
|
||||
_gps_vel_pos_innov[5] = height_innov[5];
|
||||
_gps_vel_pos_innov_var[5] = height_innov_var[5];
|
||||
_gps_vel_pos_test_ratio[VPOS] = height_test_ratio[VPOS];
|
||||
_gps_pos_innov(2) = height_innov(2);
|
||||
_gps_pos_innov_var(2) = height_innov_var(2);
|
||||
_gps_pos_test_ratio(1) = height_test_ratio(1);
|
||||
}
|
||||
|
||||
}
|
||||
@ -1371,9 +1370,6 @@ void Ekf::controlFakePosFusion()
|
||||
{
|
||||
// if we aren't doing any aiding, fake position measurements at the last known position to constrain drift
|
||||
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
|
||||
bool fake_gps_fuse_mask[4]{};
|
||||
float fake_gps_obs_var[6]{};
|
||||
float fake_gps_innov_gate[4]{};
|
||||
|
||||
if (!_control_status.flags.gps &&
|
||||
!_control_status.flags.opt_flow &&
|
||||
@ -1386,6 +1382,11 @@ void Ekf::controlFakePosFusion()
|
||||
|
||||
// Fuse synthetic position observations every 200msec
|
||||
if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) {
|
||||
|
||||
Vector3f fake_gps_pos_obs_var{};
|
||||
Vector2f fake_gps_pos_innov_gate{};
|
||||
|
||||
|
||||
// Reset position and velocity states if we re-commence this aiding method
|
||||
if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) {
|
||||
resetPosition();
|
||||
@ -1397,27 +1398,23 @@ void Ekf::controlFakePosFusion()
|
||||
}
|
||||
|
||||
}
|
||||
// Fuse horizontal position
|
||||
fake_gps_fuse_mask[HPOS] = true;
|
||||
|
||||
_time_last_fake_pos = _time_last_imu;
|
||||
|
||||
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
|
||||
fake_gps_obs_var[3] = fake_gps_obs_var[4] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
||||
fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
|
||||
|
||||
} else {
|
||||
fake_gps_obs_var[3] = fake_gps_obs_var[4] = 0.5f;
|
||||
fake_gps_pos_obs_var(0) = fake_gps_pos_obs_var(1) = 0.5f;
|
||||
}
|
||||
|
||||
_gps_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
|
||||
_gps_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
|
||||
_gps_pos_innov(0) = _state.pos(0) - _last_known_posNE(0);
|
||||
_gps_pos_innov(1) = _state.pos(1) - _last_known_posNE(1);
|
||||
|
||||
// glitch protection is not required so set gate to a large value
|
||||
fake_gps_innov_gate[HPOS] = 100.0f;
|
||||
fake_gps_pos_innov_gate(0) = 100.0f;
|
||||
|
||||
fuseVelPosHeightSeq(_gps_vel_pos_innov,fake_gps_innov_gate,
|
||||
fake_gps_obs_var,fake_gps_fuse_mask,
|
||||
_gps_vel_pos_innov_var,_gps_vel_pos_test_ratio);
|
||||
fuseHorizontalPosition(_gps_pos_innov, fake_gps_pos_innov_gate, fake_gps_pos_obs_var,
|
||||
_gps_pos_innov_var, _gps_pos_test_ratio);
|
||||
}
|
||||
|
||||
} else {
|
||||
@ -1433,31 +1430,17 @@ void Ekf::controlAuxVelFusion()
|
||||
|
||||
if (data_ready && primary_aiding) {
|
||||
|
||||
bool auxvel_fuse_mask[4]{};
|
||||
float auxvel_innov[6]{};
|
||||
float auxvel_innov_gate[4]{};
|
||||
float auxvel_obs_var[4]{};
|
||||
float auxvel_innov_var[6]{};
|
||||
float auxvel_test_ratio[4]{};
|
||||
Vector2f aux_vel_innov_gate{};
|
||||
Vector3f aux_vel_obs_var{};
|
||||
|
||||
auxvel_fuse_mask[HVEL] = true;
|
||||
auxvel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
|
||||
auxvel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
|
||||
auxvel_innov_gate[HVEL] = _params.auxvel_gate;
|
||||
auxvel_obs_var[0] = _auxvel_sample_delayed.velVarNE(0);
|
||||
auxvel_obs_var[1] = _auxvel_sample_delayed.velVarNE(1);
|
||||
_aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
|
||||
_aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
|
||||
aux_vel_innov_gate(0) = _params.auxvel_gate;
|
||||
aux_vel_obs_var(0) = _auxvel_sample_delayed.velVarNE(0);
|
||||
aux_vel_obs_var(1) = _auxvel_sample_delayed.velVarNE(1);
|
||||
|
||||
fuseVelPosHeightSeq(auxvel_innov,auxvel_innov_gate,
|
||||
auxvel_obs_var,auxvel_fuse_mask,
|
||||
auxvel_innov_var,auxvel_test_ratio);
|
||||
|
||||
_aux_vel_innov[0] = auxvel_innov[0];
|
||||
_aux_vel_innov[1] = auxvel_innov[1];
|
||||
|
||||
_aux_vel_innov_var[0] = auxvel_innov_var[0];
|
||||
_aux_vel_innov_var[1] = auxvel_innov_var[1];
|
||||
|
||||
_aux_vel_test_ratio = auxvel_test_ratio[HVEL];
|
||||
fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, aux_vel_obs_var,
|
||||
_aux_vel_innov_var, _aux_vel_test_ratio);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -302,9 +302,9 @@ bool Ekf::initialiseFilter()
|
||||
|
||||
// reset the essential fusion timeout counters
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_delpos_fuse = _time_last_imu;
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
_time_last_hagl_fuse = _time_last_imu;
|
||||
_time_last_of_fuse = _time_last_imu;
|
||||
|
||||
|
||||
55
EKF/ekf.h
55
EKF/ekf.h
@ -330,10 +330,12 @@ private:
|
||||
uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
|
||||
bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift
|
||||
|
||||
uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
|
||||
// TODO: Split those into sensor and measurement specifics
|
||||
uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
|
||||
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec)
|
||||
uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec)
|
||||
uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec)
|
||||
uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)
|
||||
uint64_t _time_last_vel_fuse{0}; ///< time the last fusion of velocity measurements was performed (uSec)
|
||||
uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of height measurements was performed (uSec)
|
||||
uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
|
||||
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
|
||||
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
|
||||
@ -373,17 +375,20 @@ private:
|
||||
Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance
|
||||
|
||||
|
||||
float _vel_pos_innov[6] {}; ///< velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
|
||||
float _vel_pos_innov_var[6] {}; ///< velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
|
||||
Vector3f _gps_vel_innov {}; ///< GPS velocity innovations (m/sec)
|
||||
Vector3f _gps_vel_innov_var {}; ///< GPS velocity innovation variances ((m/sec)**2)
|
||||
|
||||
float _gps_vel_pos_innov[6] {}; ///< GPS velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
|
||||
float _gps_vel_pos_innov_var[6] {}; ///< GPS velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
|
||||
Vector3f _gps_pos_innov {}; ///< GPS position innovations (m)
|
||||
Vector3f _gps_pos_innov_var {}; ///< GPS position innovation variances (m**2)
|
||||
|
||||
float _ev_vel_pos_innov[6] {}; ///< external vision velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
|
||||
float _ev_vel_pos_innov_var[6] {}; ///< external vision velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
|
||||
Vector3f _ev_vel_innov {}; ///< external vision velocity innovations (m/sec)
|
||||
Vector3f _ev_vel_innov_var {}; ///< external vision velocity innovation variances ((m/sec)**2)
|
||||
|
||||
float _aux_vel_innov[2] {}; ///< horizontal auxiliary velocity innovations: (m/sec)
|
||||
float _aux_vel_innov_var[2] {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
|
||||
Vector3f _ev_pos_innov {}; ///< external vision position innovations (m)
|
||||
Vector3f _ev_pos_innov_var {}; ///< external vision position innovation variances (m**2)
|
||||
|
||||
Vector3f _aux_vel_innov {}; ///< horizontal auxiliary velocity innovations: (m/sec)
|
||||
Vector3f _aux_vel_innov_var {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
|
||||
|
||||
float _heading_innov{0.0f}; ///< heading measurement innovation (rad)
|
||||
float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2)
|
||||
@ -546,11 +551,6 @@ private:
|
||||
// fuse body frame drag specific forces for multi-rotor wind estimation
|
||||
void fuseDrag();
|
||||
|
||||
// fuse velocity and position measurements sequentially
|
||||
void fuseVelPosHeightSeq(const float (&innov)[6], const float (&innov_gate)[4],
|
||||
const float (obs_var)[6], bool (&fuse_mask)[4],
|
||||
float (&innov_var)[6], float (&test_ratio)[4]);
|
||||
|
||||
// fuse single velocity and position measurement
|
||||
void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
|
||||
|
||||
@ -560,6 +560,18 @@ private:
|
||||
// fuse optical flow line of sight rate measurements
|
||||
void fuseOptFlow();
|
||||
|
||||
bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
||||
bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
||||
bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
||||
bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio);
|
||||
|
||||
// calculate optical flow body angular rate compensation
|
||||
// returns false if bias corrected body rate data is unavailable
|
||||
bool calcOptFlowBodyRateComp();
|
||||
@ -789,37 +801,26 @@ private:
|
||||
// sensor measurement
|
||||
float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted);
|
||||
|
||||
// stop GPS fusion
|
||||
void stopGpsFusion();
|
||||
|
||||
// stop GPS position fusion
|
||||
void stopGpsPosFusion();
|
||||
|
||||
// stop GPS velocity fusion
|
||||
void stopGpsVelFusion();
|
||||
|
||||
// stop GPS yaw fusion
|
||||
void stopGpsYawFusion();
|
||||
|
||||
// stop external vision fusion
|
||||
void stopEvFusion();
|
||||
|
||||
// stop external vision position fusion
|
||||
void stopEvPosFusion();
|
||||
|
||||
// stop external vision velocity fusion
|
||||
void stopEvVelFusion();
|
||||
|
||||
// stop external vision yaw fusion
|
||||
void stopEvYawFusion();
|
||||
|
||||
// stop auxiliary horizontal velocity fusion
|
||||
void stopAuxVelFusion();
|
||||
|
||||
// stop optical flow fusion
|
||||
void stopFlowFusion();
|
||||
|
||||
// Helper function to set velPos fault status
|
||||
void setVelPosFaultStatus(const int index, const bool status);
|
||||
|
||||
};
|
||||
|
||||
@ -418,7 +418,7 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
|
||||
// check for excessive horizontal GPS velocity innovations
|
||||
bool badVelInnov = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) && _control_status.flags.gps;
|
||||
bool badVelInnov = (_gps_vel_test_ratio(0) > 1.0f) && _control_status.flags.gps;
|
||||
|
||||
// calculate GPS course over ground angle
|
||||
float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
|
||||
@ -835,65 +835,75 @@ void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
|
||||
|
||||
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
||||
{
|
||||
memcpy(hvel, _gps_vel_pos_innov+0, sizeof(float) * 2);
|
||||
memcpy(&vvel, _gps_vel_pos_innov+2, sizeof(float) * 1);
|
||||
memcpy(hpos, _gps_vel_pos_innov+3, sizeof(float) * 2);
|
||||
memcpy(&vpos, _gps_vel_pos_innov+5, sizeof(float) * 1);
|
||||
hvel[0] = _gps_vel_innov(0);
|
||||
hvel[1] = _gps_vel_innov(1);
|
||||
vvel = _gps_vel_innov(2);
|
||||
hpos[0] = _gps_pos_innov(0);
|
||||
hpos[1] = _gps_pos_innov(1);
|
||||
vpos = _gps_pos_innov(2);
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
||||
{
|
||||
memcpy(hvel, _gps_vel_pos_innov_var+0, sizeof(float) * 2);
|
||||
memcpy(&vvel, _gps_vel_pos_innov_var+2, sizeof(float) * 1);
|
||||
memcpy(hpos, _gps_vel_pos_innov_var+3, sizeof(float) * 2);
|
||||
memcpy(&vpos, _gps_vel_pos_innov_var+5, sizeof(float) * 1);
|
||||
hvel[0] = _gps_vel_innov_var(0);
|
||||
hvel[1] = _gps_vel_innov_var(1);
|
||||
vvel = _gps_vel_innov_var(2);
|
||||
hpos[0] = _gps_pos_innov_var(0);
|
||||
hpos[1] = _gps_pos_innov_var(1);
|
||||
vpos = _gps_pos_innov_var(2);
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
|
||||
{
|
||||
memcpy(&hvel, _gps_vel_pos_test_ratio+HVEL, sizeof(float));
|
||||
memcpy(&vvel, _gps_vel_pos_test_ratio+VVEL, sizeof(float));
|
||||
memcpy(&hpos, _gps_vel_pos_test_ratio+HPOS, sizeof(float));
|
||||
memcpy(&vpos, _gps_vel_pos_test_ratio+VPOS, sizeof(float));
|
||||
hvel = _gps_vel_test_ratio(0);
|
||||
vvel = _gps_vel_test_ratio(1);
|
||||
hpos = _gps_pos_test_ratio(0);
|
||||
vpos = _gps_pos_test_ratio(1);
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
||||
{
|
||||
memcpy(hvel, _ev_vel_pos_innov+0, sizeof(float) * 2);
|
||||
memcpy(&vvel, _ev_vel_pos_innov+2, sizeof(float) * 1);
|
||||
memcpy(hpos, _ev_vel_pos_innov+3, sizeof(float) * 2);
|
||||
memcpy(&vpos, _ev_vel_pos_innov+5, sizeof(float) * 1);
|
||||
hvel[0] = _ev_vel_innov(0);
|
||||
hvel[1] = _ev_vel_innov(1);
|
||||
vvel = _ev_vel_innov(2);
|
||||
hpos[0] = _ev_pos_innov(0);
|
||||
hpos[1] = _ev_pos_innov(1);
|
||||
vpos = _ev_pos_innov(2);
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos)
|
||||
{
|
||||
memcpy(hvel, _ev_vel_pos_innov_var+0, sizeof(float) * 2);
|
||||
memcpy(&vvel, _ev_vel_pos_innov_var+2, sizeof(float) * 1);
|
||||
memcpy(hpos, _ev_vel_pos_innov_var+3, sizeof(float) * 2);
|
||||
memcpy(&vpos, _ev_vel_pos_innov_var+5, sizeof(float) * 1);
|
||||
hvel[0] = _ev_vel_innov_var(0);
|
||||
hvel[1] = _ev_vel_innov_var(1);
|
||||
vvel = _ev_vel_innov_var(2);
|
||||
hpos[0] = _ev_pos_innov_var(0);
|
||||
hpos[1] = _ev_pos_innov_var(1);
|
||||
vpos = _ev_pos_innov_var(2);
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos)
|
||||
{
|
||||
memcpy(&hvel, _ev_vel_pos_test_ratio+HVEL, sizeof(float));
|
||||
memcpy(&vvel, _ev_vel_pos_test_ratio+VVEL, sizeof(float));
|
||||
memcpy(&hpos, _ev_vel_pos_test_ratio+HPOS, sizeof(float));
|
||||
memcpy(&vpos, _ev_vel_pos_test_ratio+VPOS, sizeof(float));
|
||||
hvel = _ev_vel_test_ratio(0);
|
||||
vvel = _ev_vel_test_ratio(1);
|
||||
hpos = _ev_pos_test_ratio(0);
|
||||
vpos = _ev_pos_test_ratio(1);
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2])
|
||||
{
|
||||
memcpy(aux_vel_innov, _aux_vel_innov, sizeof(_aux_vel_innov));
|
||||
aux_vel_innov[0] = _aux_vel_innov(0);
|
||||
aux_vel_innov[1] = _aux_vel_innov(1);
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2])
|
||||
{
|
||||
memcpy(aux_vel_innov_var, _aux_vel_innov_var, sizeof(_aux_vel_innov_var));
|
||||
aux_vel_innov_var[0] = _aux_vel_innov_var(0);
|
||||
aux_vel_innov_var[1] = _aux_vel_innov_var(1);
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio)
|
||||
{
|
||||
memcpy(&aux_vel_innov_ratio, &_aux_vel_test_ratio, sizeof(_aux_vel_test_ratio));
|
||||
aux_vel_innov_ratio = _aux_vel_test_ratio(0);
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnov(float flow_innov[2])
|
||||
@ -908,22 +918,22 @@ void Ekf::getFlowInnovVar(float flow_innov_var[2])
|
||||
|
||||
void Ekf::getFlowInnovRatio(float &flow_innov_ratio)
|
||||
{
|
||||
memcpy(&flow_innov_ratio, &_optflow_test_ratio, sizeof(_optflow_test_ratio));
|
||||
flow_innov_ratio = _optflow_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnov(float &heading_innov)
|
||||
{
|
||||
memcpy(&heading_innov, &_heading_innov, sizeof(_heading_innov));
|
||||
heading_innov = _heading_innov;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnovVar(float &heading_innov_var)
|
||||
{
|
||||
memcpy(&heading_innov_var, &_heading_innov_var, sizeof(_heading_innov_var));
|
||||
heading_innov_var = _heading_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnovRatio(float &heading_innov_ratio)
|
||||
{
|
||||
memcpy(&heading_innov_ratio, &_yaw_test_ratio, sizeof(_yaw_test_ratio));
|
||||
heading_innov_ratio = _yaw_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getMagInnov(float mag_innov[3])
|
||||
@ -958,47 +968,47 @@ void Ekf::getDragInnovRatio(float drag_innov_ratio[2])
|
||||
|
||||
void Ekf::getAirspeedInnov(float &airspeed_innov)
|
||||
{
|
||||
memcpy(&airspeed_innov, &_airspeed_innov, sizeof(_airspeed_innov));
|
||||
airspeed_innov = _airspeed_innov;
|
||||
}
|
||||
|
||||
void Ekf::getAirspeedInnovVar(float &airspeed_innov_var)
|
||||
{
|
||||
memcpy(&airspeed_innov_var, &_airspeed_innov_var, sizeof(_airspeed_innov_var));
|
||||
airspeed_innov_var = _airspeed_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio)
|
||||
{
|
||||
memcpy(&airspeed_innov_ratio, &_tas_test_ratio, sizeof(_tas_test_ratio));
|
||||
airspeed_innov_ratio = _tas_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnov(float &beta_innov)
|
||||
{
|
||||
memcpy(&beta_innov, &_beta_innov, sizeof(_beta_innov));
|
||||
beta_innov = _beta_innov;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnovVar(float &beta_innov_var)
|
||||
{
|
||||
memcpy(&beta_innov_var, &_beta_innov_var, sizeof(_beta_innov_var));
|
||||
beta_innov_var = _beta_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnovRatio(float &beta_innov_ratio)
|
||||
{
|
||||
memcpy(&beta_innov_ratio, &_beta_test_ratio, sizeof(_beta_test_ratio));
|
||||
beta_innov_ratio = _beta_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnov(float &hagl_innov)
|
||||
{
|
||||
memcpy(&hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
|
||||
hagl_innov = _hagl_innov;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnovVar(float &hagl_innov_var)
|
||||
{
|
||||
memcpy(&hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
|
||||
hagl_innov_var = _hagl_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnovRatio(float &hagl_innov_ratio)
|
||||
{
|
||||
memcpy(&hagl_innov_ratio, &_hagl_test_ratio, sizeof(_hagl_test_ratio));
|
||||
hagl_innov_ratio = _hagl_test_ratio;
|
||||
}
|
||||
|
||||
// get GPS check status
|
||||
@ -1121,8 +1131,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
|
||||
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
|
||||
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
||||
// and using state variances for accuracy reporting is overly optimistic in these situations
|
||||
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
|
||||
if (_is_dead_reckoning && (_control_status.flags.gps)) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
|
||||
}
|
||||
else if (_is_dead_reckoning && (_control_status.flags.ev_pos)) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
|
||||
}
|
||||
|
||||
*ekf_eph = hpos_err;
|
||||
@ -1138,8 +1151,8 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
|
||||
// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
|
||||
// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
|
||||
// and using state variances for accuracy reporting is overly optimistic in these situations
|
||||
if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
|
||||
if (_is_dead_reckoning && _control_status.flags.gps) {
|
||||
hpos_err = math::max(hpos_err, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
|
||||
}
|
||||
|
||||
*ekf_eph = hpos_err;
|
||||
@ -1162,15 +1175,16 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
|
||||
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
|
||||
}
|
||||
|
||||
if (_control_status.flags.gps || _control_status.flags.ev_pos) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
|
||||
if (_control_status.flags.gps) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_gps_pos_innov(0)) + sq(_gps_pos_innov(1))));
|
||||
}
|
||||
else if (_control_status.flags.ev_pos) {
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1))));
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_vel) {
|
||||
// What is the right thing to do here
|
||||
// vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
|
||||
vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1))));
|
||||
}
|
||||
|
||||
hvel_err = math::max(hvel_err, vel_err_conservative);
|
||||
}
|
||||
|
||||
@ -1270,15 +1284,15 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &gps_ve
|
||||
// return the largest magnetometer innovation test ratio
|
||||
mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
|
||||
// return the largest NED GPS velocity innovation test ratio
|
||||
gps_vel = sqrtf(math::max(_gps_vel_pos_test_ratio[HVEL], _gps_vel_pos_test_ratio[VVEL]));
|
||||
gps_vel = sqrtf(math::max(_gps_vel_test_ratio(0), _gps_vel_test_ratio(1)));
|
||||
// return the largest NE GPS position innovation test ratio
|
||||
gps_pos = sqrtf(_gps_vel_pos_test_ratio[HPOS]);
|
||||
gps_pos = sqrtf(_gps_pos_test_ratio(0));
|
||||
// return the largest external vision velocity innovation test ratio
|
||||
ev_vel = sqrtf(math::max(_ev_vel_pos_test_ratio[HVEL], _ev_vel_pos_test_ratio[VVEL]));
|
||||
ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1)));
|
||||
// return the largest horizontal external vision position innovation test ratio
|
||||
ev_pos = sqrtf(_ev_vel_pos_test_ratio[HPOS]);
|
||||
ev_pos = sqrtf(_ev_pos_test_ratio(0));
|
||||
// return the vertical position innovation test ratio
|
||||
hgt = sqrtf(_gps_vel_pos_test_ratio[VPOS]);
|
||||
hgt = sqrtf(_gps_pos_test_ratio(0));
|
||||
// return the airspeed fusion innovation test ratio
|
||||
tas = sqrtf(_tas_test_ratio);
|
||||
// return the terrain height innovation test ratio
|
||||
@ -1302,8 +1316,8 @@ void Ekf::get_ekf_soln_status(uint16_t *status)
|
||||
soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
|
||||
soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
|
||||
soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
|
||||
bool gps_vel_innov_bad = (_gps_vel_pos_test_ratio[HVEL] > 1.0f) || (_gps_vel_pos_test_ratio[VVEL] > 1.0f);
|
||||
bool gps_pos_innov_bad = (_gps_vel_pos_test_ratio[HPOS] > 1.0f);
|
||||
bool gps_vel_innov_bad = (_gps_vel_test_ratio(0) > 1.0f) || (_gps_vel_test_ratio(1) > 1.0f);
|
||||
bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f);
|
||||
bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
|
||||
soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
|
||||
soln_status.flags.accel_error = _bad_vert_accel_detected;
|
||||
@ -1438,8 +1452,8 @@ bool Ekf::global_position_is_valid()
|
||||
void Ekf::update_deadreckoning_status()
|
||||
{
|
||||
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel)
|
||||
&& (((_time_last_imu - _time_last_pos_fuse) <= _params.no_aid_timeout_max)
|
||||
|| ((_time_last_imu - _time_last_vel_fuse) <= _params.no_aid_timeout_max)
|
||||
&& (((_time_last_imu - _time_last_hor_pos_fuse) <= _params.no_aid_timeout_max)
|
||||
|| ((_time_last_imu - _time_last_hor_vel_fuse) <= _params.no_aid_timeout_max)
|
||||
|| ((_time_last_imu - _time_last_delpos_fuse) <= _params.no_aid_timeout_max));
|
||||
bool optFlowAiding = _control_status.flags.opt_flow && ((_time_last_imu - _time_last_of_fuse) <= _params.no_aid_timeout_max);
|
||||
bool airDataAiding = _control_status.flags.wind && ((_time_last_imu - _time_last_arsp_fuse) <= _params.no_aid_timeout_max) && ((_time_last_imu - _time_last_beta_fuse) <= _params.no_aid_timeout_max);
|
||||
@ -1892,16 +1906,16 @@ void Ekf::stopGpsPosFusion()
|
||||
{
|
||||
_control_status.flags.gps = false;
|
||||
_control_status.flags.gps_hgt = false;
|
||||
memset(_gps_vel_pos_innov+3,0.0f, sizeof(float)*3);
|
||||
memset(_gps_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
|
||||
memset(_gps_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
|
||||
_gps_pos_innov.setZero();
|
||||
_gps_pos_innov_var.setZero();
|
||||
_gps_pos_test_ratio.setZero();
|
||||
}
|
||||
|
||||
void Ekf::stopGpsVelFusion()
|
||||
{
|
||||
memset(_gps_vel_pos_innov,0.0f, sizeof(float)*3);
|
||||
memset(_gps_vel_pos_innov_var,0.0f, sizeof(float)*3);
|
||||
memset(_gps_vel_pos_test_ratio,0.0f, sizeof(float)*2);
|
||||
_gps_vel_innov.setZero();
|
||||
_gps_vel_innov_var.setZero();
|
||||
_gps_vel_test_ratio.setZero();
|
||||
}
|
||||
|
||||
void Ekf::stopGpsYawFusion()
|
||||
@ -1919,27 +1933,29 @@ void Ekf::stopEvFusion()
|
||||
void Ekf::stopEvPosFusion()
|
||||
{
|
||||
_control_status.flags.ev_pos = false;
|
||||
memset(_ev_vel_pos_innov+3,0.0f, sizeof(float)*3);
|
||||
memset(_ev_vel_pos_innov_var+3,0.0f, sizeof(float)*3);
|
||||
memset(_ev_vel_pos_test_ratio+HPOS,0.0f, sizeof(float)*2);
|
||||
_ev_pos_innov.setZero();
|
||||
_ev_pos_innov_var.setZero();
|
||||
_ev_pos_test_ratio.setZero();
|
||||
}
|
||||
|
||||
void Ekf::stopEvVelFusion()
|
||||
{
|
||||
_control_status.flags.ev_vel = false;
|
||||
memset(_ev_vel_pos_innov,0.0f, sizeof(float)*3);
|
||||
memset(_ev_vel_pos_innov_var,0.0f, sizeof(float)*3);
|
||||
memset(_ev_vel_pos_test_ratio,0.0f, sizeof(float)*2);}
|
||||
_ev_vel_innov.setZero();
|
||||
_ev_vel_innov_var.setZero();
|
||||
_ev_vel_test_ratio.setZero();
|
||||
}
|
||||
|
||||
void Ekf::stopEvYawFusion()
|
||||
{
|
||||
_control_status.flags.ev_yaw = false;
|
||||
|
||||
}
|
||||
|
||||
void Ekf::stopAuxVelFusion()
|
||||
{
|
||||
// TODO: Add proper handling of auxiliar velocity fusion
|
||||
_aux_vel_innov.setZero();
|
||||
_aux_vel_innov_var.setZero();
|
||||
_aux_vel_test_ratio.setZero();
|
||||
}
|
||||
|
||||
void Ekf::stopFlowFusion()
|
||||
|
||||
@ -67,82 +67,44 @@ public:
|
||||
virtual void resetStatesAndCovariances() = 0;
|
||||
virtual bool update() = 0;
|
||||
|
||||
// gets the GPS innovations of velocity and position measurements
|
||||
virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;
|
||||
// gets the GPS innovation variances of velocity and position measurements
|
||||
virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;
|
||||
// gets the GPS innovation test ratios of velocity and position measurements
|
||||
virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0;
|
||||
|
||||
|
||||
// gets the external vision innovations of velocity and position measurements
|
||||
virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;
|
||||
// gets the external vision innovation variances of velocity and position measurements
|
||||
virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;
|
||||
// gets the external vision innovation test ratios of velocity and position measurements
|
||||
virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0;
|
||||
|
||||
|
||||
// gets the innovations for of the horizontal auxiliary velocity measurement
|
||||
virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0;
|
||||
// gets the innovation variances for of the horizontal auxiliary velocity measurement
|
||||
virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0;
|
||||
// gets the innovation test ratios of the horizontal auxiliary velocity measurement
|
||||
virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovation of the flow measurement
|
||||
virtual void getFlowInnov(float flow_innov[2]) = 0;
|
||||
// gets the innovation variance of the flow measurement
|
||||
virtual void getFlowInnovVar(float flow_innov_var[2]) = 0;
|
||||
// gets the innovation test ratios of the flow measurement
|
||||
virtual void getFlowInnovRatio(float &flow_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovations of the heading measurement
|
||||
virtual void getHeadingInnov(float &heading_innov) = 0;
|
||||
// gets the innovation variance of the heading measurement
|
||||
virtual void getHeadingInnovVar(float &heading_innov_var) = 0;
|
||||
// gets the innovation test ratios of the heading measurement
|
||||
virtual void getHeadingInnovRatio(float &heading_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovations of the earth magnetic field measurements
|
||||
virtual void getMagInnov(float mag_innov[3]) = 0;
|
||||
// gets the innovation variances of the earth magnetic field measurements
|
||||
virtual void getMagInnovVar(float mag_innov_var[3]) = 0;
|
||||
// gets the innovation test ratios of the earth magnetic field measurements
|
||||
virtual void getMagInnovRatio(float &mag_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovation of the drag specific force measurement
|
||||
virtual void getDragInnov(float drag_innov[2]) = 0;
|
||||
// gets the innovation variance of the drag specific force measurement
|
||||
virtual void getDragInnovVar(float drag_innov_var[2]) = 0;
|
||||
// gets the innovation test ratios of the drag specific force measurement
|
||||
virtual void getDragInnovRatio(float drag_innov_ratio[2]) = 0;
|
||||
|
||||
// gets the innovation of airspeed measurement
|
||||
virtual void getAirspeedInnov(float &airspeed_innov) = 0;
|
||||
// gets the innovation variance of the airspeed measurement
|
||||
virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) = 0;
|
||||
// gets the innovation test ratios of the airspeed measurement
|
||||
virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovation of the synthetic sideslip measurement
|
||||
virtual void getBetaInnov(float &beta_innov) = 0;
|
||||
// gets the innovation variance of the synthetic sideslip measurement
|
||||
virtual void getBetaInnovVar(float &get_beta_innov_var) = 0;
|
||||
// gets the innovation test ratios of the synthetic sideslip measurement
|
||||
virtual void getBetaInnovRatio(float &beta_innov_ratio) = 0;
|
||||
|
||||
|
||||
// gets the innovation of the HAGL measurement
|
||||
virtual void getHaglInnov(float &hagl_innov) = 0;
|
||||
// gets the innovation variance of the HAGL measurement
|
||||
virtual void getHaglInnovVar(float &hagl_innov_var) = 0;
|
||||
// gets the innovation test ratios of the HAGL measurement
|
||||
virtual void getHaglInnovRatio(float &hagl_innov_ratio) = 0;
|
||||
|
||||
|
||||
@ -512,10 +474,12 @@ protected:
|
||||
// innovation consistency check monitoring ratios
|
||||
float _yaw_test_ratio{}; // yaw innovation consistency check ratio
|
||||
float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios
|
||||
float _gps_vel_pos_test_ratio[4] {}; // GPS velocity and position innovation consistency check ratios
|
||||
float _ev_vel_pos_test_ratio[4] {}; // EV velocity and position innovation consistency check ratios
|
||||
float _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
Vector2f _gps_vel_test_ratio {}; // GPS velocity innovation consistency check ratios
|
||||
Vector2f _gps_pos_test_ratio {}; // GPS position innovation consistency check ratios
|
||||
Vector2f _ev_vel_test_ratio {}; // EV velocity innovation consistency check ratios
|
||||
Vector2f _ev_pos_test_ratio {}; // EV position innovation consistency check ratios
|
||||
Vector2f _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio
|
||||
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
|
||||
float _tas_test_ratio{}; // tas innovation consistency check ratio
|
||||
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
|
||||
float _beta_test_ratio{}; // sideslip innovation consistency check ratio
|
||||
|
||||
@ -45,98 +45,97 @@
|
||||
#include <ecl.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
/**
|
||||
* Update the EKF state with velocity and position measurements sequentially. [(vx vy) (vz) (x y) (z)]
|
||||
*
|
||||
* @param innov Input [vx vy vz x y z]
|
||||
* @param innov_gate Input [vxy vz xy z]
|
||||
* @param obs_var Input [vx vy vz x y z]
|
||||
* @param fuse_mask Input/Output [vxy vz xy z]
|
||||
* Specify which innovation components should be fused,
|
||||
* components that do not pass innovations checks will be set to zero
|
||||
* @param innov_var Ouput [vx vy vz x y z]
|
||||
* @param test_ratio Output [vxy vz xy z]
|
||||
*/
|
||||
void Ekf::fuseVelPosHeightSeq(const float (&innov)[6], const float (&innov_gate)[4],
|
||||
const float (obs_var)[6], bool (&fuse_mask)[4],
|
||||
float (&innov_var)[6], float (&test_ratio)[4])
|
||||
|
||||
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
|
||||
{
|
||||
// check position, velocity and height innovations sequentially and if checks are passed fuse it
|
||||
// treat 2D horizintal velocity, vertical velocity, 2D horizontal position and vertical height as separate sensors
|
||||
// At the moment we still fuse velocity as 3D measurement, but this should be split in the future
|
||||
innov_var(0) = P[4][4] + obs_var(0);
|
||||
innov_var(1) = P[5][5] + obs_var(1);
|
||||
test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
|
||||
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
|
||||
|
||||
// horizontal and vertical velocity
|
||||
if(fuse_mask[HVEL] && fuse_mask[VVEL]){
|
||||
innov_var[0] = P[4][4] + obs_var[0];
|
||||
innov_var[1] = P[5][5] + obs_var[1];
|
||||
test_ratio[HVEL] = fmaxf( sq(innov[0]) / (sq(innov_gate[HVEL]) * innov_var[0]),
|
||||
sq(innov[1]) / (sq(innov_gate[HVEL]) * innov_var[1]));
|
||||
bool innov_check_pass = (test_ratio(0) <= 1.0f);
|
||||
if (innov_check_pass)
|
||||
{
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_hor_vel = false;
|
||||
|
||||
innov_var[2] = P[6][6] + obs_var[2];
|
||||
test_ratio[VVEL] = sq(innov[2]) / (sq(innov_gate[VVEL]) * innov_var[2]);
|
||||
fuseVelPosHeight(innov(0),innov_var(0),0);
|
||||
fuseVelPosHeight(innov(1),innov_var(1),1);
|
||||
|
||||
bool innov_check_pass = (test_ratio[HVEL] <= 1.0f) && (test_ratio[VVEL] <= 1.0f);
|
||||
if (innov_check_pass) {
|
||||
_time_last_vel_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_vel_NED = false;
|
||||
|
||||
// fuse the horizontal and vertical velocity measurements
|
||||
fuseVelPosHeight(innov[0],innov_var[0],0);
|
||||
fuseVelPosHeight(innov[1],innov_var[1],1);
|
||||
fuseVelPosHeight(innov[2],innov_var[2],2);
|
||||
|
||||
}else{
|
||||
fuse_mask[HVEL] = fuse_mask[VVEL] = false;
|
||||
_innov_check_fail_status.flags.reject_vel_NED = true;
|
||||
}
|
||||
return true;
|
||||
}else{
|
||||
_innov_check_fail_status.flags.reject_hor_vel = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// horizontal position
|
||||
if(fuse_mask[HPOS]){
|
||||
innov_var[3] = P[7][7] + obs_var[3];
|
||||
innov_var[4] = P[8][8] + obs_var[4];
|
||||
test_ratio[HPOS] = fmaxf( sq(innov[3]) / (sq(innov_gate[HPOS]) * innov_var[3]),
|
||||
sq(innov[4]) / (sq(innov_gate[HPOS]) * innov_var[4]));
|
||||
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
|
||||
{
|
||||
innov_var(2) = P[6][6] + obs_var(2);
|
||||
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
|
||||
|
||||
bool innov_check_pass = (test_ratio[HPOS] <= 1.0f) || !_control_status.flags.tilt_align;
|
||||
bool innov_check_pass = (test_ratio(1) <= 1.0f);
|
||||
if (innov_check_pass) {
|
||||
_time_last_ver_vel_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_vel = false;
|
||||
|
||||
fuseVelPosHeight(innov(2),innov_var(2),2);
|
||||
|
||||
return true;
|
||||
}else{
|
||||
_innov_check_fail_status.flags.reject_ver_vel = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
|
||||
{
|
||||
innov_var(0) = P[7][7] + obs_var(0);
|
||||
innov_var(1) = P[8][8] + obs_var(1);
|
||||
test_ratio(0) = fmaxf( sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
|
||||
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
|
||||
|
||||
bool innov_check_pass = (test_ratio(0) <= 1.0f) || !_control_status.flags.tilt_align;
|
||||
if (innov_check_pass) {
|
||||
if (!_fuse_hpos_as_odom) {
|
||||
_time_last_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
|
||||
} else {
|
||||
_time_last_delpos_fuse = _time_last_imu;
|
||||
}
|
||||
_innov_check_fail_status.flags.reject_pos_NE = false;
|
||||
_innov_check_fail_status.flags.reject_hor_pos = false;
|
||||
|
||||
// fuse the horizontal position measurements
|
||||
fuseVelPosHeight(innov[3],innov_var[3],3);
|
||||
fuseVelPosHeight(innov[4],innov_var[4],4);
|
||||
fuseVelPosHeight(innov(0),innov_var(0),3);
|
||||
fuseVelPosHeight(innov(1),innov_var(1),4);
|
||||
|
||||
return true;
|
||||
}else{
|
||||
fuse_mask[HPOS] = false;
|
||||
_innov_check_fail_status.flags.reject_pos_NE = true;
|
||||
_innov_check_fail_status.flags.reject_hor_pos = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,
|
||||
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
|
||||
{
|
||||
innov_var(2) = P[9][9] + obs_var(2);
|
||||
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
|
||||
|
||||
bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
|
||||
if (innov_check_pass) {
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_ver_pos = false;
|
||||
|
||||
fuseVelPosHeight(innov(2),innov_var(2),5);
|
||||
|
||||
return true;
|
||||
}else{
|
||||
_innov_check_fail_status.flags.reject_ver_pos = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
// vertical position
|
||||
if(fuse_mask[VPOS]){
|
||||
innov_var[5] = P[9][9] + obs_var[5];
|
||||
test_ratio[VPOS] = sq(innov[5]) / (sq(innov_gate[VPOS]) * innov_var[5]);
|
||||
|
||||
bool innov_check_pass = (test_ratio[VPOS] <= 1.0f) || !_control_status.flags.tilt_align;
|
||||
if (innov_check_pass) {
|
||||
_time_last_hgt_fuse = _time_last_imu;
|
||||
_innov_check_fail_status.flags.reject_pos_D = false;
|
||||
|
||||
// fuse the horizontal position measurements
|
||||
fuseVelPosHeight(innov[5],innov_var[5],5);
|
||||
|
||||
}else{
|
||||
fuse_mask[VPOS] = false;
|
||||
_innov_check_fail_status.flags.reject_pos_D = true;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// Helper function that fuses a single velocity or position measurement
|
||||
@ -170,10 +169,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
|
||||
|
||||
healthy = false;
|
||||
|
||||
setVelPosFaultStatus(obs_index,true);
|
||||
setVelPosFaultStatus(obs_index, true);
|
||||
|
||||
} else {
|
||||
setVelPosFaultStatus(obs_index,false);
|
||||
setVelPosFaultStatus(obs_index, false);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user