From 46b0e9654cac69d6b71d291c96a8604d0daf8408 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 28 Jan 2016 21:52:39 +1100 Subject: [PATCH] Add filter control logic --- CMakeLists.txt | 3 +- EKF/control.cpp | 129 +++++++++++++++++++++++++++++++++++++++++ EKF/ekf.cpp | 52 ++++++++++++----- EKF/ekf.h | 77 +++++++++++++++++++++++- EKF/ekf_helper.cpp | 15 +++++ EKF/estimator_base.cpp | 22 +------ EKF/estimator_base.h | 45 +++----------- EKF/gps_checks.cpp | 23 ++++++-- EKF/mag_fusion.cpp | 2 +- EKF/vel_pos_fusion.cpp | 25 +++++++- 10 files changed, 309 insertions(+), 84 deletions(-) create mode 100644 EKF/control.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 368c0e651c..3b39af9f6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,7 +54,8 @@ px4_add_module( EKF/covariance.cpp EKF/vel_pos_fusion.cpp EKF/mag_fusion.cpp - EKF/gps_checks.cpp + EKF/gps_checks.cpp + EKF/control.cpp DEPENDS platforms__common ) diff --git a/EKF/control.cpp b/EKF/control.cpp new file mode 100644 index 0000000000..cc69c650c3 --- /dev/null +++ b/EKF/control.cpp @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file control.cpp + * Control functions for ekf attitude and position estimator. + * + * @author Paul Riseborough + * + */ + +#include "ekf.h" + +void Ekf::controlFusionModes() +{ + // Determine the vehicle status + calculateVehicleStatus(); + + // optical flow fusion mode selection logic + _control_status.flags.opt_flow = false; + + // GPS fusion mode selection logic + // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data + if (!_control_status.flags.gps) { + if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _gps_initialised) { + _control_status.flags.gps = true; + resetPosition(); + resetVelocity(); + } + } + + // decide when to start using optical flow data + if (!_control_status.flags.opt_flow) { + // TODO optical flow start logic + } + + // 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 veloity 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(); + } else { + // Reset states to the last GPS measurement + resetPosition(); + resetVelocity(); + } + } + } + + // handle the case when we are relying on optical flow fusion and lose it + if (_control_status.flags.opt_flow && !_control_status.flags.gps) { + // TODO + } + + // 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 (!_control_status.flags.armed) { + // always use simple mag fusion for initial startup + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } else { + if (_control_status.flags.in_air) { + // always use 3-axis mag fusion when airborne + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = true; + } else { + // always use simple heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } + } +} + +void Ekf::calculateVehicleStatus() +{ + // determine if the vehicle is armed + _control_status.flags.armed = _vehicle_armed; + + // record vertical position whilst disarmed to use as a height change reference + if (!_control_status.flags.armed) { + _last_disarmed_posD = _state.pos(2); + } + + // Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming + if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) { + _control_status.flags.in_air = true; + } + + // Transition to on-ground occurs when disarmed. + if (_control_status.flags.in_air && !_control_status.flags.armed) { + _control_status.flags.in_air = false; + } +} diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8ff0b496b6..3d074b55cb 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -43,25 +43,31 @@ #include Ekf::Ekf(): - _filter_initialised(false), + _control_status{}, + _filter_initialised(false), _earth_rate_initialised(false), _fuse_height(false), _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), - _time_last_fake_gps(0), - _vel_pos_innov{}, + _time_last_fake_gps(0), + _time_last_pos_fuse(0), + _time_last_vel_fuse(0), + _time_last_hgt_fuse(0), + _time_last_of_fuse(0), + _vel_pos_innov{}, _mag_innov{}, _heading_innov{}, _vel_pos_innov_var{}, _mag_innov_var{}, - _heading_innov_var{} + _heading_innov_var{} { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); _delta_angle_corr.setZero(); _delta_vel_corr.setZero(); _vel_corr.setZero(); + _last_known_posNE.setZero(); } @@ -91,28 +97,41 @@ bool Ekf::update() predictCovariance(); } - // measurement updates + // control logic + controlFusionModes(); + // measurement updates + + // Fuse magnetometer data using the selected fuson method and only if angular alignment is complete if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { - //fuseHeading(); - fuseMag(); + if (_control_status.flags.mag_3D && _control_status.flags.angle_align) { + fuseMag(); + if (_control_status.flags.mag_dec) { + // TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding + // otherwise heading will slowly drift + } + } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) { + fuseHeading(); + } } if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { _fuse_height = true; } - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { + // If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it + // 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 (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { _fuse_pos = true; _fuse_vert_vel = true; _fuse_hor_vel = true; - - } else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) { - // if gps does not update then fake position and horizontal veloctiy measurements - _fuse_hor_vel = true; // we only fake horizontal velocity because we still have baro - _gps_sample_delayed.vel.setZero(); - } - + } else 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; + } if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) { fuseVelPosHeight(); @@ -178,6 +197,9 @@ bool Ekf::initialiseFilter(void) _state.quat_nominal = Quaternion(euler_init); _output_new.quat_nominal = _state.quat_nominal; + // TODO replace this with a conditional test based on fitered angle error states. + _control_status.flags.angle_align = true; + // calculate initial earth magnetic field states matrix::Dcm R_to_earth(euler_init); _state.mag_I = R_to_earth * mag_init.mag; diff --git a/EKF/ekf.h b/EKF/ekf.h index 640a7cf8e6..7e69916025 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -36,6 +36,7 @@ * Class for core functions for ekf attitude and position estimator. * * @author Roman Bast + * @author Paul Riseborough * */ @@ -78,6 +79,28 @@ public: // get the diagonal elements of the covariance matrix void get_covariances(float *covariances); + // bitmask containing filter control status + union filter_control_status_u { + struct { + uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete + uint8_t gps : 1; // 1 - true if GPS measurements are being fused + uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused + uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused + uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused + uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused + uint8_t in_air : 1; // 6 - true when the vehicle is airborne + uint8_t armed : 1; // 7 - true when the vehicle motors are armed + } flags; + uint16_t value; + }; + filter_control_status_u _control_status={}; + + // get the ekf WGS-84 origin positoin and height and the system time it was last set + void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); + + // set vehicle arm status data + void set_arm_status(bool data); + private: static const uint8_t _k_num_states = 24; @@ -91,7 +114,14 @@ private: bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - uint64_t _time_last_fake_gps; + uint64_t _time_last_fake_gps; + + uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec) + uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec) + uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec) + uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec) + Vector2f _last_known_posNE; // last known local NE position vector (m) + float _last_disarmed_posD; // vertical position recorded at arming (m) Vector3f _earth_rate_NED; @@ -112,7 +142,40 @@ private: Vector3f _delta_vel_corr; Vector3f _vel_corr; - void calculateOutputStates(); + // variables used for the GPS quality checks + float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s) + float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s) + float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) + float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) + float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) + float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) + uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks + + // Variables used to publish the WGS-84 location of the EKF local NED origin + uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) + struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) + float _gps_alt_ref = 0.0f; // WGS-84 height (m) + + bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground + + // publish the status of various GPS quality checks + union gps_check_fail_status_u { + struct { + uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient + uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient + uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient + uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient + uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient + uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground) + uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground) + uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground) + uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive + } flags; + uint16_t value; + }; + gps_check_fail_status_u _gps_check_fail_status; + + void calculateOutputStates(); bool initialiseFilter(void); @@ -156,4 +219,14 @@ private: void calcEarthRateNED(Vector3f &omega, double lat_rad) const; + void initialiseGPS(struct gps_message *gps); + + // return true id the GPS quality is good enough to set an origin and start aiding + bool gps_is_good(struct gps_message *gps); + + // Control the filter fusion modes + void controlFusionModes(); + + // Determine if we are airborne or motors are armed + void calculateVehicleStatus(); }; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 620740ec42..68b9d565d5 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -254,3 +254,18 @@ void Ekf::get_covariances(float *covariances) covariances[i] = P[i][i]; } } + +// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set +void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) +{ + memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); + memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); + memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); +} + +// set the vehicle arm status +void Ekf::set_arm_status(bool data) +{ + _vehicle_armed = data; + +} diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 55ff6547c8..0fa2fc01c4 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -165,22 +165,20 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) return; } - if (time_usec - _time_last_gps > 70000) { + // Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz + if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) { gpsSample gps_sample_new = {}; gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; - gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; _time_last_gps = time_usec; gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us); - memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data)); _gps_speed_valid = gps->vel_ned_valid; - float lpos_x = 0.0f; float lpos_y = 0.0f; map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); @@ -293,7 +291,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _gps_speed_valid = false; _mag_healthy = false; - _in_air = false; // XXX get this flag from the application _time_last_imu = 0; _time_last_gps = 0; @@ -306,21 +303,6 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) } -void EstimatorBase::initialiseGPS(struct gps_message *gps) -{ - //Check if the GPS fix is good enough for us to use - if (gps_is_good(gps)) { - printf("gps is good\n"); - // Initialise projection - double lat = gps->lat / 1.0e7; - double lon = gps->lon / 1.0e7; - map_projection_init(&_pos_ref, lat, lon); - _gps_alt_ref = gps->alt / 1e3f; - _gps_initialised = true; - _last_gps_origin_time_us = _time_last_imu; - } -} - bool EstimatorBase::position_is_valid() { // return true if the position estimate is valid diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 75af63dda8..5b7ddb7a11 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -159,6 +159,13 @@ public: // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() {return &_params;} + + // get the ekf WGS-84 origin positoin and height and the system time it was last set + virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0; + + // set vehicle arm status data + void set_arm_status(bool data); + protected: typedef matrix::Vector Vector2f; @@ -267,8 +274,6 @@ protected: float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios - bool _in_air = true; // indicates if the vehicle is in the air - RingBuffer _imu_buffer; RingBuffer _gps_buffer; RingBuffer _mag_buffer; @@ -296,19 +301,6 @@ protected: void initialiseVariables(uint64_t timestamp); - void initialiseGPS(struct gps_message *gps); - - bool gps_is_good(struct gps_message *gps); - - // variables used for the GPS quality checks - float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s) - float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s) - float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s) - float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s) - float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s) - float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s) - uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks - public: void printIMU(struct imuSample *data); void printStoredIMU(); @@ -345,29 +337,6 @@ public: { *time_us = _imu_time_last; } - - // Variables used to publish the WGS-84 location of the EKF local NED origin - uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec) - struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) - float _gps_alt_ref = 0.0f; // WGS-84 height (m) - bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground - // publish the status of various GPS quality checks - union gps_check_fail_status_u { - struct { - uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient - uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient - uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient - uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient - uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient - uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground) - uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground) - uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground) - uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive - } flags; - uint16_t value; - }; - gps_check_fail_status_u _gps_check_fail_status; - }; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 019a72809b..c167a6b7c2 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -52,6 +52,21 @@ #define MASK_GPS_HSPD (1<<7) #define MASK_GPS_VSPD (1<<8) +void Ekf::initialiseGPS(struct gps_message *gps) +{ + //Check if the GPS fix is good enough for us to use + if (gps_is_good(gps)) { + printf("gps is good\n"); + // Initialise projection + double lat = gps->lat / 1.0e7; + double lon = gps->lon / 1.0e7; + map_projection_init(&_pos_ref, lat, lon); + _gps_alt_ref = gps->alt / 1e3f; + _gps_initialised = true; + _last_gps_origin_time_us = _time_last_imu; + } +} + /* * Return true if the GPS solution quality is adequate to set an origin for the EKF * and start GPS aiding. @@ -59,7 +74,7 @@ * Checks are activated using the EKF2_GPS_CHECKS bitmask parameter * Checks are adjusted using the EKF2_REQ_* parameters */ -bool EstimatorBase::gps_is_good(struct gps_message *gps) +bool Ekf::gps_is_good(struct gps_message *gps) { // Check the number of satellites _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); @@ -104,7 +119,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps) // Calculate the horizontal drift speed and fail if too high // This check can only be used if the vehicle is stationary during alignment - if(_vehicle_armed) { + if(!_control_status.flags.armed) { float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE); _gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift); } else { @@ -127,7 +142,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps) // Fail if the vertical drift speed is too high // This check can only be used if the vehicle is stationary during alignment - if(_vehicle_armed) { + if(!_control_status.flags.armed) { _gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift); } else { _gps_check_fail_status.flags.vdrift = false; @@ -135,7 +150,7 @@ bool EstimatorBase::gps_is_good(struct gps_message *gps) // Check the magnitude of the filtered horizontal GPS velocity // This check can only be used if the vehicle is stationary during alignment - if (_vehicle_armed) { + if (!_control_status.flags.armed) { vel_limit = 10.0f * _params.req_hdrift; float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit); float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 01878e2e0c..1078ebe324 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -561,7 +561,7 @@ void Ekf::fuseHeading() // if we are in air we don't want to fuse the measurement // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias - if (_in_air) { + if (_control_status.flags.in_air) { return; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index a4b1aa449d..1ca95847bd 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -49,7 +49,7 @@ void Ekf::fuseVelPosHeight() float gate_size[6] = {}; float Kfusion[24] = {}; - // calculate innovations + // calculate innovations and gate sizes if (_fuse_hor_vel) { fuse_map[0] = fuse_map[1] = true; _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); @@ -97,10 +97,29 @@ void Ekf::fuseVelPosHeight() // check position, velocity and height innovations // treat 3D velocity, 2D position and height as separate sensors - innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); - innov_check_pass_map[4] = innov_check_pass_map[3] = (_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f); + // always pass position checks if using synthetic position measurements + bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); + innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; + bool using_synthetic_measurements = !_control_status.flags.gps && !_control_status.flags.opt_flow; + bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || using_synthetic_measurements; + innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f); + // record the successful velocity fusion time + if (vel_check_pass && _fuse_hor_vel) { + _time_last_vel_fuse = _time_last_imu; + } + + // record the successful position fusion time + if (pos_check_pass && _fuse_pos) { + _time_last_pos_fuse = _time_last_imu; + } + + // record the successful height fusion time + if (innov_check_pass_map[5] && _fuse_height) { + _time_last_hgt_fuse = _time_last_imu; + } + for (unsigned obs_index = 0; obs_index < 6; obs_index++) { // skip fusion if not requested or checks have failed if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {