From 72d8f91b4d4e1911b4e92b451f6e7b417aea16e8 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 13 Feb 2020 20:51:29 +0100 Subject: [PATCH] Innovation getters: Add const modifier --- EKF/ekf.h | 72 +++++++++++++++++++------------------- EKF/ekf_helper.cpp | 72 +++++++++++++++++++------------------- EKF/estimator_interface.h | 73 +++++++++++++++++++-------------------- 3 files changed, 108 insertions(+), 109 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 9b81e26c6e..e110972daa 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -62,77 +62,77 @@ public: // should be called every time new data is pushed into the filter bool update() override; - void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) 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) 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) 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) 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) 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) override; + void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override; - void getBaroHgtInnov(float &baro_hgt_innov) override; + void getBaroHgtInnov(float &baro_hgt_innov) const override; - void getBaroHgtInnovVar(float &baro_hgt_innov_var) override; + void getBaroHgtInnovVar(float &baro_hgt_innov_var) const override; - void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) override; + void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override; - void getRngHgtInnov(float &rng_hgt_innov) override; + void getRngHgtInnov(float &rng_hgt_innov) const override; - void getRngHgtInnovVar(float &rng_hgt_innov_var) override; + void getRngHgtInnovVar(float &rng_hgt_innov_var) const override; - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) override; + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override; - void getAuxVelInnov(float aux_vel_innov[2]) override; + void getAuxVelInnov(float aux_vel_innov[2]) const override; - void getAuxVelInnovVar(float aux_vel_innov[2]) override; + void getAuxVelInnovVar(float aux_vel_innov[2]) const override; - void getAuxVelInnovRatio(float &aux_vel_innov_ratio) override; + void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override; - void getFlowInnov(float flow_innov[2]) override; + void getFlowInnov(float flow_innov[2]) const override; - void getFlowInnovVar(float flow_innov_var[2]) override; + void getFlowInnovVar(float flow_innov_var[2]) const override; - void getFlowInnovRatio(float &flow_innov_ratio) override; + void getFlowInnovRatio(float &flow_innov_ratio) const override; - void getHeadingInnov(float &heading_innov) override; + void getHeadingInnov(float &heading_innov) const override; - void getHeadingInnovVar(float &heading_innov_var) override; + void getHeadingInnovVar(float &heading_innov_var) const override; - void getHeadingInnovRatio(float &heading_innov_ratio) override; + void getHeadingInnovRatio(float &heading_innov_ratio) const override; - void getMagInnov(float mag_innov[3]) override; + void getMagInnov(float mag_innov[3]) const override; - void getMagInnovVar(float mag_innov_var[3]) override; + void getMagInnovVar(float mag_innov_var[3]) const override; - void getMagInnovRatio(float &mag_innov_ratio) override; + void getMagInnovRatio(float &mag_innov_ratio) const override; - void getDragInnov(float drag_innov[2]) override; + void getDragInnov(float drag_innov[2]) const override; - void getDragInnovVar(float drag_innov_var[2]) override; + void getDragInnovVar(float drag_innov_var[2]) const override; - void getDragInnovRatio(float drag_innov_ratio[2]) override; + void getDragInnovRatio(float drag_innov_ratio[2]) const override; - void getAirspeedInnov(float &airspeed_innov) override; + void getAirspeedInnov(float &airspeed_innov) const override; - void getAirspeedInnovVar(float &airspeed_innov_var) override; + void getAirspeedInnovVar(float &airspeed_innov_var) const override; - void getAirspeedInnovRatio(float &airspeed_innov_ratio) override; + void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override; - void getBetaInnov(float &beta_innov) override; + void getBetaInnov(float &beta_innov) const override; - void getBetaInnovVar(float &beta_innov_var) override; + void getBetaInnovVar(float &beta_innov_var) const override; - void getBetaInnovRatio(float &beta_innov_ratio) override; + void getBetaInnovRatio(float &beta_innov_ratio) const override; - void getHaglInnov(float &hagl_innov) override; + void getHaglInnov(float &hagl_innov) const override; - void getHaglInnovVar(float &hagl_innov_var) override; + void getHaglInnovVar(float &hagl_innov_var) const override; - void getHaglInnovRatio(float &hagl_innov_ratio) override; + void getHaglInnovRatio(float &hagl_innov_ratio) const override; // get the state vector at the delayed time horizon void get_state_delayed(float *state) override; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ec8c7b3162..6cd3bd4d14 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -661,7 +661,7 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad)); } -void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) +void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _gps_vel_innov(0); hvel[1] = _gps_vel_innov(1); @@ -671,7 +671,7 @@ void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &v vpos = _gps_pos_innov(2); } -void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) +void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _gps_vel_innov_var(0); hvel[1] = _gps_vel_innov_var(1); @@ -681,7 +681,7 @@ void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float vpos = _gps_pos_innov_var(2); } -void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) +void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const { hvel = _gps_vel_test_ratio(0); vvel = _gps_vel_test_ratio(1); @@ -689,7 +689,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v vpos = _gps_pos_test_ratio(1); } -void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) +void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _ev_vel_innov(0); hvel[1] = _ev_vel_innov(1); @@ -699,7 +699,7 @@ void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpo vpos = _ev_pos_innov(2); } -void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) +void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const { hvel[0] = _ev_vel_innov_var(0); hvel[1] = _ev_vel_innov_var(1); @@ -709,7 +709,7 @@ void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float & vpos = _ev_pos_innov_var(2); } -void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) +void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const { hvel = _ev_vel_test_ratio(0); vvel = _ev_vel_test_ratio(1); @@ -717,154 +717,154 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp vpos = _ev_pos_test_ratio(1); } -void Ekf::getBaroHgtInnov(float &baro_hgt_innov) +void Ekf::getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); } -void Ekf::getBaroHgtInnovVar(float &baro_hgt_innov_var) +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) +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) +void Ekf::getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); } -void Ekf::getRngHgtInnovVar(float &rng_hgt_innov_var) +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) +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]) +void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const { aux_vel_innov[0] = _aux_vel_innov(0); aux_vel_innov[1] = _aux_vel_innov(1); } -void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) +void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const { aux_vel_innov_var[0] = _aux_vel_innov_var(0); aux_vel_innov_var[1] = _aux_vel_innov_var(1); } -void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) +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]) +void Ekf::getFlowInnov(float flow_innov[2]) const { memcpy(flow_innov, _flow_innov, sizeof(_flow_innov)); } -void Ekf::getFlowInnovVar(float flow_innov_var[2]) +void Ekf::getFlowInnovVar(float flow_innov_var[2]) const { memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var)); } -void Ekf::getFlowInnovRatio(float &flow_innov_ratio) +void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; } -void Ekf::getHeadingInnov(float &heading_innov) +void Ekf::getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } -void Ekf::getHeadingInnovVar(float &heading_innov_var) +void Ekf::getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } -void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) +void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; } -void Ekf::getMagInnov(float mag_innov[3]) +void Ekf::getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _mag_innov, sizeof(_mag_innov)); } -void Ekf::getMagInnovVar(float mag_innov_var[3]) +void Ekf::getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _mag_innov_var, sizeof(_mag_innov_var)); } -void Ekf::getMagInnovRatio(float &mag_innov_ratio) +void Ekf::getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2]); } -void Ekf::getDragInnov(float drag_innov[2]) +void Ekf::getDragInnov(float drag_innov[2]) const { memcpy(drag_innov, _drag_innov, sizeof(_drag_innov)); } -void Ekf::getDragInnovVar(float drag_innov_var[2]) +void Ekf::getDragInnovVar(float drag_innov_var[2]) const { memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var)); } -void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) +void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) const { memcpy(drag_innov_ratio, &_drag_test_ratio, sizeof(_drag_test_ratio)); } -void Ekf::getAirspeedInnov(float &airspeed_innov) +void Ekf::getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; } -void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) +void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; } -void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) +void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; } -void Ekf::getBetaInnov(float &beta_innov) +void Ekf::getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; } -void Ekf::getBetaInnovVar(float &beta_innov_var) +void Ekf::getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; } -void Ekf::getBetaInnovRatio(float &beta_innov_ratio) +void Ekf::getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; } -void Ekf::getHaglInnov(float &hagl_innov) +void Ekf::getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } -void Ekf::getHaglInnovVar(float &hagl_innov_var) +void Ekf::getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } -void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) +void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 2803421459..ada016d581 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -68,54 +68,53 @@ public: virtual bool update() = 0; - virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; - virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; - virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0; + virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; + virtual void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; + virtual void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0; - virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; - virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0; - virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) = 0; + virtual void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; + virtual void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const = 0; + virtual void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const = 0; - virtual void getBaroHgtInnov(float &baro_hgt_innov) = 0; - virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) = 0; - virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) = 0; + virtual void getBaroHgtInnov(float &baro_hgt_innov) const = 0; + virtual void getBaroHgtInnovVar(float &baro_hgt_innov_var) const = 0; + virtual void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const = 0; - virtual void getRngHgtInnov(float &rng_hgt_innov) = 0; - virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) = 0; - virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) = 0; + virtual void getRngHgtInnov(float &rng_hgt_innov) const = 0; + virtual void getRngHgtInnovVar(float &rng_hgt_innov_var) const = 0; + virtual void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const = 0; - virtual void getAuxVelInnov(float aux_vel_innov[2]) = 0; - virtual void getAuxVelInnovVar(float aux_vel_innov[2]) = 0; - virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) = 0; + virtual void getAuxVelInnov(float aux_vel_innov[2]) const = 0; + virtual void getAuxVelInnovVar(float aux_vel_innov[2]) const = 0; + virtual void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const = 0; - virtual void getFlowInnov(float flow_innov[2]) = 0; - virtual void getFlowInnovVar(float flow_innov_var[2]) = 0; - virtual void getFlowInnovRatio(float &flow_innov_ratio) = 0; + 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 void getHeadingInnov(float &heading_innov) = 0; - virtual void getHeadingInnovVar(float &heading_innov_var) = 0; - virtual void getHeadingInnovRatio(float &heading_innov_ratio) = 0; + virtual void getHeadingInnov(float &heading_innov) const = 0; + virtual void getHeadingInnovVar(float &heading_innov_var) const = 0; + virtual void getHeadingInnovRatio(float &heading_innov_ratio) const = 0; - virtual void getMagInnov(float mag_innov[3]) = 0; - virtual void getMagInnovVar(float mag_innov_var[3]) = 0; - virtual void getMagInnovRatio(float &mag_innov_ratio) = 0; + virtual void getMagInnov(float mag_innov[3]) const = 0; + virtual void getMagInnovVar(float mag_innov_var[3]) const = 0; + virtual void getMagInnovRatio(float &mag_innov_ratio) const = 0; - virtual void getDragInnov(float drag_innov[2]) = 0; - virtual void getDragInnovVar(float drag_innov_var[2]) = 0; - virtual void getDragInnovRatio(float drag_innov_ratio[2]) = 0; + virtual void getDragInnov(float drag_innov[2]) const = 0; + virtual void getDragInnovVar(float drag_innov_var[2]) const = 0; + virtual void getDragInnovRatio(float drag_innov_ratio[2]) const = 0; - virtual void getAirspeedInnov(float &airspeed_innov) = 0; - virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) = 0; - virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) = 0; + virtual void getAirspeedInnov(float &airspeed_innov) const = 0; + virtual void getAirspeedInnovVar(float &get_airspeed_innov_var) const = 0; + virtual void getAirspeedInnovRatio(float &airspeed_innov_ratio) const = 0; - virtual void getBetaInnov(float &beta_innov) = 0; - virtual void getBetaInnovVar(float &get_beta_innov_var) = 0; - virtual void getBetaInnovRatio(float &beta_innov_ratio) = 0; - - virtual void getHaglInnov(float &hagl_innov) = 0; - virtual void getHaglInnovVar(float &hagl_innov_var) = 0; - virtual void getHaglInnovRatio(float &hagl_innov_ratio) = 0; + virtual void getBetaInnov(float &beta_innov) const = 0; + virtual void getBetaInnovVar(float &get_beta_innov_var) const = 0; + virtual void getBetaInnovRatio(float &beta_innov_ratio) const = 0; + virtual void getHaglInnov(float &hagl_innov) const = 0; + virtual void getHaglInnovVar(float &hagl_innov_var) const = 0; + virtual void getHaglInnovRatio(float &hagl_innov_ratio) const = 0; virtual void get_state_delayed(float *state) = 0;