diff --git a/EKF/ekf.h b/EKF/ekf.h index baf6cdcc6e..dd4f9edd2b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -70,82 +70,57 @@ class Ekf : public EstimatorInterface bool update() override; void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override; void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override; - void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override; - void getBaroHgtInnov(float &baro_hgt_innov) const override; - - void getBaroHgtInnovVar(float &baro_hgt_innov_var) const override; - - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override; - - void getRngHgtInnov(float &rng_hgt_innov) const override; - - void getRngHgtInnovVar(float &rng_hgt_innov_var) const override; + void getBaroHgtInnov(float &baro_hgt_innov) const override { baro_hgt_innov = _baro_hgt_innov(2); } + void getBaroHgtInnovVar(float &baro_hgt_innov_var) const override { baro_hgt_innov_var = _baro_hgt_innov_var(2); } + void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override; + void getRngHgtInnov(float &rng_hgt_innov) const override { rng_hgt_innov = _rng_hgt_innov(2); } + void getRngHgtInnovVar(float &rng_hgt_innov_var) const override { rng_hgt_innov_var = _rng_hgt_innov_var(2); } + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); } void getAuxVelInnov(float aux_vel_innov[2]) const override; - void getAuxVelInnovVar(float aux_vel_innov[2]) const override; + void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override { aux_vel_innov_ratio = _aux_vel_test_ratio(0); } - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override; - - void getFlowInnov(float flow_innov[2]) const override; - - void getFlowInnovVar(float flow_innov_var[2]) const override; - - void getFlowInnovRatio(float &flow_innov_ratio) const override; - - Vector2f getFlowVelBody() const override; - Vector2f getFlowVelNE() const override; - Vector2f getFlowCompensated() const override; - Vector2f getFlowUncompensated() const override; - Vector3f getFlowGyro() const override; - - void getHeadingInnov(float &heading_innov) const override; - - void getHeadingInnovVar(float &heading_innov_var) const override; - - void getHeadingInnovRatio(float &heading_innov_ratio) const override; + void getFlowInnov(float flow_innov[2]) const override { _flow_innov.copyTo(flow_innov); } + void getFlowInnovVar(float flow_innov_var[2]) const override { _flow_innov_var.copyTo(flow_innov_var); } + void getFlowInnovRatio(float &flow_innov_ratio) const override { flow_innov_ratio = _optflow_test_ratio; } + const Vector2f &getFlowVelBody() const override { return _flow_vel_body; } + const Vector2f &getFlowVelNE() const override { return _flow_vel_ne; } + const Vector2f &getFlowCompensated() const override { return _flow_compensated_XY_rad; } + const Vector2f &getFlowUncompensated() const override { return _flow_sample_delayed.flow_xy_rad; } + const Vector3f &getFlowGyro() const override { return _flow_sample_delayed.gyro_xyz; } - void getMagInnov(float mag_innov[3]) const override; + void getHeadingInnov(float &heading_innov) const override { heading_innov = _heading_innov; } + void getHeadingInnovVar(float &heading_innov_var) const override { heading_innov_var = _heading_innov_var; } - void getMagInnovVar(float mag_innov_var[3]) const override; + void getHeadingInnovRatio(float &heading_innov_ratio) const override { heading_innov_ratio = _yaw_test_ratio; } + void getMagInnov(float mag_innov[3]) const override { _mag_innov.copyTo(mag_innov); } + void getMagInnovVar(float mag_innov_var[3]) const override { _mag_innov_var.copyTo(mag_innov_var); } + void getMagInnovRatio(float &mag_innov_ratio) const override { mag_innov_ratio = _mag_test_ratio.max(); } - void getMagInnovRatio(float &mag_innov_ratio) const override; + void getDragInnov(float drag_innov[2]) const override { _drag_innov.copyTo(drag_innov); } + void getDragInnovVar(float drag_innov_var[2]) const override { _drag_innov_var.copyTo(drag_innov_var); } + void getDragInnovRatio(float drag_innov_ratio[2]) const override { _drag_test_ratio.copyTo(drag_innov_ratio); } - void getDragInnov(float drag_innov[2]) const override; + void getAirspeedInnov(float &airspeed_innov) const override { airspeed_innov = _airspeed_innov; } + void getAirspeedInnovVar(float &airspeed_innov_var) const override { airspeed_innov_var = _airspeed_innov_var; } + void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override { airspeed_innov_ratio = _tas_test_ratio; } - void getDragInnovVar(float drag_innov_var[2]) const override; + void getBetaInnov(float &beta_innov) const override { beta_innov = _beta_innov; } + void getBetaInnovVar(float &beta_innov_var) const override { beta_innov_var = _beta_innov_var; } + void getBetaInnovRatio(float &beta_innov_ratio) const override { beta_innov_ratio = _beta_test_ratio; } - void getDragInnovRatio(float drag_innov_ratio[2]) const override; - - void getAirspeedInnov(float &airspeed_innov) const override; - - void getAirspeedInnovVar(float &airspeed_innov_var) const override; - - void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override; - - void getBetaInnov(float &beta_innov) const override; - - void getBetaInnovVar(float &beta_innov_var) const override; - - void getBetaInnovRatio(float &beta_innov_ratio) const override; - - void getHaglInnov(float &hagl_innov) const override; - - void getHaglInnovVar(float &hagl_innov_var) const override; - - void getHaglInnovRatio(float &hagl_innov_ratio) const override; + void getHaglInnov(float &hagl_innov) const override { hagl_innov = _hagl_innov; } + void getHaglInnovVar(float &hagl_innov_var) const override { hagl_innov_var = _hagl_innov_var; } + void getHaglInnovRatio(float &hagl_innov_ratio) const override { hagl_innov_ratio = _hagl_test_ratio; } // get the state vector at the delayed time horizon matrix::Vector getStateAtFusionHorizonAsVector() const override; @@ -206,7 +181,7 @@ class Ekf : public EstimatorInterface // return an array containing the output predictor angular, velocity and position tracking // error magnitudes (rad), (m/sec), (m) - Vector3f getOutputTrackingError() const override; + Vector3f getOutputTrackingError() const override { return _output_tracking_error; } /* Returns following IMU vibration metrics in the following array locations @@ -214,7 +189,7 @@ class Ekf : public EstimatorInterface 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) */ - Vector3f getImuVibrationMetrics() const override; + Vector3f getImuVibrationMetrics() const override { return _vibe_metrics; } /* First argument returns GPS drift metrics in the following array locations @@ -245,13 +220,13 @@ class Ekf : public EstimatorInterface float get_terrain_var() const { return _terrain_var; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBias() const override; + Vector3f getAccelBias() const override { return _state.delta_vel_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s - Vector3f getGyroBias() const override; + Vector3f getGyroBias() const override { return _state.delta_ang_bias / _dt_ekf_avg; } // get GPS check status - void get_gps_check_status(uint16_t *val) override; + void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; } // 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) override {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;} @@ -301,13 +276,14 @@ class Ekf : public EstimatorInterface // 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]) override; + 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]) override; // 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() override; + void requestEmergencyNavReset() override { _do_ekfgsf_yaw_reset = true; } private: struct { @@ -528,7 +504,7 @@ class Ekf : public EstimatorInterface // and the correction step void calculateOutputStates(); void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); - void applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Vector3f& pos_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); @@ -565,7 +541,7 @@ class Ekf : public EstimatorInterface // 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); + 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(); @@ -678,31 +654,37 @@ class Ekf : public EstimatorInterface // 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 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++) { + 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++) { + + 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; + + KHP(row, column) = tmp; } } + return KHP; } // measurement update with a single measurement // returns true if fusion is performed template - bool measurementUpdate(const Vector24f& K, const SparseVector24f& H, float innovation) { + bool measurementUpdate(const Vector24f &K, const SparseVector24f &H, float innovation) + { // apply covariance correction via P_new = (I -K*H)*P // first calculate expression for KHP // then calculate P - KHP @@ -719,12 +701,13 @@ class Ekf : public EstimatorInterface // 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); + bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true @@ -735,7 +718,7 @@ class Ekf : public EstimatorInterface // 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); + void fuse(const Vector24f &K, float innovation); float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 57ccb69738..853dd1094f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -646,36 +646,6 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp vpos = _ev_pos_test_ratio(1); } -void Ekf::getBaroHgtInnov(float &baro_hgt_innov) const -{ - baro_hgt_innov = _baro_hgt_innov(2); -} - -void Ekf::getBaroHgtInnovVar(float &baro_hgt_innov_var) const -{ - baro_hgt_innov_var = _baro_hgt_innov_var(2); -} - -void Ekf::getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const -{ - baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); -} - -void Ekf::getRngHgtInnov(float &rng_hgt_innov) const -{ - rng_hgt_innov = _rng_hgt_innov(2); -} - -void Ekf::getRngHgtInnovVar(float &rng_hgt_innov_var) const -{ - rng_hgt_innov_var = _rng_hgt_innov_var(2); -} - -void Ekf::getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const -{ - rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); -} - void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const { aux_vel_innov[0] = _aux_vel_innov(0); @@ -688,147 +658,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const aux_vel_innov_var[1] = _aux_vel_innov_var(1); } -void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) const -{ - aux_vel_innov_ratio = _aux_vel_test_ratio(0); -} - -void Ekf::getFlowInnov(float flow_innov[2]) const -{ - _flow_innov.copyTo(flow_innov); -} - -void Ekf::getFlowInnovVar(float flow_innov_var[2]) const -{ - _flow_innov_var.copyTo(flow_innov_var); -} - -void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const -{ - flow_innov_ratio = _optflow_test_ratio; -} - -Vector2f Ekf::getFlowVelBody() const -{ - return _flow_vel_body; -} - -Vector2f Ekf::getFlowVelNE() const -{ - return _flow_vel_ne; -} - -Vector2f Ekf::getFlowCompensated() const -{ - return _flow_compensated_XY_rad; -} - -Vector2f Ekf::getFlowUncompensated() const -{ - return _flow_sample_delayed.flow_xy_rad; -} - -Vector3f Ekf::getFlowGyro() const -{ - return _flow_sample_delayed.gyro_xyz; -} - -void Ekf::getHeadingInnov(float &heading_innov) const -{ - heading_innov = _heading_innov; -} - -void Ekf::getHeadingInnovVar(float &heading_innov_var) const -{ - heading_innov_var = _heading_innov_var; -} - -void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) const -{ - heading_innov_ratio = _yaw_test_ratio; -} - -void Ekf::getMagInnov(float mag_innov[3]) const -{ - _mag_innov.copyTo(mag_innov); -} - -void Ekf::getMagInnovVar(float mag_innov_var[3]) const -{ - _mag_innov_var.copyTo(mag_innov_var); -} - -void Ekf::getMagInnovRatio(float &mag_innov_ratio) const -{ - mag_innov_ratio = _mag_test_ratio.max(); -} - -void Ekf::getDragInnov(float drag_innov[2]) const -{ - _drag_innov.copyTo(drag_innov); -} - -void Ekf::getDragInnovVar(float drag_innov_var[2]) const -{ - _drag_innov_var.copyTo(drag_innov_var); -} - -void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) const -{ - _drag_test_ratio.copyTo(drag_innov_ratio); -} - -void Ekf::getAirspeedInnov(float &airspeed_innov) const -{ - airspeed_innov = _airspeed_innov; -} - -void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) const -{ - airspeed_innov_var = _airspeed_innov_var; -} - -void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) const -{ - airspeed_innov_ratio = _tas_test_ratio; -} - -void Ekf::getBetaInnov(float &beta_innov) const -{ - beta_innov = _beta_innov; -} - -void Ekf::getBetaInnovVar(float &beta_innov_var) const -{ - beta_innov_var = _beta_innov_var; -} - -void Ekf::getBetaInnovRatio(float &beta_innov_ratio) const -{ - beta_innov_ratio = _beta_test_ratio; -} - -void Ekf::getHaglInnov(float &hagl_innov) const -{ - hagl_innov = _hagl_innov; -} - -void Ekf::getHaglInnovVar(float &hagl_innov_var) const -{ - hagl_innov_var = _hagl_innov_var; -} - -void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) const -{ - hagl_innov_ratio = _hagl_test_ratio; -} - -// get GPS check status -void Ekf::get_gps_check_status(uint16_t *val) -{ - *val = _gps_check_fail_status.value; -} - // get the state vector at the delayed time horizon matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { @@ -844,16 +673,6 @@ matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const return state; } -Vector3f Ekf::getAccelBias() const -{ - return _state.delta_vel_bias / _dt_ekf_avg; -} - -Vector3f Ekf::getGyroBias() const -{ - return _state.delta_ang_bias / _dt_ekf_avg; -} - // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set // return true if the origin is valid bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) @@ -864,24 +683,6 @@ bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig return _NED_origin_initialised; } -// return a vector containing the output predictor angular, velocity and position tracking -// error magnitudes (rad), (m/s), (m) -Vector3f Ekf::getOutputTrackingError() const -{ - return _output_tracking_error; -} - -/* -Returns following IMU vibration metrics in the following array locations -0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) -1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) -2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) -*/ -Vector3f Ekf::getImuVibrationMetrics() const -{ - return _vibe_metrics; -} - /* First argument returns GPS drift metrics in the following array locations 0 : Horizontal position drift rate (m/s) @@ -1738,11 +1539,6 @@ bool Ekf::resetYawToEKFGSF() return true; } -void Ekf::requestEmergencyNavReset() -{ - _do_ekfgsf_yaw_reset = true; -} - bool Ekf::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]) { return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 26fb670db9..3e22564671 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2020 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 @@ -60,7 +60,7 @@ class EstimatorInterface { public: - EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){}; + EstimatorInterface(): _imu_down_sampler(FILTER_UPDATE_PERIOD_S) {}; virtual ~EstimatorInterface() = default; virtual bool init(uint64_t timestamp) = 0; @@ -91,11 +91,11 @@ class EstimatorInterface virtual void getFlowInnov(float flow_innov[2]) const = 0; virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0; virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0; - virtual Vector2f getFlowVelBody() const = 0; - virtual Vector2f getFlowVelNE() const = 0; - virtual Vector2f getFlowCompensated() const = 0; - virtual Vector2f getFlowUncompensated() const = 0; - virtual Vector3f getFlowGyro() const = 0; + virtual const Vector2f &getFlowVelBody() const = 0; + virtual const Vector2f &getFlowVelNE() const = 0; + virtual const Vector2f &getFlowCompensated() const = 0; + virtual const Vector2f &getFlowUncompensated() const = 0; + virtual const Vector3f &getFlowGyro() const = 0; virtual void getHeadingInnov(float &heading_innov) const = 0; virtual void getHeadingInnovVar(float &heading_innov_var) const = 0; @@ -181,15 +181,15 @@ class EstimatorInterface void setAirspeedData(const airspeedSample &airspeed_sample); - void setRangeData(const rangeSample& range_sample); + void setRangeData(const rangeSample &range_sample); // if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead - void setOpticalFlowData(const flowSample& flow); + void setOpticalFlowData(const flowSample &flow); // set external vision position and attitude data - void setExtVisionData(const extVisionSample& evdata); + void setExtVisionData(const extVisionSample &evdata); - void setAuxVelData(const auxVelSample& auxvel_sample); + void setAuxVelData(const auxVelSample &auxvel_sample); // return a address to the parameters struct // in order to give access to the application @@ -328,6 +328,7 @@ class EstimatorInterface bool get_mag_decl_deg(float *val) { *val = 0.0f; + if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) { *val = math::degrees(_mag_declination_gps); return true; @@ -407,7 +408,8 @@ class EstimatorInterface virtual void requestEmergencyNavReset() = 0; // get ekf-gsf debug data - virtual 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]) = 0; + virtual 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]) = 0; protected: @@ -591,5 +593,5 @@ class EstimatorInterface virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0; - void printBufferAllocationFailed(const char * buffer_name); + void printBufferAllocationFailed(const char *buffer_name); };