Innovation getters: Add const modifier

This commit is contained in:
kamilritz 2020-02-13 20:51:29 +01:00 committed by Mathieu Bresciani
parent 89b5c77d5d
commit 72d8f91b4d
3 changed files with 108 additions and 109 deletions

View File

@ -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;

View File

@ -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;
}

View File

@ -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;