/**************************************************************************** * * 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 ekf.h * Class for core functions for ekf attitude and position estimator. * * @author Roman Bast * @author Paul Riseborough * */ #pragma once #include "estimator_interface.h" #include "EKFGSF_yaw.h" class Ekf final : public EstimatorInterface { public: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states typedef matrix::Vector Vector24f; typedef matrix::SquareMatrix SquareMatrix24f; typedef matrix::SquareMatrix Matrix2f; typedef matrix::Vector Vector4f; template using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; Ekf() = default; virtual ~Ekf() = default; // initialise variables to sane values (also interface class) bool init(uint64_t timestamp) override; // should be called every time new data is pushed into the filter bool update(); void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); } void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); } void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); } void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); } void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); } void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); } void getAuxVelInnov(float aux_vel_innov[2]) const; void getAuxVelInnovVar(float aux_vel_innov[2]) const; void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); } void getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); } void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); } void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; } const Vector2f &getFlowVelBody() const { return _flow_vel_body; } const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; } const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; } void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } void getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; } void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); } void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); } void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); } void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); } void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); } void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; } void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; } void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; } void getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; } void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; } void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; } void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const; // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; // get the wind velocity var Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } // get the true airspeed in m/s void get_true_airspeed(float *tas) const; // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } // get the orientation (quaterion) covariances matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } // get the linear velocity covariances matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } // get the position covariances matrix::SquareMatrix position_covariances() const { return P.slice<3, 3>(7, 7); } // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(const gps_message &gps) override; // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; } bool setEkfGlobalOriginAltitude(const float altitude); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const; // get the 1-sigma horizontal and vertical velocity uncertainty void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const; // get the vehicle control limits required by the estimator to keep within sensor limitations void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; // Reset all IMU bias states and covariances to initial alignment values. void resetImuBias(); void resetGyroBias(); void resetAccelBias(); // Reset all magnetometer bias states and covariances to initial alignment values. void resetMagBias(); Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); } // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/sec), (m) const Vector3f &getOutputTrackingError() const { return _output_tracking_error; } /* First argument returns GPS drift metrics in the following array locations 0 : Horizontal position drift rate (m/s) 1 : Vertical position drift rate (m/s) 2 : Filtered horizontal velocity (m/s) Second argument returns true when IMU movement is blocking the drift calculation Function returns true if the metrics have been updated and not returned previously by this function */ bool get_gps_drift_metrics(float drift[3], bool *blocked); // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation // and have not started using synthetic position observations to constrain drift bool global_position_is_valid() const { return (_NED_origin_initialised && local_position_is_valid()); } // return true if the local position estimate is valid bool local_position_is_valid() const { return (!_deadreckon_time_exceeded && !_using_synthetic_position); } bool isTerrainEstimateValid() const { return _hagl_valid; }; uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } // get the estimated terrain vertical position relative to the NED origin float getTerrainVertPos() const { return _terrain_vpos; }; // get the terrain variance float get_terrain_var() const { return _terrain_var; } Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2 const Vector3f &getMagBias() const { return _state.mag_B; } Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / _dt_ekf_avg; } // get the gyroscope bias variance in rad/s Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / _dt_ekf_avg; } // get the accelerometer bias variance in m/s**2 Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; } // get GPS check status void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; } const auto &state_reset_status() const { return _state_reset_status; } // return the amount the local vertical position changed in the last reset and the number of reset events void get_posD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter; } // return the amount the local vertical velocity changed in the last reset and the number of reset events void get_velD_reset(float *delta, uint8_t *counter) const { *delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter; } // return the amount the local horizontal position changed in the last reset and the number of reset events void get_posNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.posNE_change.copyTo(delta); *counter = _state_reset_status.posNE_counter; } // return the amount the local horizontal velocity changed in the last reset and the number of reset events void get_velNE_reset(float delta[2], uint8_t *counter) const { _state_reset_status.velNE_change.copyTo(delta); *counter = _state_reset_status.velNE_counter; } // return the amount the quaternion has changed in the last reset and the number of reset events void get_quat_reset(float delta_quat[4], uint8_t *counter) const { _state_reset_status.quat_change.copyTo(delta_quat); *counter = _state_reset_status.quat_counter; } // get EKF innovation consistency check status information comprising of: // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, float &hagl, float &beta) const; // return a bitmask integer that describes which state estimates can be used for flight control void get_ekf_soln_status(uint16_t *status) const; // return the quaternion defining the rotation from the External Vision to the EKF reference frame matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); }; // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } // get solution data from the EKF-GSF emergency yaw estimator // returns false when data is not available bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]); private: // set the internal states and status to their default value void reset(); bool initialiseTilt(); // Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter // and reset the velocity and position states to the GPS. This will cause the EKF // to ignore the magnetometer for the remainder of flight. // This should only be used as a last resort before activating a loss of navigation failsafe void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; } // check if the EKF is dead reckoning horizontal velocity using inertial data only void update_deadreckoning_status(); void updateTerrainValidity(); struct { uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255) uint8_t posNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t posD_counter; ///< number of vertical position reset events (allow to wrap if count exceeds 255) uint8_t quat_counter; ///< number of quaternion reset events (allow to wrap if count exceeds 255) Vector2f velNE_change; ///< North East velocity change due to last reset (m) float velD_change; ///< Down velocity change due to last reset (m/sec) Vector2f posNE_change; ///< North, East position change due to last reset (m) float posD_change; ///< Down position change due to last reset (m) Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf Vector3f _ang_rate_delayed_raw; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m) Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec) uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec) uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec) uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) Vector2f _last_known_posNE; ///< last known local NE position vector (m) float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec) Vector3f _earth_rate_NED; ///< earth rotation vector (NED) in rad/s Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF prediction // used by magnetometer fusion mode selection Vector2f _accel_lpf_NE; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve. float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements bool _non_mag_yaw_aiding_running_prev{false}; ///< true when heading is being fused from other sources that are not the magnetometer (for example EV or GPS). bool _is_yaw_fusion_inhibited{false}; ///< true when yaw sensor use is being inhibited SquareMatrix24f P; ///< state covariance matrix Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance Vector3f _last_vel_obs; ///< last velocity observation (m/s) Vector3f _last_vel_obs_var; ///< last velocity observation variance (m/s)**2 Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2 float _vert_pos_innov_ratio; ///< vertical position innovation divided by estimated standard deviation of innovation uint64_t _vert_pos_fuse_attempt_time_us; ///< last system time in usec vertical position measurement fuson was attempted float _vert_vel_innov_ratio; ///< standard deviation of vertical velocity innovation uint64_t _vert_vel_fuse_time_us; ///< last system time in usec time vertical velocity measurement fuson was attempted Vector3f _gps_vel_innov; ///< GPS velocity innovations (m/sec) Vector3f _gps_vel_innov_var; ///< GPS velocity innovation variances ((m/sec)**2) Vector3f _gps_pos_innov; ///< GPS position innovations (m) Vector3f _gps_pos_innov_var; ///< GPS position innovation variances (m**2) Vector3f _ev_vel_innov; ///< external vision velocity innovations (m/sec) Vector3f _ev_vel_innov_var; ///< external vision velocity innovation variances ((m/sec)**2) Vector3f _ev_pos_innov; ///< external vision position innovations (m) Vector3f _ev_pos_innov_var; ///< external vision position innovation variances (m**2) Vector3f _baro_hgt_innov; ///< baro hgt innovations (m) Vector3f _baro_hgt_innov_var; ///< baro hgt innovation variances (m**2) Vector3f _rng_hgt_innov; ///< range hgt innovations (m) Vector3f _rng_hgt_innov_var; ///< range hgt innovation variances (m**2) Vector3f _aux_vel_innov; ///< horizontal auxiliary velocity innovations: (m/sec) Vector3f _aux_vel_innov_var; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) float _heading_innov{0.0f}; ///< heading measurement innovation (rad) float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2) Vector3f _mag_innov; ///< earth magnetic field innovations (Gauss) Vector3f _mag_innov_var; ///< earth magnetic field innovation variance (Gauss**2) Vector2f _drag_innov; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec) float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2) float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad) float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2) float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) // optical flow processing Vector2f _flow_innov; ///< flow measurement innovation (rad/sec) Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2) Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector2f _flow_vel_body; ///< velocity from corrected flow measurement (body frame)(m/s) Vector2f _flow_vel_ne; ///< velocity from corrected flow measurement (local frame) (m/s) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited Vector2f _flow_compensated_XY_rad; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive // output predictor states Vector3f _delta_angle_corr; ///< delta angle correction vector (rad) Vector3f _vel_err_integ; ///< integral of velocity tracking error (m) Vector3f _pos_err_integ; ///< integral of position tracking error (m.s) Vector3f _output_tracking_error; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m) // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt; ///< GPS NED position derivative (m/sec) Vector2f _gps_velNE_filt; ///< filtered GPS North and East velocity (m/sec) float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec) uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks float _gps_error_norm{1.0f}; ///< normalised gps error uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed // 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) float _gps_alt_ref{0.0f}; ///< WGS-84 height (m) // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) // Variables used to perform in flight resets and switch between height sources AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m) // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) Matrix2f _saved_mag_ef_covmat; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required gps_check_fail_status_u _gps_check_fail_status{}; // variables used to inhibit accel bias learning bool _accel_bias_inhibit[3] {}; ///< true when the accel bias learning is being inhibited for the specified axis Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2) float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2 // Terrain height state estimation float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized bool _hagl_valid{false}; ///< true when the height above ground estimate is valid terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground // height sensor status bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent bool _is_gps_yaw_faulty{false}; ///< true if gps yaw data is rejected by the filter for too long // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected uint16_t _clip_counter{0}; ///< counter that increments when clipping ad decrements when not // variables used to control range aid functionality bool _is_range_aid_suitable{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor float _height_rate_lpf{0.0f}; // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(const imuSample &imu); void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction); // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); // initialise ekf covariance matrix void initialiseCovariance(); // predict ekf state void predictState(); // predict ekf covariance void predictCovariance(); // ekf sequential fusion of magnetometer measurements void fuseMag(); // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer) void fuseHeading(); // fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence // yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation); // fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence // yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad) // yaw_variance : variance of the yaw angle observation (rad^2) // zero_innovation : Fuse data with innovation set to zero void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector // innovation : prediction - measurement // variance : observaton variance // gate_sigma : innovation consistency check gate size (Sigma) // jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian); // fuse the yaw angle obtained from a dual antenna GPS unit void fuseGpsYaw(); // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit // return true if the reset was successful bool resetYawToGps(); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians void fuseDeclination(float decl_sigma); // apply sensible limits to the declination and length of the NE mag field states estimates void limitDeclination(); // fuse airspeed measurement void fuseAirspeed(); // fuse synthetic zero sideslip measurement void fuseSideslip(); // fuse body frame drag specific forces for multi-rotor wind estimation void fuseDrag(); // fuse single velocity and position measurement void fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); void resetVelocity(); void resetVelocityToGps(); inline void resetHorizontalVelocityToOpticalFlow(); inline void resetVelocityToVision(); inline void resetHorizontalVelocityToZero(); inline void resetVelocityTo(const Vector3f &vel); inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel); inline void resetVerticalVelocityTo(float new_vert_vel); void resetHorizontalPosition(); void resetHorizontalPositionToGps(); inline void resetHorizontalPositionToVision(); inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos); inline void resetVerticalPositionTo(const float &new_vert_pos); void resetHeight(); // fuse optical flow line of sight rate measurements void fuseOptFlow(); bool fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); bool fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); bool fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false); bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable bool calcOptFlowBodyRateComp(); // initialise the terrain vertical position estimator // return true if the initialisation is successful bool initHagl(); bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); } bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); } // run the terrain estimator void runTerrainEstimator(); // update the terrain vertical position estimate using a height above ground measurement from the range finder void fuseHagl(); // update the terrain vertical position estimate using an optical flow measurement void fuseFlowForTerrain(); // reset the heading and magnetic field states using the declination and magnetometer/external vision measurements // return true if successful bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true); // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS. // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle. bool realignYawGPS(); // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(); // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); Vector3f getVisionVelocityInEkfFrame() const; Vector3f getVisionVelocityVarianceInEkfFrame() const; // matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24> // that is optimized by exploring the sparsity in H template SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f &H) const { SquareMatrix24f KHP; constexpr size_t non_zeros = sizeof...(Idxs); float KH[non_zeros]; for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned i = 0; i < H.non_zeros(); i++) { KH[i] = K(row) * H.atCompressedIndex(i); } for (unsigned column = 0; column < _k_num_states; column++) { float tmp = 0.f; for (unsigned i = 0; i < H.non_zeros(); i++) { const size_t index = H.index(i); tmp += KH[i] * P(index, column); } KHP(row, column) = tmp; } } return KHP; } // measurement update with a single measurement // returns true if fusion is performed template bool measurementUpdate(Vector24f &K, const SparseVector24f &H, float innovation) { for (unsigned i = 0; i < 3; i++) { if (_accel_bias_inhibit[i]) { K(13 + i) = 0.0f; } } // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP const SquareMatrix24f KHP = computeKHP(K, H); const bool is_healthy = checkAndFixCovarianceUpdate(KHP); if (is_healthy) { // apply the covariance corrections P -= KHP; fixCovarianceErrors(true); // apply the state corrections fuse(K, innovation); } return is_healthy; } // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); // constrain the ekf states void constrainStates(); // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value void fuse(const Vector24f &K, float innovation); float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override; // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; // return true id the GPS quality is good enough to set an origin and start aiding bool gps_is_good(const gps_message &gps); // Control the filter fusion modes void controlFusionModes(); // control fusion of external vision observations void controlExternalVisionFusion(); // control fusion of optical flow observations void controlOpticalFlowFusion(); void updateOnGroundMotionForOpticalFlowChecks(); void resetOnGroundMotionForOpticalFlowChecks(); // control fusion of GPS observations void controlGpsFusion(); void controlGpsYawFusion(); // control fusion of magnetometer observations void controlMagFusion(); bool noOtherYawAidingThanMag() const; bool otherHeadingSourcesHaveStopped(); void checkHaglYawResetReq(); float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } void runOnGroundYawReset(); bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; } bool canResetMagHeading() const; void runInAirYawReset(); bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; } void runVelPosReset(); void selectMagAuto(); void check3DMagFusionSuitability(); void checkYawAngleObservability(); void checkMagBiasObservability(); bool isYawAngleObservable() const { return _yaw_angle_observable; } bool isMagBiasObservable() const { return _mag_bias_observable; } bool canUse3DMagFusion() const; void checkMagDeclRequired(); void checkMagInhibition(); bool shouldInhibitMag() const; void checkMagFieldStrength(); bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; } bool isMeasuredMatchingGpsMagStrength() const; bool isMeasuredMatchingAverageMagStrength() const; static bool isMeasuredMatchingExpected(float measured, float expected, float gate); void runMagAndMagDeclFusions(); void run3DMagAndDeclFusions(); // control fusion of range finder observations void controlRangeFinderFusion(); // control fusion of air data observations void controlAirDataFusion(); // control fusion of synthetic sideslip observations void controlBetaFusion(); // control fusion of multi-rotor drag specific force observations void controlDragFusion(); // control fusion of pressure altitude observations void controlBaroFusion(); // control fusion of fake position observations to constrain drift void controlFakePosFusion(); // control fusion of auxiliary velocity observations void controlAuxVelFusion(); // control for height sensor timeouts, sensor changes and state resets void controlHeightSensorTimeouts(); void checkVerticalAccelerationHealth(); // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); // determine if flight condition is suitable to use range finder instead of the primary height sensor void checkRangeAidSuitability(); bool isRangeAidSuitable() const { return _is_range_aid_suitable; } // set control flags to use baro height void setControlBaroHeight(); // set control flags to use range height void setControlRangeHeight(); // set control flags to use GPS height void setControlGPSHeight(); // set control flags to use external vision height void setControlEVHeight(); void stopMagFusion(); void stopMag3DFusion(); void stopMagHdgFusion(); void startMagHdgFusion(); void startMag3DFusion(); void startBaroHgtFusion(); void startGpsHgtFusion(); void updateBaroHgtOffset(); // return an estimation of the GPS altitude variance float getGpsHeightVariance(); // calculate the measurement variance for the optical flow sensor float calcOptFlowMeasVar(); // rotate quaternion covariances into variances for an equivalent rotation vector Vector3f calcRotVecVariances(); // initialise the quaternion covariances using rotation vector variances // do not call before quaternion states are initialised void initialiseQuatCovariances(Vector3f &rot_vec_var); // perform a limited reset of the magnetic field related state covariances void resetMagRelatedCovariances(); void resetQuatCov(); void zeroQuatCov(); void resetMagCov(); // perform a limited reset of the wind state covariances void resetWindCovariance(); // perform a reset of the wind states void resetWindStates(); // check that the range finder data is continuous void updateRangeDataContinuity(); // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void increaseQuatYawErrVariance(float yaw_variance); // load and save mag field state covariance data for re-use void loadMagCovData(); void saveMagCovData(); void clearMagCov(); void zeroMagCov(); // uncorrelate quaternion states from other states void uncorrelateQuatFromOtherStates(); // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter // sensor measurement float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { return last_sensor_timestamp + timeout_period < _time_last_imu; } bool isRecent(uint64_t sensor_timestamp, uint64_t acceptance_interval) const { return sensor_timestamp + acceptance_interval > _time_last_imu; } void startGpsFusion(); void stopGpsFusion(); void stopGpsPosFusion(); void stopGpsVelFusion(); void startGpsYawFusion(); void stopGpsYawFusion(); void startEvPosFusion(); void startEvVelFusion(); void startEvYawFusion(); void stopEvFusion(); void stopEvPosFusion(); void stopEvVelFusion(); void stopEvYawFusion(); void stopAuxVelFusion(); void stopFlowFusion(); void setVelPosFaultStatus(const int index, const bool status); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) // yaw_variance : yaw error variance (rad^2) // update_buffer : true if the state change should be also applied to the output observer buffer void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer); // Declarations used to control use of the EKF-GSF yaw estimator // yaw estimator instance EKFGSF_yaw _yawEstimator; int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate // Call once per _imu_sample_delayed update after all main EKF data fusion oeprations have been completed void runYawEKFGSF(); // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator // Resets the horizontal velocity and position to the default navigation sensor // Returns true if the reset was successful bool resetYawToEKFGSF(); void resetGpsDriftCheckFilters(); };