From 7bc9217f001157e5ffcca6b2966d559597af8b1a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 7 Jun 2016 16:54:12 +1000 Subject: [PATCH 1/5] EKF: Clean up control of observation fusion All the decision for a sensor are made within a specific function for that sensor and when there is data to process at the fusion time horizon. Information and warning messages are improved. --- EKF/control.cpp | 738 +++++++++++++++++++++++++++-------------- EKF/ekf.cpp | 174 +--------- EKF/ekf.h | 26 +- EKF/ekf_helper.cpp | 55 +-- EKF/optflow_fusion.cpp | 40 ++- 5 files changed, 553 insertions(+), 480 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2f6083ce5c..1d1c7c9120 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -41,6 +41,7 @@ #include "../ecl.h" #include "ekf.h" +#include "mathlib.h" void Ekf::controlFusionModes() { @@ -60,227 +61,350 @@ void Ekf::controlFusionModes() if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + ECL_INFO("EKF alignment complete"); + } + } - // control use of various external sources for position and velocity aiding + // check for arrival of new sensor data at the fusion time horizon + _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); + _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); + _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); + _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) + && (_R_to_earth(2, 2) > 0.7071f); + _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) + && (_R_to_earth(2, 2) > 0.7071f); + _ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); + _tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); + + // check for height sensor timeouts and reset and change sensor if necessary + controlHeightSensorTimeouts(); + + // control use of observations for aiding + controlMagAiding(); controlExternalVisionAiding(); controlOpticalFlowAiding(); controlGpsAiding(); - controlHeightAiding(); - controlMagAiding(); + controlBaroAiding(); + controlRangeFinderAiding(); + controlAirDataAiding(); + + // for efficiency, fusion of direct state observations for position ad velocity is performed sequentially + // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) + controlVelPosFusion(); } void Ekf::controlExternalVisionAiding() { - // external vision position aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { - // check for a exernal vision measurement that has fallen behind the fusion time horizon - if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { - // turn on use of external vision measurements for position and height - _control_status.flags.ev_pos = true; - ECL_INFO("EKF switching to external vision position fusion"); - // turn off other forms of height aiding + // Check for new exernal vision data + if (_ev_data_ready) { + + // external vision position aiding selection logic + if ((_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_pos && _control_status.flags.tilt_align && _control_status.flags.yaw_align) { + // check for a exernal vision measurement that has fallen behind the fusion time horizon + if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { + // turn on use of external vision measurements for position and height + _control_status.flags.ev_pos = true; + ECL_INFO("EKF switching to external vision position fusion"); + // turn off other forms of height aiding + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + // reset the position, height and velocity + resetPosition(); + resetVelocity(); + resetHeight(); + } + } + + // external vision yaw aiding selection logic + if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + // check for a exernal vision measurement that has fallen behind the fusion time horizon + if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { + // reset the yaw angle to the value from the observaton quaternion + // get the roll, pitch, yaw estimates from the quaternion states + matrix::Quaternion q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), + _state.quat_nominal(3)); + matrix::Euler euler_init(q_init); + + // get initial yaw from the observation quaternion + extVisionSample ev_newest = _ext_vision_buffer.get_newest(); + matrix::Quaternion q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); + matrix::Euler euler_obs(q_obs); + euler_init(2) = euler_obs(2); + + // save a copy of the quaternion state for later use in calculating the amount of reset change + Quaternion quat_before_reset = _state.quat_nominal; + + // calculate initial quaternion states for the ekf + _state.quat_nominal = Quaternion(euler_init); + + // calculate the amount that the quaternion has changed by + _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= _state_reset_status.quat_change; + _output_buffer.push_to_index(i,output_states); + } + + // capture the reset event + _state_reset_status.quat_counter++; + + // flag the yaw as aligned + _control_status.flags.yaw_align = true; + + // turn on fusion of external vision yaw measurements and disable all magnetoemter fusion + _control_status.flags.ev_yaw = true; + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = false; + _control_status.flags.mag_dec = false; + + ECL_INFO("EKF switching to external vision yaw fusion"); + } + } + + // determine if we should use the height observation + if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; - // reset the position, height and velocity - resetPosition(); - resetVelocity(); - resetHeight(); + _control_status.flags.ev_hgt = true; + _fuse_height = true; + + } + + // determine if we should use the horizontal position observations + if (_control_status.flags.ev_pos) { + _fuse_pos = true; + + // correct position and height for offset relative to IMU + Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); + _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); + _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); + } + + // determine if we should use the yaw observation + if (_control_status.flags.ev_yaw) { + fuseHeading(); } } - - // external vision yaw aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { - // check for a exernal vision measurement that has fallen behind the fusion time horizon - if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { - // reset the yaw angle to the value from the observaton quaternion - // get the roll, pitch, yaw estimates from the quaternion states - matrix::Quaternion q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2), - _state.quat_nominal(3)); - matrix::Euler euler_init(q_init); - - // get initial yaw from the observation quaternion - extVisionSample ev_newest = _ext_vision_buffer.get_newest(); - matrix::Quaternion q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3)); - matrix::Euler euler_obs(q_obs); - euler_init(2) = euler_obs(2); - - // save a copy of the quaternion state for later use in calculating the amount of reset change - Quaternion quat_before_reset = _state.quat_nominal; - - // calculate initial quaternion states for the ekf - _state.quat_nominal = Quaternion(euler_init); - - // calculate the amount that the quaternion has changed by - _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); - - // add the reset amount to the output observer buffered data - outputSample output_states; - unsigned output_length = _output_buffer.get_length(); - for (unsigned i=0; i < output_length; i++) { - output_states = _output_buffer.get_from_index(i); - output_states.quat_nominal *= _state_reset_status.quat_change; - _output_buffer.push_to_index(i,output_states); - } - - // capture the reset event - _state_reset_status.quat_counter++; - - // flag the yaw as aligned - _control_status.flags.yaw_align = true; - - // turn on fusion of external vision yaw measurements and disable all magnetoemter fusion - _control_status.flags.ev_yaw = true; - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = false; - _control_status.flags.mag_dec = false; - - ECL_INFO("EKF switching to external vision yaw fusion"); - } - } - } void Ekf::controlOpticalFlowAiding() { - // optical flow fusion mode selection logic - // to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data - if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align - && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) { - // If the heading is not aligned, reset the yaw and magnetic field states - if (!_control_status.flags.yaw_align) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + // Check for new optical flow data that has fallen behind the fusion time horizon + if (_flow_data_ready) { + + // optical flow fusion mode selection logic + if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user + && !_control_status.flags.opt_flow // we are not yet using flow data + && _control_status.flags.tilt_align // we know our tilt attitude + && (_time_last_imu - _time_last_hagl_fuse) < 5e5) // we have a valid distance to ground estimate + { + + // If the heading is not aligned, reset the yaw and magnetic field states + if (!_control_status.flags.yaw_align) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + + // If the heading is valid, start using optical flow aiding + if (_control_status.flags.yaw_align) { + // set the flag and reset the fusion timeout + _control_status.flags.opt_flow = true; + _time_last_of_fuse = _time_last_imu; + + // if we are not using GPS then the velocity and position states and covariances need to be set + if (!_control_status.flags.gps) { + // constrain height above ground to be above minimum possible + float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + + // calculate absolute distance from focal point to centre of frame assuming a flat earth + float range = heightAboveGndEst / _R_to_earth(2, 2); + + if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + // we should have reliable OF measurements so + // calculate X and Y body relative velocities from OF measurements + Vector3f vel_optflow_body; + vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; + vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; + vel_optflow_body(2) = 0.0f; + + // rotate from body to earth frame + Vector3f vel_optflow_earth; + vel_optflow_earth = _R_to_earth * vel_optflow_body; + + // take x and Y components + _state.vel(0) = vel_optflow_earth(0); + _state.vel(1) = vel_optflow_earth(1); + + } else { + _state.vel(0) = 0.0f; + _state.vel(1) = 0.0f; + } + + // reset the velocity covariance terms + zeroRows(P,4,5); + zeroCols(P,4,5); + + // reset the horizontal velocity variance using the optical flow noise variance + P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); + + if (!_control_status.flags.in_air) { + // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states + _state.pos(0) = 0.0f; + _state.pos(1) = 0.0f; + + // reset the corresponding covariances + // we are by definition at the origin at commencement so variances are also zeroed + zeroRows(P,7,8); + zeroCols(P,7,8); + + // align the output observer to the EKF states + alignOutputFilter(); + + } + } + } + + } else if (!(_params.fusion_mode & MASK_USE_OF)) { + _control_status.flags.opt_flow = false; + } - // If the heading is valid, start using optical flow aiding - if (_control_status.flags.yaw_align) { - // set the flag and reset the fusion timeout - _control_status.flags.opt_flow = true; - _time_last_of_fuse = _time_last_imu; + // handle the case when we are relying on optical flow fusion and lose it + if (_control_status.flags.opt_flow && !_control_status.flags.gps) { + // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something + if ((_time_last_imu - _time_last_of_fuse > 5e6)) { + // Switch to the non-aiding mode, zero the velocity states + // and set the synthetic position to the current estimate + _control_status.flags.opt_flow = false; + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); - // if we are not using GPS then the velocity and position states and covariances need to be set - if (!_control_status.flags.gps) { - // constrain height above ground to be above minimum possible - float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); - - // calculate absolute distance from focal point to centre of frame assuming a flat earth - float range = heightAboveGndEst / _R_to_earth(2, 2); - - if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { - // we should have reliable OF measurements so - // calculate X and Y body relative velocities from OF measurements - Vector3f vel_optflow_body; - vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; - vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; - vel_optflow_body(2) = 0.0f; - - // rotate from body to earth frame - Vector3f vel_optflow_earth; - vel_optflow_earth = _R_to_earth * vel_optflow_body; - - // take x and Y components - _state.vel(0) = vel_optflow_earth(0); - _state.vel(1) = vel_optflow_earth(1); - - } else { - _state.vel(0) = 0.0f; - _state.vel(1) = 0.0f; - } - - // reset the velocity covariance terms - zeroRows(P,4,5); - zeroCols(P,4,5); - - // reset the horizontal velocity variance using the optical flow noise variance - P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); - - if (!_control_status.flags.in_air) { - // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states - _state.pos(0) = 0.0f; - _state.pos(1) = 0.0f; - - // reset the corresponding covariances - // we are by definition at the origin at commencement so variances are also zeroed - zeroRows(P,7,8); - zeroCols(P,7,8); - - // align the output observer to the EKF states - alignOutputFilter(); - } } } - } else if (!(_params.fusion_mode & MASK_USE_OF)) { - _control_status.flags.opt_flow = false; - } + // fuse the data + if (_control_status.flags.opt_flow) { + // Update optical flow bias estimates + calcOptFlowBias(); - // handle the case when we are relying on optical flow fusion and lose it - if (_control_status.flags.opt_flow && !_control_status.flags.gps) { - // We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something - if ((_time_last_imu - _time_last_of_fuse > 5e6)) { - // Switch to the non-aiding mode, zero the velocity states - // and set the synthetic position to the current estimate - _control_status.flags.opt_flow = false; + // Fuse optical flow LOS rate observations into the main filter + fuseOptFlow(); _last_known_posNE(0) = _state.pos(0); _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); + } } } void Ekf::controlGpsAiding() { - // GPS fusion mode selection logic - // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data - if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { - if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised - && (_time_last_imu - _last_gps_fail_us > 5e6)) { - // If the heading is not aligned, reset the yaw and magnetic field states - if (!_control_status.flags.yaw_align) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + // Check for new GPS data that has fallen behind the fusion time horizon + if (_gps_data_ready) { + + // Determine if we should use GPS aiding for velocity and horizontal position + // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently + if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { + if (_control_status.flags.tilt_align && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { + // If the heading is not aligned, reset the yaw and magnetic field states + if (!_control_status.flags.yaw_align) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + + } + + // If the heading is valid start using gps aiding + if (_control_status.flags.yaw_align) { + _control_status.flags.gps = true; + _time_last_gps = _time_last_imu; + + // if we are not already aiding with optical flow, then we need to reset the position and velocity + if (!_control_status.flags.opt_flow) { + if (resetPosition() && resetVelocity()) { + _control_status.flags.gps = true; + + } else { + _control_status.flags.gps = false; + + } + } + if (_control_status.flags.gps) { + ECL_INFO("EKF commencing GPS aiding"); + + } + } } - // If the heading is valid start using gps aiding - if (_control_status.flags.yaw_align) { - _control_status.flags.gps = true; - _time_last_gps = _time_last_imu; + } else if (!(_params.fusion_mode & MASK_USE_GPS)) { + _control_status.flags.gps = false; + + } + + // handle the case when we are relying on GPS fusion and lose it + if (_control_status.flags.gps && !_control_status.flags.opt_flow) { + // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something + if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { + if (_time_last_imu - _time_last_gps > 5e5) { + // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states + // and set the synthetic GPS position to the current estimate + _control_status.flags.gps = false; + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + _state.vel.setZero(); + ECL_WARN("EKF GPS fusion timout - stopping GPS aiding"); + + } else { + // Reset states to the last GPS measurement + resetPosition(); + resetVelocity(); + ECL_WARN("EKF GPS fusion timout - resetting to GPS"); + + // Reset the timeout counters + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; - // if we are not already aiding with optical flow, then we need to reset the position and velocity - if (!_control_status.flags.opt_flow) { - _control_status.flags.gps = resetPosition(); - _control_status.flags.gps = resetVelocity(); } } } - } else if (!(_params.fusion_mode & MASK_USE_GPS)) { - _control_status.flags.gps = false; - } + // 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; - // handle the case when we are relying on GPS fusion and lose it - if (_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something - if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { - if (_time_last_imu - _time_last_gps > 5e5) { - // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states - // and set the synthetic GPS position to the current estimate - _control_status.flags.gps = false; - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); + // correct velocity for offset relative to IMU + Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); + Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); + Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + _gps_sample_delayed.vel -= vel_offset_earth; - } else { - // Reset states to the last GPS measurement - resetPosition(); - resetVelocity(); + // correct position and height for offset relative to IMU + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _gps_sample_delayed.pos(0) -= pos_offset_earth(0); + _gps_sample_delayed.pos(1) -= pos_offset_earth(1); + _gps_sample_delayed.hgt += pos_offset_earth(2); + + } + + // Determine if GPS should be used as the height source + if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && !_gps_hgt_faulty) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + _fuse_height = true; - // Reset the timeout counters - _time_last_pos_fuse = _time_last_imu; - _time_last_vel_fuse = _time_last_imu; - } } } } @@ -336,31 +460,39 @@ void Ekf::controlHeightSensorTimeouts() // set height sensor health _baro_hgt_faulty = true; _gps_hgt_faulty = false; + // declare the GPS height healthy _gps_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF baro hgt timeout - reset to GPS"); + } else if (reset_to_baro){ // set height sensor health _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF baro hgt timeout - reset to baro"); + } else { // we have nothing we can reset to // deny a reset reset_height = false; + } } @@ -370,6 +502,7 @@ void Ekf::controlHeightSensorTimeouts() gpsSample gps_init = _gps_buffer.get_newest(); bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + // check the baro height source for consistency and freshness baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); @@ -389,28 +522,35 @@ void Ekf::controlHeightSensorTimeouts() // set height sensor health _gps_hgt_faulty = true; _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF gps hgt timeout - reset to baro"); + } else if (reset_to_gps) { // set height sensor health _gps_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF gps hgt timeout - reset to GPS"); + } else { // we have nothing to reset to reset_height = false; + } } @@ -419,6 +559,7 @@ void Ekf::controlHeightSensorTimeouts() // check if range finder data is available rangeSample rng_init = _range_buffer.get_newest(); bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); + // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); @@ -433,28 +574,35 @@ void Ekf::controlHeightSensorTimeouts() // set height sensor health _rng_hgt_faulty = true; _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF rng hgt timeout - reset to baro"); + } else if (reset_to_rng) { // set height sensor health _rng_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF rng hgt timeout - reset to rng hgt"); + } else { // we have nothing to reset to reset_height = false; + } } @@ -463,6 +611,7 @@ void Ekf::controlHeightSensorTimeouts() // check if vision data is available extVisionSample ev_init = _ext_vision_buffer.get_newest(); bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL); + // check if baro data is available baroSample baro_init = _baro_buffer.get_newest(); bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); @@ -477,26 +626,32 @@ void Ekf::controlHeightSensorTimeouts() // set height sensor health _rng_hgt_faulty = true; _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = false; + // request a reset reset_height = true; ECL_INFO("EKF ev hgt timeout - reset to baro"); + } else if (reset_to_ev) { // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; _control_status.flags.ev_hgt = true; + // request a reset reset_height = true; ECL_INFO("EKF ev hgt timeout - reset to ev hgt"); + } else { // we have nothing to reset to reset_height = false; + } } @@ -505,58 +660,99 @@ void Ekf::controlHeightSensorTimeouts() resetHeight(); // Reset the timout timer _time_last_hgt_fuse = _time_last_imu; + } } } -void Ekf::controlHeightAiding() +void Ekf::controlBaroAiding() { - // check for height sensor timeouts and reset and change sensor if necessary - controlHeightSensorTimeouts(); + if (_baro_data_ready) { + // determine if we should use the baro as our height source + uint64_t last_baro_time_us = _baro_sample_delayed.time_us; + if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && !_baro_hgt_faulty) { + _control_status.flags.baro_hgt = true; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + _control_status.flags.ev_hgt = false; + _fuse_height = true; - // Control the source of height measurements for the main filter - // do not switch to a sensor if it is unhealthy or the data is stale - if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && - !_baro_hgt_faulty && - (((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) { + } - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference + if (!_control_status.flags.baro_hgt) { + float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); + local_time_step = math::constrain(local_time_step,0.0f,1.0f); + last_baro_time_us = _baro_sample_delayed.time_us; + float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; + _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && - !_gps_hgt_faulty && - (((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) { + } + } +} - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; +void Ekf::controlRangeFinderAiding() +{ + // determine if we should use range finder data for height + if (_range_data_ready) { + // set the height data source to range if requested + if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = true; + _control_status.flags.ev_hgt = false; - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && - !_rng_hgt_faulty && - (((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) { + } - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = true; - _control_status.flags.ev_hgt = false; + // correct the range data for position offset relative to the IMU + Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2); - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) && - (((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) { + // always fuse available range finder data into a terrain height estimator if the estimator has been initialised + if (_terrain_initialised) { + fuseHagl(); - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = true; + } + + // only use range finder as a height observation in the main filter if specifically enabled + if (_control_status.flags.rng_hgt) { + _fuse_height = true; + + } + + } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) { + // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements + // and are on the ground, then synthesise a measurement at the expected on ground value + if (!_control_status.flags.in_air) { + _range_sample_delayed.rng = _params.rng_gnd_clearance; + _range_sample_delayed.time_us = _imu_sample_delayed.time_us; + + } + + _fuse_height = true; + + } +} + +void Ekf::controlAirDataAiding() +{ + // TODO This is just to get the logic inside but we will only start fusion once we tested this again + //if (_tas_data_ready) { + if (false) { + fuseAirspeed(); } - // If we are on ground, store the local position and time to use as a reference for takeoff checks - if (!_control_status.flags.in_air) { - _last_on_ground_posD = _state.pos(2); + // control airspeed fusion - TODO move to a function + // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid + if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) { + _control_status.flags.wind = false; + + } else { + _control_status.flags.wind = true; + } } @@ -567,65 +763,103 @@ void Ekf::controlMagAiding() return; } - // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances - // or the more accurate 3-axis fusion - if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { - // start 3D fusion if in-flight and height has increased sufficiently - // to be away from ground magnetic anomalies - // don't switch back to heading fusion until we are back on the ground - bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; - bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved); + // If we are on ground, store the local position and time to use as a reference + if (!_control_status.flags.in_air) { + _last_on_ground_posD = _state.pos(2); - if (use_3D_fusion && _control_status.flags.tilt_align) { + } + + // checs for new magnetometer data tath has fallen beind the fusion time horizon + if (_mag_data_ready) { + + // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances + // or the more accurate 3-axis fusion + if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { + // start 3D fusion if in-flight and height has increased sufficiently + // to be away from ground magnetic anomalies + // don't switch back to heading fusion until we are back on the ground + bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; + bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved); + + if (use_3D_fusion && _control_status.flags.tilt_align) { + // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states + if (!_control_status.flags.mag_3D) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } + + // use 3D mag fusion when airborne + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = true; + + } else { + // use heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } + + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { + // always use heading fusion + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states if (!_control_status.flags.mag_3D) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // use 3D mag fusion when airborne + // always use 3-axis mag fusion _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = true; } else { - // use heading fusion when on the ground - _control_status.flags.mag_hdg = true; + // do no magnetometer fusion at all + _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; } - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { - // always use heading fusion - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; + // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift + // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time + if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) { + _control_status.flags.mag_dec = true; - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) { - // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states - if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } else { + _control_status.flags.mag_dec = false; } - // always use 3-axis mag fusion - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = true; + // fuse magnetometer data using the selected methods + if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { + fuseMag(); - } else { - // do no magnetometer fusion at all - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = false; - } + if (_control_status.flags.mag_dec) { + fuseDeclination(); + } - // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift - // fusing declination when gps aiding is available is optional, but recommneded to prevent problem if the vehicle is static for extended periods of time - if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) { - _control_status.flags.mag_dec = true; + } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { + // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence + fuseHeading(); + + } else { + // do no fusion at all + } + } +} + +void Ekf::controlVelPosFusion() +{ + // if we aren't doing any aiding, fake GPS 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 (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos + && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + _fuse_pos = true; + _time_last_fake_gps = _time_last_imu; + + } + + // Fuse available NED velocity and position data into the main filter + if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { + fuseVelPosHeight(); + _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; - } else { - _control_status.flags.mag_dec = false; - } - - // if the airspeed measurements have timed out for 10 seconds we declare the wind estimate to be invalid - if (_time_last_imu - _time_last_arsp_fuse > 10e6 || _time_last_arsp_fuse == 0) { - _control_status.flags.wind = false; - } else { - _control_status.flags.wind = true; } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 329fa64938..14a1e26fa0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -39,6 +39,7 @@ * @author Paul Riseborough */ +#include "../ecl.h" #include "ekf.h" #include "mathlib.h" @@ -67,8 +68,13 @@ Ekf::Ekf(): _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), - _fuse_flow(false), - _fuse_hagl_data(false), + _gps_data_ready(false), + _mag_data_ready(false), + _baro_data_ready(false), + _range_data_ready(false), + _flow_data_ready(false), + _ev_data_ready(false), + _tas_data_ready(false), _time_last_fake_gps(0), _time_last_pos_fuse(0), _time_last_vel_fuse(0), @@ -209,166 +215,9 @@ bool Ekf::update() predictHagl(); } - // control logic + // control fusion of observation data controlFusionModes(); - // measurement updates - - // Fuse magnetometer data using the selected fusion method and only if angular alignment is complete - if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) { - fuseMag(); - - if (_control_status.flags.mag_dec) { - fuseDeclination(); - } - - } else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) { - // fusion of an Euler yaw angle from either a 321 or 312 rotation sequence - fuseHeading(); - - } else { - // do no fusion at all - } - } - - // determine if range finder data has fallen behind the fusion time horizon fuse it if we are - // not tilted too much to use it - if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_to_earth(2, 2) > 0.7071f)) { - // correct the range data for position offset relative to the IMU - Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sample_delayed.rng += pos_offset_earth(2) / _R_to_earth(2, 2); - - // if we have range data we always try to estimate terrain height - _fuse_hagl_data = true; - - // only use range finder as a height observation in the main filter if specifically enabled - if (_control_status.flags.rng_hgt) { - _fuse_height = true; - } - - } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) { - // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements - // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air) { - _range_sample_delayed.rng = _params.rng_gnd_clearance; - _range_sample_delayed.time_us = _imu_sample_delayed.time_us; - - } - - _fuse_height = true; - } - - // determine if baro data has fallen behind the fusion time horizon and fuse it in the main filter if enabled - uint64_t last_baro_time_us = _baro_sample_delayed.time_us; - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { - if (_control_status.flags.baro_hgt) { - _fuse_height = true; - - } else { - // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference - float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); - local_time_step = math::constrain(local_time_step,0.0f,1.0f); - last_baro_time_us = _baro_sample_delayed.time_us; - float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; - _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); - } - } - - // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { - // 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; - - // correct velocity for offset relative to IMU - Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); - Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); - Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - _gps_sample_delayed.vel -= vel_offset_earth; - - // correct position and height for offset relative to IMU - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _gps_sample_delayed.pos(0) -= pos_offset_earth(0); - _gps_sample_delayed.pos(1) -= pos_offset_earth(1); - _gps_sample_delayed.hgt += pos_offset_earth(2); - - } - - // only use gps height observation in the main filter if specifically enabled - if (_control_status.flags.gps_hgt) { - _fuse_height = true; - } - - } - - // If we are using external vision aiding and data has fallen behind the fusion time horizon then fuse it - if (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) { - // use external vision posiiton and height observations - if (_control_status.flags.ev_pos) { - _fuse_pos = true; - _fuse_height = true; - - // correct position and height for offset relative to IMU - Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _ev_sample_delayed.posNED(0) -= pos_offset_earth(0); - _ev_sample_delayed.posNED(1) -= pos_offset_earth(1); - _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); - } - // use external vision yaw observation - if (_control_status.flags.ev_yaw) { - fuseHeading(); - } - } - - // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it - if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) - && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 - && (_R_to_earth(2, 2) > 0.7071f)) { - _fuse_flow = true; - } - - // if we aren't doing any aiding, fake GPS 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 (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { - _fuse_pos = true; - _time_last_fake_gps = _time_last_imu; - } - - // fuse available range finder data into a terrain height estimator if it has been initialised - if (_fuse_hagl_data && _terrain_initialised) { - fuseHagl(); - _fuse_hagl_data = false; - } - - // Fuse available NED velocity and position data into the main filter - if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { - fuseVelPosHeight(); - _fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false; - } - - // Update optical flow bias estimates - calcOptFlowBias(); - - // Fuse optical flow LOS rate observations into the main filter - if (_fuse_flow) { - fuseOptFlow(); - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _fuse_flow = false; - } - - // If we are using airspeed measurements and data has fallen behind the fusion time horizon then fuse it - if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) { - fuseAirspeed(); - } } // the output observer always runs @@ -546,11 +395,16 @@ bool Ekf::initialiseFilter(void) baroSample baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt; _state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance); + ECL_INFO("EKF using range finder height - commencing alignment"); } else if (_control_status.flags.ev_hgt) { // if we are using external vision data for height, then the vertical position state needs to be reset // because the initialisation position is not the zero datum resetHeight(); + ECL_INFO("EKF using vision height - commencing alignment"); + + } else if (_control_status.flags.baro_hgt){ + ECL_INFO("EKF using pressure height - commencing alignment"); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 84181828ef..b27b6e197b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -187,8 +187,15 @@ private: bool _fuse_pos; // gps position data should be fused bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - bool _fuse_flow; // flow measurement should be fused - bool _fuse_hagl_data; // if true then range data will be fused to estimate terrain height + + // booleans true when fresh sensor data is available at the fusion time horizon + bool _gps_data_ready; + bool _mag_data_ready; + bool _baro_data_ready; + bool _range_data_ready; + bool _flow_data_ready; + bool _ev_data_ready; + bool _tas_data_ready; uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode @@ -378,12 +385,21 @@ private: // control fusion of GPS observations void controlGpsAiding(); - // control fusion of height position observations - void controlHeightAiding(); - // control fusion of magnetometer observations void controlMagAiding(); + // control fusion of range finder observations + void controlRangeFinderAiding(); + + // control fusion of air data observations + void controlAirDataAiding(); + + // control fusion of pressure altitude observations + void controlBaroAiding(); + + // control fusion of velocity and position observations + void controlVelPosFusion(); + // control for height sensor timeouts, sensor changes and state resets void controlHeightSensorTimeouts(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4336ee2ab4..4734ba2a3d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -56,21 +56,15 @@ bool Ekf::resetVelocity() // reset EKF states if (_control_status.flags.gps) { - // if we have a valid GPS measurement use it to initialise velocity states - gpsSample gps_newest = _gps_buffer.get_newest(); + // this reset is only called if we have new gps data at the fusion time horizon + _state.vel = _gps_sample_delayed.vel; - if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) { - _state.vel = gps_newest.vel; - - } else { - // XXX use the value of the last known velocity - return false; - } } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { _state.vel.setZero(); } else { return false; + } // calculate the change in velocity and apply to the output predictor state history @@ -81,6 +75,7 @@ bool Ekf::resetVelocity() output_states = _output_buffer.get_from_index(index); output_states.vel += velocity_change; _output_buffer.push_to_index(index,output_states); + } // capture the reset event @@ -103,45 +98,21 @@ bool Ekf::resetPosition() posNE_before_reset(1) = _state.pos(1); if (_control_status.flags.gps) { - // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay - gpsSample gps_newest = _gps_buffer.get_newest(); + // this reset is only called if we have new gps data at the fusion time horizon + _state.pos(0) = _gps_sample_delayed.pos(0); + _state.pos(1) = _gps_sample_delayed.pos(1); + return true; - float time_delay = 1e-6f * (float)(_imu_sample_delayed.time_us - gps_newest.time_us); - float max_time_delay = 1e-6f * (float)GPS_MAX_INTERVAL; - - if (time_delay < max_time_delay) { - _state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay; - _state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay; - return true; - - } else { - // XXX use the value of the last known position - return false; - } } else if (_control_status.flags.opt_flow) { _state.pos(0) = 0.0f; _state.pos(1) = 0.0f; return true; - } else if (_control_status.flags.ev_pos) { - // if we have fresh data, reset the position to the measurement - extVisionSample ev_newest = _ext_vision_buffer.get_newest(); - if (_time_last_imu - ev_newest.time_us < 2*EV_MAX_INTERVAL) { - // use the most recent data if it's time offset from the fusion time horizon is smaller - int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; - int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; - if (abs(dt_newest) < abs(dt_delayed)) { - _state.pos(0) = ev_newest.posNED(0); - _state.pos(1) = ev_newest.posNED(1); - } else { - _state.pos(0) = _ev_sample_delayed.posNED(0); - _state.pos(1) = _ev_sample_delayed.posNED(1); - } - return true; - } else { - // XXX use the value of the last known position - return false; - } + } else if (_control_status.flags.ev_pos) { + // this reset is only called if we have new ev data at the fusion time horizon + _state.pos(0) = _ev_sample_delayed.posNED(0); + _state.pos(1) = _ev_sample_delayed.posNED(1); + return true; } else { return false; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index e6b9bc142f..1f7bc293a8 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -522,30 +522,28 @@ void Ekf::calcOptFlowBias() // if accumulation time differences are not excessive and accumulation time is adequate // compare the optical flow and and navigation rate data and calculate a bias error - if (_fuse_flow) { - if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) - && (_flow_sample_delayed.dt > 0.01f)) { - // calculate a reference angular rate - Vector3f reference_body_rate; - reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); + if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f) + && (_flow_sample_delayed.dt > 0.01f)) { + // calculate a reference angular rate + Vector3f reference_body_rate; + reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); - // calculate the optical flow sensor measured body rate - Vector3f of_body_rate; - of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); + // calculate the optical flow sensor measured body rate + Vector3f of_body_rate; + of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); - // calculate the bias estimate using a combined LPF and spike filter - _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), - -0.1f, 0.1f); - _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), - -0.1f, 0.1f); - _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), - -0.1f, 0.1f); - } - - // reset the accumulators - _imu_del_ang_of.setZero(); - _delta_time_of = 0.0f; + // calculate the bias estimate using a combined LPF and spike filter + _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), + -0.1f, 0.1f); + _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), + -0.1f, 0.1f); + _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), + -0.1f, 0.1f); } + + // reset the accumulators + _imu_del_ang_of.setZero(); + _delta_time_of = 0.0f; } // calculate the measurement variance for the optical flow sensor (rad/sec)^2 From 5f9752cac84595b8c30b9d4e437b423f2c974a22 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 7 Jun 2016 18:44:47 +1000 Subject: [PATCH 2/5] EKF: Improve function names --- EKF/control.cpp | 28 ++++++++++++++-------------- EKF/ekf.h | 14 +++++++------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 1d1c7c9120..d90169c943 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -82,13 +82,13 @@ void Ekf::controlFusionModes() controlHeightSensorTimeouts(); // control use of observations for aiding - controlMagAiding(); - controlExternalVisionAiding(); - controlOpticalFlowAiding(); - controlGpsAiding(); - controlBaroAiding(); - controlRangeFinderAiding(); - controlAirDataAiding(); + controlMagFusion(); + controlExternalVisionFusion(); + controlOpticalFlowFusion(); + controlGpsFusion(); + controlBaroFusion(); + controlRangeFinderFusion(); + controlAirDataFusion(); // for efficiency, fusion of direct state observations for position ad velocity is performed sequentially // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) @@ -96,7 +96,7 @@ void Ekf::controlFusionModes() } -void Ekf::controlExternalVisionAiding() +void Ekf::controlExternalVisionFusion() { // Check for new exernal vision data if (_ev_data_ready) { @@ -198,7 +198,7 @@ void Ekf::controlExternalVisionAiding() } } -void Ekf::controlOpticalFlowAiding() +void Ekf::controlOpticalFlowFusion() { // Check for new optical flow data that has fallen behind the fusion time horizon if (_flow_data_ready) { @@ -307,7 +307,7 @@ void Ekf::controlOpticalFlowAiding() } } -void Ekf::controlGpsAiding() +void Ekf::controlGpsFusion() { // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { @@ -666,7 +666,7 @@ void Ekf::controlHeightSensorTimeouts() } } -void Ekf::controlBaroAiding() +void Ekf::controlBaroFusion() { if (_baro_data_ready) { // determine if we should use the baro as our height source @@ -692,7 +692,7 @@ void Ekf::controlBaroAiding() } } -void Ekf::controlRangeFinderAiding() +void Ekf::controlRangeFinderFusion() { // determine if we should use range finder data for height if (_range_data_ready) { @@ -736,7 +736,7 @@ void Ekf::controlRangeFinderAiding() } } -void Ekf::controlAirDataAiding() +void Ekf::controlAirDataFusion() { // TODO This is just to get the logic inside but we will only start fusion once we tested this again //if (_tas_data_ready) { @@ -756,7 +756,7 @@ void Ekf::controlAirDataAiding() } } -void Ekf::controlMagAiding() +void Ekf::controlMagFusion() { // If we are using external vision data for heading then no magnetometer fusion is used if (_control_status.flags.ev_yaw) { diff --git a/EKF/ekf.h b/EKF/ekf.h index b27b6e197b..3412d1cf4f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -377,25 +377,25 @@ private: void controlFusionModes(); // control fusion of external vision observations - void controlExternalVisionAiding(); + void controlExternalVisionFusion(); // control fusion of optical flow observtions - void controlOpticalFlowAiding(); + void controlOpticalFlowFusion(); // control fusion of GPS observations - void controlGpsAiding(); + void controlGpsFusion(); // control fusion of magnetometer observations - void controlMagAiding(); + void controlMagFusion(); // control fusion of range finder observations - void controlRangeFinderAiding(); + void controlRangeFinderFusion(); // control fusion of air data observations - void controlAirDataAiding(); + void controlAirDataFusion(); // control fusion of pressure altitude observations - void controlBaroAiding(); + void controlBaroFusion(); // control fusion of velocity and position observations void controlVelPosFusion(); From 573f252b73567c97df6357b5df3dc201f7f9e848 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 7 Jun 2016 18:50:35 +1000 Subject: [PATCH 3/5] EKF: fix bug preventing GPS use as a backup height source --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index d90169c943..7897722412 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -398,7 +398,7 @@ void Ekf::controlGpsFusion() } // Determine if GPS should be used as the height source - if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) && !_gps_hgt_faulty) { + if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS) || _control_status.flags.gps) && !_gps_hgt_faulty) { _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; From eb2afc522d5f5ffe3b4964d451a6d1ff0a7f9366 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 7 Jun 2016 18:51:55 +1000 Subject: [PATCH 4/5] EKF: fix bug preventing use of baro as a backup height source --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7897722412..fd3cc7d8bd 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -671,7 +671,7 @@ void Ekf::controlBaroFusion() if (_baro_data_ready) { // determine if we should use the baro as our height source uint64_t last_baro_time_us = _baro_sample_delayed.time_us; - if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) && !_baro_hgt_faulty) { + if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) { _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; From f5e9ea16268c35411dac093560ebe07aba046948 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Jun 2016 17:03:44 +1000 Subject: [PATCH 5/5] EKF: Prevent potential divide by zero --- EKF/ekf_helper.cpp | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4734ba2a3d..666e60f54e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -815,21 +815,27 @@ Vector3f Ekf::calcRotVecVariances() float t3 = acos(q0); float t4 = -t2+1.0f; float t5 = t2-1.0f; - float t6 = 1.0f/t5; - float t7 = q1*t6*2.0f; - float t8 = 1.0f/powf(t4,1.5f); - float t9 = q0*q1*t3*t8*2.0f; - float t10 = t7+t9; - float t11 = 1.0f/sqrtf(t4); - float t12 = q2*t6*2.0f; - float t13 = q0*q2*t3*t8*2.0f; - float t14 = t12+t13; - float t15 = q3*t6*2.0f; - float t16 = q0*q3*t3*t8*2.0f; - float t17 = t15+t16; - rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; - rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; - rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + if ((t4 > 1e-9f) && (t5 < -1e-9f)) { + float t6 = 1.0f/t5; + float t7 = q1*t6*2.0f; + float t8 = 1.0f/powf(t4,1.5f); + float t9 = q0*q1*t3*t8*2.0f; + float t10 = t7+t9; + float t11 = 1.0f/sqrtf(t4); + float t12 = q2*t6*2.0f; + float t13 = q0*q2*t3*t8*2.0f; + float t14 = t12+t13; + float t15 = q3*t6*2.0f; + float t16 = q0*q3*t3*t8*2.0f; + float t17 = t15+t16; + rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; + rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; + rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + } else { + rot_var_vec(0) = 4.0f * P[1][1]; + rot_var_vec(1) = 4.0f * P[2][2]; + rot_var_vec(2) = 4.0f * P[3][3]; + } return rot_var_vec; }