From 3a0fcd03d722b56bcdeeff7789d82a7b991abbd7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 19:41:40 +1100 Subject: [PATCH 01/19] EKF: Add interfaces and variables to use ext vision data --- EKF/common.h | 21 +++++++++++++++++++++ EKF/estimator_interface.cpp | 29 +++++++++++++++++++++++++++++ EKF/estimator_interface.h | 5 +++++ 3 files changed, 55 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index f8840b5267..f0b40bd611 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -71,6 +71,13 @@ struct flow_message { uint32_t dt; // integration time of flow samples }; +struct ext_vision_message { + Vector3f posNED; // measured NED position relative to the local origin (m) + Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + float posErr; // 1-Sigma spherical position accuracy (m) + float angErr; // 1-Sigma angular error (rad) +}; + struct outputSample { Quaternion quat_nominal; // nominal quaternion describing vehicle attitude Vector3f vel; // NED velocity estimate in earth frame in m/s @@ -126,6 +133,14 @@ struct flowSample { uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; +struct extVisionSample { + Vector3f posNED; // measured NED position relative to the local origin (m) + Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + float posErr; // 1-Sigma spherical position accuracy (m) + float angErr; // 1-Sigma angular error (rad) + uint64_t time_us; // timestamp of the measurement in microseconds +}; + // Integer definitions for vdist_sensor_type #define VDIST_SENSOR_BARO 0 // Use baro height #define VDIST_SENSOR_GPS 1 // Use GPS height @@ -140,6 +155,8 @@ struct flowSample { #define MASK_USE_GPS (1<<0) // set to true to use GPS data #define MASK_USE_OF (1<<1) // set to true to use optical flow data #define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias +#define MASK_USE_EVPOS (1<<3) // set to true to use external vision NED position data +#define MASK_USE_EVYAW (1<<4) // set to true to use exernal vision quaternion data for yaw // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic @@ -163,6 +180,7 @@ struct parameters { float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec) float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval float range_delay_ms; // range finder measurement delay relative to the IMU (msec) + float ev_delay_ms; // off-board vision measurement delay relative to the IMU (msec) // input noise float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec) @@ -247,6 +265,7 @@ struct parameters { airspeed_delay_ms = 200.0f; flow_delay_ms = 5.0f; range_delay_ms = 5.0f; + ev_delay_ms = 100.0f; // input noise gyro_noise = 1.5e-2f; @@ -383,6 +402,8 @@ union filter_control_status_u { uint16_t baro_hgt : 1; // 9 - true when baro height is being fused as a primary height reference uint16_t rng_hgt : 1; // 10 - true when range finder height is being fused as a primary height reference uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference + uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused + uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused } flags; uint16_t value; }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 65bfa295c7..3fa6858e8f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -65,6 +65,7 @@ EstimatorInterface::EstimatorInterface(): _time_last_baro(0), _time_last_range(0), _time_last_airspeed(0), + _time_last_ext_vision(0), _mag_declination_gps(0.0f), _mag_declination_to_save_deg(0.0f) { @@ -284,6 +285,30 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl } } +// set attitude and position data derived from an external vision system +void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata) +{ + if (!_initialised) { + return; + } + + // if data passes checks, push to buffer + if (time_usec > _time_last_ext_vision) { + extVisionSample ev_sample_new; + // calculate the system time-stamp for the mid point of the integration period + ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; + // copy required data + ev_sample_new.angErr = evdata->angErr; + ev_sample_new.posErr = evdata->posErr; + ev_sample_new.quat = evdata->quat; + ev_sample_new.posNED = evdata->posNED; + // record time for comparison next measurement + _time_last_ext_vision = time_usec; + // push to buffer + _ext_vision_buffer.push(ev_sample_new); + } +} + bool EstimatorInterface::initialise_interface(uint64_t timestamp) { @@ -294,6 +319,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _range_buffer.allocate(OBS_BUFFER_LENGTH) && _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(OBS_BUFFER_LENGTH) && + _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { printf("EKF buffer allocation failed!"); unallocate_buffers(); @@ -314,6 +340,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.push(airspeed_sample_init); flowSample flow_sample_init = {}; _flow_buffer.push(flow_sample_init); + extVisionSample ext_vision_sample_init = {}; + _ext_vision_buffer.push(ext_vision_sample_init); } // zero the data in the imu data and output observer state buffers @@ -357,6 +385,7 @@ void EstimatorInterface::unallocate_buffers() _range_buffer.unallocate(); _airspeed_buffer.unallocate(); _flow_buffer.unallocate(); + _ext_vision_buffer.unallocate(); _output_buffer.unallocate(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index bd2ecf40a0..1789323c56 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -145,6 +145,9 @@ public: // set optical flow data void setOpticalFlowData(uint64_t time_usec, flow_message *flow); + // set external vision position and attitude data + void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); + // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() {return &_params;} @@ -277,6 +280,7 @@ protected: RingBuffer _range_buffer; RingBuffer _airspeed_buffer; RingBuffer _flow_buffer; + RingBuffer _ext_vision_buffer; RingBuffer _output_buffer; uint64_t _time_last_imu; // timestamp of last imu sample in microseconds @@ -285,6 +289,7 @@ protected: uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds + uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds uint64_t _time_last_optflow; fault_status_u _fault_status; From 81469d66219cc08944d8ecb7b86020920170c0ae Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 21:00:52 +1100 Subject: [PATCH 02/19] EKF: Add position, height and velocity reset for EV aiding --- EKF/common.h | 2 + EKF/control.cpp | 19 +++++++++ EKF/ekf.cpp | 8 +++- EKF/ekf_helper.cpp | 87 ++++++++++++++++++++++++++++++--------- EKF/estimator_interface.h | 1 + 5 files changed, 96 insertions(+), 21 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index f0b40bd611..9ea14ad859 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -145,6 +145,7 @@ struct extVisionSample { #define VDIST_SENSOR_BARO 0 // Use baro height #define VDIST_SENSOR_GPS 1 // Use GPS height #define VDIST_SENSOR_RANGE 2 // Use range finder height +#define VDIST_SENSOR_EV 3 // USe external vision // Bit locations for mag_declination_source #define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value @@ -167,6 +168,7 @@ struct extVisionSample { #define GPS_MAX_INTERVAL 5e5 #define BARO_MAX_INTERVAL 2e5 #define RNG_MAX_INTERVAL 2e5 +#define EV_MAX_INTERVAL 2e5 struct parameters { // measurement source control diff --git a/EKF/control.cpp b/EKF/control.cpp index 2830d696dc..8a151fc178 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -56,6 +56,25 @@ void Ekf::controlFusionModes() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } + // 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 (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) { + // turn on use of external vision measurements for position + _control_status.flags.ev_pos = true; + // 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 + && (_time_last_imu - _time_last_ext_vision) < 5e5) { + //TODO + } + // 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 diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 5578f38ec6..9e7862ff83 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -390,7 +390,11 @@ bool Ekf::initialiseFilter(void) // set the default height source from the adjustable parameter if (_hgt_counter == 0) { - _primary_hgt_source = _params.vdist_sensor_type; + if (_params.fusion_mode & MASK_USE_EVPOS) { + _primary_hgt_source = VDIST_SENSOR_EV; + } else { + _primary_hgt_source = _params.vdist_sensor_type; + } } if (_primary_hgt_source == VDIST_SENSOR_RANGE) { @@ -409,7 +413,7 @@ bool Ekf::initialiseFilter(void) } } - } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) { + } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS || _primary_hgt_source == VDIST_SENSOR_EV) { // if the user parameter specifies use of GPS for height we use baro height initially and switch to GPS // later when it passes checks. if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index d1d6d85c63..0e3ac8b008 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -51,15 +51,22 @@ // gps measurement then use for velocity initialisation bool Ekf::resetVelocity() { - // if we have a valid GPS measurement use it to initialise velocity states - gpsSample gps_newest = _gps_buffer.get_newest(); + 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(); - if (_time_last_imu - gps_newest.time_us < 400000) { - _state.vel = gps_newest.vel; + if (_time_last_imu - gps_newest.time_us < 2*GPS_MAX_INTERVAL) { + _state.vel = gps_newest.vel; + return true; + + } 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(); return true; - } else { - // XXX use the value of the last known velocity return false; } } @@ -68,18 +75,48 @@ bool Ekf::resetVelocity() // gps measurement then use for position initialisation bool Ekf::resetPosition() { - // 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(); + 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(); - float time_delay = 1e-6f * (float)(_time_last_imu - gps_newest.time_us); + 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 < 0.4f) { - _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; + 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 { - // XXX use the value of the last known position return false; } } @@ -116,13 +153,14 @@ void Ekf::resetHeight() vert_pos_reset = true; + // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else { // TODO: reset to last known range based estimate } - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); } else if (_control_status.flags.baro_hgt) { // initialize vertical position with newest baro measurement @@ -158,13 +196,24 @@ void Ekf::resetHeight() vert_pos_reset = true; + // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else { // TODO: reset to last known gps based estimate } - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else if (_control_status.flags.ev_pos) { + // initialize vertical position with newest measurement + extVisionSample ev_newest = _ext_vision_buffer.get_newest(); + + if (_time_last_imu - ev_newest.time_us < 2 * EV_MAX_INTERVAL) { + _state.pos(2) = ev_newest.posNED(2); + + } else { + // TODO: reset to last known baro based estimate + } } // reset the vertical velocity covariance values diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1789323c56..e4c09b125f 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -248,6 +248,7 @@ protected: rangeSample _range_sample_delayed; airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; + extVisionSample _ev_sample_delayed; outputSample _output_sample_delayed; // filter output on the delayed time horizon outputSample _output_new; // filter output on the non-delayed time horizon From 25f1d1d76693692fedeaaa3dc5cba88252b2d164 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 21:42:22 +1100 Subject: [PATCH 03/19] EKF: Add fusion of external vision 3D pos data --- EKF/ekf.cpp | 11 +++++++++-- EKF/vel_pos_fusion.cpp | 30 +++++++++++++++++++++++------- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 9e7862ff83..4bac4bdd63 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -303,6 +303,15 @@ bool Ekf::update() } + // 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; + } + } + // 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 @@ -315,8 +324,6 @@ bool Ekf::update() if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { _fuse_pos = true; - _gps_sample_delayed.pos(0) = _last_known_posNE(0); - _gps_sample_delayed.pos(1) = _last_known_posNE(1); _time_last_fake_gps = _time_last_imu; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 433b6eeb1a..c71e014b7d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -83,24 +83,31 @@ void Ekf::fuseVelPosHeight() if (_fuse_pos) { fuse_map[3] = fuse_map[4] = true; - // horizontal position innovations - _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); - _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); - // observation variance - user parameter defined - // if we are in flight and not using GPS, then use a specific parameter - if (!_control_status.flags.gps) { + // Calculate innovations and observation variance depending on type of observations + // being used + if (!_control_status.flags.gps && !_control_status.flags.ev_pos) { + // No observations - use a static position to constrain drift if (_control_status.flags.in_air) { R[3] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise); } else { R[3] = _params.gps_pos_noise; } + _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); + _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); - } else { + } else if (_control_status.flags.gps) { float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); + _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); + _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); + + } else if (_control_status.flags.ev_pos) { + R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); + _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); + _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); } @@ -145,6 +152,15 @@ void Ekf::fuseVelPosHeight() R[5] = R[5] * R[5]; // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); + } else if (_control_status.flags.ev_pos) { + fuse_map[5] = true; + // calculate the innovation assuming the external vision observaton is in local NED frame + _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2); + // observation variance - defined externally + R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f); + R[5] = R[5] * R[5]; + // innovation gate size + gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); } } From 37a09c61bcbfcb6ae30fac5fedf71bfe2f408a83 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 21:54:12 +1100 Subject: [PATCH 04/19] EKF: Don't use delayed data to start EV aiding --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a151fc178..557afd0633 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -59,7 +59,7 @@ void Ekf::controlFusionModes() // 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 (_ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed)) { + if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position _control_status.flags.ev_pos = true; // reset the position, height and velocity From e917d6c7f2ca66f6ad68e98afbe914130ebc5dc1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 20 Mar 2016 23:22:53 +1100 Subject: [PATCH 05/19] EKF: Add fusion of external yaw data --- EKF/common.h | 1 + EKF/control.cpp | 27 ++++++++++++++++++++++++--- EKF/ekf.cpp | 4 ++++ EKF/mag_fusion.cpp | 38 +++++++++++++++++++++++++++++++------- 4 files changed, 60 insertions(+), 10 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 9ea14ad859..53c89f3c0f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -163,6 +163,7 @@ struct extVisionSample { #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions +#define USE_EV_YAW 3 // Fuse yaw angle from external vision system // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 diff --git a/EKF/control.cpp b/EKF/control.cpp index 557afd0633..f16ab8da63 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -70,9 +70,30 @@ void Ekf::controlFusionModes() } // external vision yaw aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align - && (_time_last_imu - _time_last_ext_vision) < 5e5) { - //TODO + if ((_params.mag_fusion_type == USE_EV_YAW) && !_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); + + // calculate initial quaternion states for the ekf + _state.quat_nominal = Quaternion(euler_init); + + // flag the yaw as aligned + _control_status.flags.yaw_align = true; + + // turn on fusion of external vision yaw measurements + _control_status.flags.ev_yaw = true; + } } // optical flow fusion mode selection logic diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4bac4bdd63..49df81540f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -310,6 +310,10 @@ bool Ekf::update() _fuse_pos = true; _fuse_height = true; } + // 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 diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 88c133deae..e55062dbd9 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -385,6 +385,7 @@ void Ekf::fuseHeading() float predicted_hdg; float H_YAW[4]; matrix::Vector3f mag_earth_pred; + float measured_hdg; // determine if a 321 or 312 Euler sequence is best if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { @@ -426,8 +427,21 @@ void Ekf::fuseHeading() euler321(2) = 0.0f; matrix::Dcm R_to_earth(euler321); - // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // calculate the observed yaw angle + if (_control_status.flags.mag_hdg) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + measured_hdg = atan2f(R_to_earth(1, 0) , R_to_earth(0, 0)); + } else { + // there is no yaw observation + return; + } } else { // calculate observaton jacobian when we are observing a rotation in a 312 sequence @@ -491,8 +505,21 @@ void Ekf::fuseHeading() R_to_earth(2, 0) = -s2 * c1; R_to_earth(2, 1) = s1; - // rotate the magnetometer measurements into earth frame using a zero yaw angle - mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // calculate the observed yaw angle + if (_control_status.flags.mag_hdg) { + // rotate the magnetometer measurements into earth frame using a zero yaw angle + mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; + // the angle of the projection onto the horizontal gives the yaw angle + measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + measured_hdg = atan2f(-R_to_earth(0, 1) , R_to_earth(1, 1)); + } else { + // there is no yaw observation + return; + } } // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero @@ -553,9 +580,6 @@ void Ekf::fuseHeading() } } - // Use the difference between the horizontal projection of the mag field and declination to give the measured heading - float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; - // wrap the heading to the interval between +-pi measured_hdg = matrix::wrap_pi(measured_hdg); From ff8f03b5ddffebd94ab7b66b5f15727440c8da5e Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 13:51:50 +0200 Subject: [PATCH 06/19] Added compensation for VI sensor offset. Check sign. --- EKF/common.h | 2 ++ EKF/ekf.cpp | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index 53c89f3c0f..f589b6277f 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -249,6 +249,7 @@ struct parameters { Vector3f gps_pos_body; // xyz position of the GPS antenna in body frame (m) Vector3f rng_pos_body; // xyz position of range sensor in body frame (m) Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m) + Vector3f visn_pos_body; // xyz position of VI-sensor focal point in body frame (m) // output complementary filter tuning float vel_Tau; // velocity state correction time constant (1/sec) @@ -333,6 +334,7 @@ struct parameters { gps_pos_body = {}; rng_pos_body = {}; flow_pos_body = {}; + visn_pos_body = {}; // output complementary filter tuning time constants vel_Tau = 0.5f; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 49df81540f..d7f9aa2e6e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -309,6 +309,13 @@ bool Ekf::update() 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.visn_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); // + or - ? } // use external vision yaw observation if (_control_status.flags.ev_yaw) { From b681c9a5d0024898b7d61844eaa2ed48f2f7e44a Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 14:24:45 +0200 Subject: [PATCH 07/19] Added external vision noise parameters etc and position offset --- EKF/common.h | 10 ++++++++-- EKF/ekf.cpp | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index f589b6277f..35d2966704 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -226,6 +226,11 @@ struct parameters { float range_innov_gate; // range finder fusion innovation consistency gate size (STD) float rng_gnd_clearance; // minimum valid value for range when on ground (m) + // vision position fusion + float ev_noise; // observation noise for vision sensor estimates (m) + float ev_innov_gate; // vision estimator fusion innovation consistency gate size (STD) + float ev_gnd_clearance; // minimum valid value for vision based height estimate when on ground (m) + // optical flow fusion float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) @@ -249,7 +254,7 @@ struct parameters { Vector3f gps_pos_body; // xyz position of the GPS antenna in body frame (m) Vector3f rng_pos_body; // xyz position of range sensor in body frame (m) Vector3f flow_pos_body; // xyz position of range sensor focal point in body frame (m) - Vector3f visn_pos_body; // xyz position of VI-sensor focal point in body frame (m) + Vector3f ev_pos_body; // xyz position of VI-sensor focal point in body frame (m) // output complementary filter tuning float vel_Tau; // velocity state correction time constant (1/sec) @@ -334,11 +339,12 @@ struct parameters { gps_pos_body = {}; rng_pos_body = {}; flow_pos_body = {}; - visn_pos_body = {}; + ev_pos_body = {}; // output complementary filter tuning time constants vel_Tau = 0.5f; pos_Tau = 0.25f; + } }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d7f9aa2e6e..3e93b58a3e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -311,7 +311,7 @@ bool Ekf::update() _fuse_height = true; // correct position and height for offset relative to IMU - Vector3f pos_offset_body = _params.visn_pos_body - _params.imu_pos_body; + 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); From 6d20a426e0b82cd42ec2249d4f3d57b17780dd54 Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 16:36:22 +0200 Subject: [PATCH 08/19] Reset time_last_ext_vision --- EKF/estimator_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3fa6858e8f..7ebd465abe 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -371,8 +371,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _time_last_range = 0; _time_last_airspeed = 0; _time_last_optflow = 0; - memset(&_fault_status.flags, 0, sizeof(_fault_status.flags)); + _time_last_ext_vision = 0; return true; } From d3bad9fdb03199c03d166898cfa9b28a0f345021 Mon Sep 17 00:00:00 2001 From: devbharat Date: Tue, 19 Apr 2016 22:25:38 +0200 Subject: [PATCH 09/19] Correct height fusion flag when using sensor other than baro --- EKF/ekf.cpp | 2 ++ EKF/vel_pos_fusion.cpp | 4 +++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 3e93b58a3e..cc36639b26 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -406,6 +406,8 @@ bool Ekf::initialiseFilter(void) } } + // warnx("_params.fusion_mode & MASK_USE_EVPOS %d", (int)(_params.fusion_mode & MASK_USE_EVPOS)); + // warnx("_primary_hgt_source == VDIST_SENSOR_EV %d", (int)(_primary_hgt_source == VDIST_SENSOR_EV)); // set the default height source from the adjustable parameter if (_hgt_counter == 0) { if (_params.fusion_mode & MASK_USE_EVPOS) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c71e014b7d..2b8d87d71a 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -105,6 +105,7 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); } else if (_control_status.flags.ev_pos) { + // warnx("Fusing EV pos"); R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); @@ -119,7 +120,7 @@ void Ekf::fuseVelPosHeight() } if (_fuse_height) { - if (_control_status.flags.baro_hgt) { + if (_control_status.flags.baro_hgt && !_control_status.flags.gps && !_control_status.flags.ev_pos && !(_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f))) { fuse_map[5] = true; // vertical position innovation - baro measurement has opposite sign to earth z axis _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; @@ -153,6 +154,7 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); } else if (_control_status.flags.ev_pos) { + // warnx("fusing ext_visn_hight"); fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2); From d16f413b55304934855c2848091f246c9952d37a Mon Sep 17 00:00:00 2001 From: devbharat Date: Wed, 20 Apr 2016 09:31:26 +0200 Subject: [PATCH 10/19] Hack to fix external vision pos offset compensation --- EKF/ekf.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index cc36639b26..f0e01dc3bc 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -313,9 +313,10 @@ bool Ekf::update() // 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); // + or - ? + // warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); + _ev_sample_delayed.posNED(0) += pos_offset_earth(1); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! + _ev_sample_delayed.posNED(1) -= pos_offset_earth(0); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! + _ev_sample_delayed.posNED(2) -= pos_offset_earth(2); } // use external vision yaw observation if (_control_status.flags.ev_yaw) { From cf1df5861a1af374dec8d27c3bf0848286ca0aa6 Mon Sep 17 00:00:00 2001 From: devbharat Date: Wed, 20 Apr 2016 11:43:11 +0200 Subject: [PATCH 11/19] Undid hack in ecl. Handled yaw missalignment on the vision side. --- EKF/ekf.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f0e01dc3bc..d6072e1e5b 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -313,9 +313,9 @@ bool Ekf::update() // 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; - // warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); - _ev_sample_delayed.posNED(0) += pos_offset_earth(1); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! - _ev_sample_delayed.posNED(1) -= pos_offset_earth(0); // XXX FIX IT Hack After "_R_to_earth * pos_offset_body" x and y offset seem to be switched! + warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); + _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 From ac9b7a3df63bfcbf25d5747b429e3b477566c8cf Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 21 Apr 2016 08:51:59 +1000 Subject: [PATCH 12/19] EKF: Ensure use of EV aiding inhibits use of other height sources --- EKF/control.cpp | 12 ++++++++++-- EKF/vel_pos_fusion.cpp | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index f16ab8da63..d49b2d3bdc 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -60,8 +60,12 @@ void Ekf::controlFusionModes() 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 + // turn on use of external vision measurements for position and height _control_status.flags.ev_pos = true; + // 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(); @@ -456,7 +460,11 @@ void Ekf::controlFusionModes() } // Control the soure of height measurements for the main filter - if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { + if (_control_status.flags.ev_pos) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 2b8d87d71a..607a9c901e 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight() } if (_fuse_height) { - if (_control_status.flags.baro_hgt && !_control_status.flags.gps && !_control_status.flags.ev_pos && !(_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f))) { + if (_control_status.flags.baro_hgt) { fuse_map[5] = true; // vertical position innovation - baro measurement has opposite sign to earth z axis _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; From 349c7313755581be99ac188b12c2a8c49724f79c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 21 Apr 2016 08:55:53 +1000 Subject: [PATCH 13/19] EKF: remove PX4 dependant text output --- EKF/ekf.cpp | 3 --- EKF/vel_pos_fusion.cpp | 2 -- 2 files changed, 5 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d6072e1e5b..aaf25c9d7e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -313,7 +313,6 @@ bool Ekf::update() // 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; - warnx("pos_offset_earth %lf %lf %lf", (double)pos_offset_earth(0)*100, (double)pos_offset_earth(1)*100, (double)pos_offset_earth(2)*100); _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); @@ -407,8 +406,6 @@ bool Ekf::initialiseFilter(void) } } - // warnx("_params.fusion_mode & MASK_USE_EVPOS %d", (int)(_params.fusion_mode & MASK_USE_EVPOS)); - // warnx("_primary_hgt_source == VDIST_SENSOR_EV %d", (int)(_primary_hgt_source == VDIST_SENSOR_EV)); // set the default height source from the adjustable parameter if (_hgt_counter == 0) { if (_params.fusion_mode & MASK_USE_EVPOS) { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 607a9c901e..c71e014b7d 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -105,7 +105,6 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); } else if (_control_status.flags.ev_pos) { - // warnx("Fusing EV pos"); R[3] = fmaxf(_ev_sample_delayed.posErr, 0.01f); _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); @@ -154,7 +153,6 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); } else if (_control_status.flags.ev_pos) { - // warnx("fusing ext_visn_hight"); fuse_map[5] = true; // calculate the innovation assuming the external vision observaton is in local NED frame _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2); From f4a0f69f6e90fbb998da1ad48fa62385a69689c7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 21 Apr 2016 09:02:08 +1000 Subject: [PATCH 14/19] EKF: print to console when starting EV fusion --- EKF/control.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index d49b2d3bdc..2a63cb6b8b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -62,6 +62,7 @@ void Ekf::controlFusionModes() 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; + printf("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; @@ -97,6 +98,7 @@ void Ekf::controlFusionModes() // turn on fusion of external vision yaw measurements _control_status.flags.ev_yaw = true; + printf("EKF switching to external vision yaw fusion"); } } From b985e583331dfe75ca7081332994ed300a6539d1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 24 Apr 2016 22:20:18 +1000 Subject: [PATCH 15/19] EKF: clean up control function With the addition of new observation types, the control function has become too large and needed be broken up into separate functions --- EKF/control.cpp | 95 ++++++++++++++++++++++++++++++++----------------- EKF/ekf.h | 18 ++++++++++ 2 files changed, 80 insertions(+), 33 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2a63cb6b8b..b9191abeb7 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -56,6 +56,17 @@ void Ekf::controlFusionModes() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } + // control use of various external souces for positon and velocity aiding + controlExternalVisionAiding(); + controlOpticalFlowAiding(); + controlGpsAiding(); + controlHeightAiding(); + controlMagAiding(); + +} + +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 @@ -102,6 +113,10 @@ void Ekf::controlFusionModes() } } +} + +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 @@ -173,6 +188,22 @@ void Ekf::controlFusionModes() _control_status.flags.opt_flow = false; } + // 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(); + } + } +} + +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) { @@ -223,7 +254,10 @@ void Ekf::controlFusionModes() } } } +} +void Ekf::controlHeightSensorTimeouts() +{ /* * Handle the case where we have not fused height measurements recently and * uncertainty exceeds the max allowable. Reset using the best available height @@ -397,20 +431,37 @@ void Ekf::controlFusionModes() } } +} - // 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 veloity 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(); - } +void Ekf::controlHeightAiding() +{ + // check for height sensor timeouts and reset and change sensor if necessary + controlHeightSensorTimeouts(); + + // Control the soure of height measurements for the main filter + if (_control_status.flags.ev_pos) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { + _control_status.flags.baro_hgt = true; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + + } else 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; } +} +void Ekf::controlMagAiding() +{ // 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) { @@ -461,32 +512,10 @@ void Ekf::controlFusionModes() _control_status.flags.mag_dec = false; } - // Control the soure of height measurements for the main filter - if (_control_status.flags.ev_pos) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - - } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - - } else 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; - } - // 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.h b/EKF/ekf.h index 85d85d94c6..554cd74977 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -330,6 +330,24 @@ private: // Control the filter fusion modes void controlFusionModes(); + // control fusion of external vision observations + void controlExternalVisionAiding(); + + // control fusion of optical flow observtions + void controlOpticalFlowAiding(); + + // control fusion of GPS observations + void controlGpsAiding(); + + // control fusion of height position observations + void controlHeightAiding(); + + // control fusion of magnetometer observations + void controlMagAiding(); + + // control for height sensor timeouts, sensor changes and state resets + void controlHeightSensorTimeouts(); + // return the square of two floating point numbers - used in auto coded sections inline float sq(float var) { From 59eb9eb3db41a253ab5ee886573bd58ce117d1fd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 8 May 2016 16:32:12 +1000 Subject: [PATCH 16/19] EKF: fix shadow declaration arising from rebase --- EKF/mag_fusion.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e55062dbd9..950516b449 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -435,9 +435,9 @@ void Ekf::fuseHeading() measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; } else if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence - measured_hdg = atan2f(R_to_earth(1, 0) , R_to_earth(0, 0)); + measured_hdg = atan2f(R_to_earth_ev(1, 0) , R_to_earth_ev(0, 0)); } else { // there is no yaw observation return; @@ -513,9 +513,9 @@ void Ekf::fuseHeading() measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + _mag_declination; } else if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix - matrix::Dcm R_to_earth(_ev_sample_delayed.quat); // transformation matrix from body to world frame + matrix::Dcm R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence - measured_hdg = atan2f(-R_to_earth(0, 1) , R_to_earth(1, 1)); + measured_hdg = atan2f(-R_to_earth_ev(0, 1) , R_to_earth_ev(1, 1)); } else { // there is no yaw observation return; From 13c3a95bc18d72263a563fd9ef48dc924a83b85e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 15:22:44 +0930 Subject: [PATCH 17/19] EKF: Add missing line returns --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index b9191abeb7..2d7dbf6dec 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -73,7 +73,7 @@ void Ekf::controlExternalVisionAiding() 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; - printf("EKF switching to external vision position fusion"); + printf("EKF switching to external vision position fusion\n"); // turn off other forms of height aiding _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; @@ -109,7 +109,7 @@ void Ekf::controlExternalVisionAiding() // turn on fusion of external vision yaw measurements _control_status.flags.ev_yaw = true; - printf("EKF switching to external vision yaw fusion"); + printf("EKF switching to external vision yaw fusion\n"); } } From 920d83d68ce6b146f51967953c8a7cb366aeb8bc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 May 2016 16:10:34 +0930 Subject: [PATCH 18/19] EKF: Fix bugs preventing use of external vision yaw data --- EKF/common.h | 1 - EKF/control.cpp | 13 +++++++++++-- EKF/mag_fusion.cpp | 17 +++++++++++++---- 3 files changed, 24 insertions(+), 7 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 35d2966704..f7ac319286 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -163,7 +163,6 @@ struct extVisionSample { #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions -#define USE_EV_YAW 3 // Fuse yaw angle from external vision system // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 diff --git a/EKF/control.cpp b/EKF/control.cpp index 2d7dbf6dec..006e933564 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -86,7 +86,7 @@ void Ekf::controlExternalVisionAiding() } // external vision yaw aiding selection logic - if ((_params.mag_fusion_type == USE_EV_YAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + 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 @@ -107,8 +107,12 @@ void Ekf::controlExternalVisionAiding() // flag the yaw as aligned _control_status.flags.yaw_align = true; - // turn on fusion of external vision yaw measurements + // 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; + printf("EKF switching to external vision yaw fusion\n"); } } @@ -462,6 +466,11 @@ void Ekf::controlHeightAiding() void Ekf::controlMagAiding() { + // If we are using external vision data for heading then no magnetometer fusion is used + if (_control_status.flags.ev_yaw) { + 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) { diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 950516b449..d93e416159 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -379,9 +379,7 @@ void Ekf::fuseHeading() float q2 = _state.quat_nominal(2); float q3 = _state.quat_nominal(3); - float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f); - R_YAW = R_YAW * R_YAW; - + float R_YAW = 1.0f; float predicted_hdg; float H_YAW[4]; matrix::Vector3f mag_earth_pred; @@ -522,11 +520,22 @@ void Ekf::fuseHeading() } } + // Calculate the observation variance + if (_control_status.flags.mag_hdg) { + // using magnetic heading tuning parameter + R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)); + } else if (_control_status.flags.ev_yaw) { + // using error estimate from external vision data + R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); + } else { + // there is no yaw observation + return; + } + // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero // calculate the innovaton variance float PH[4]; _heading_innov_var = R_YAW; - for (unsigned row = 0; row <= 3; row++) { PH[row] = 0.0f; From c955bfbf932587135e52e6cbbe9d0077ade0253d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 15 May 2016 20:36:51 +1000 Subject: [PATCH 19/19] EKF: fix previous merge error --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index aaf25c9d7e..e15cb646a5 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -572,7 +572,7 @@ void Ekf::predictState() // filter and limit input between -50% and +100% of nominal value input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS)); - _dt_ekf_avg = 0.99f*_dt_ekf_avg + 0.005f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); + _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; } bool Ekf::collect_imu(imuSample &imu)