forked from Archive/PX4-Autopilot
ekf2: delete redundant aid src status getters
This commit is contained in:
Daniel Agar
GitHub
parent
bdaf0acfca
commit
408c30de13
|
|
@ -19,7 +19,6 @@ float32 baro_vpos # barometer height innovation (m) and innovation variance (m**
|
|||
|
||||
# Auxiliary velocity
|
||||
float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
|
||||
float32 aux_vvel # vertical auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2)
|
||||
|
||||
# Optical flow
|
||||
float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
|
||||
|
|
|
|||
|
|
@ -83,19 +83,9 @@ public:
|
|||
|
||||
static uint8_t getNumberOfStates() { return State::size; }
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
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;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
|
||||
const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
|
||||
|
||||
void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
|
||||
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
|
||||
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
|
|
@ -115,28 +105,10 @@ public:
|
|||
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; }
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; }
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; }
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
|
||||
|
||||
void getTerrainFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void getTerrainFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
|
|
@ -146,32 +118,14 @@ public:
|
|||
const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
|
||||
const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
|
||||
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }
|
||||
|
||||
void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
|
||||
void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
|
||||
void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
|
||||
float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); }
|
||||
float getHaglRateInnovVar() const { return _rng_consistency_check.getInnovVar(); }
|
||||
float getHaglRateInnovRatio() const { return _rng_consistency_check.getSignedTestRatioLpf(); }
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
|
||||
|
||||
void getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
flow_innov[0] = _aid_src_optical_flow.innovation[0];
|
||||
flow_innov[1] = _aid_src_optical_flow.innovation[1];
|
||||
}
|
||||
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0];
|
||||
flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1];
|
||||
}
|
||||
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); }
|
||||
|
||||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
|
||||
|
||||
|
|
@ -185,140 +139,93 @@ public:
|
|||
const Vector3f &getMeasuredBodyRate() const { return _measured_body_rate; }
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
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 = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const
|
||||
float getHeadingInnov() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov = _aid_src_mag_heading.innovation;
|
||||
return;
|
||||
return _aid_src_mag_heading.innovation;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_3D) {
|
||||
heading_innov = Vector3f(_aid_src_mag.innovation).max();
|
||||
return;
|
||||
if (_control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.innovation).max();
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
heading_innov = _aid_src_gnss_yaw.innovation;
|
||||
return;
|
||||
return _aid_src_gnss_yaw.innovation;
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov = _aid_src_ev_yaw.innovation;
|
||||
return;
|
||||
return _aid_src_ev_yaw.innovation;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
void getHeadingInnovVar(float &heading_innov_var) const
|
||||
float getHeadingInnovVar() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov_var = _aid_src_mag_heading.innovation_variance;
|
||||
return;
|
||||
return _aid_src_mag_heading.innovation_variance;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_3D) {
|
||||
heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
|
||||
return;
|
||||
if (_control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.innovation_variance).max();
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
|
||||
return;
|
||||
return _aid_src_gnss_yaw.innovation_variance;
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov_var = _aid_src_ev_yaw.innovation_variance;
|
||||
return;
|
||||
return _aid_src_ev_yaw.innovation_variance;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const
|
||||
float getHeadingInnovRatio() const
|
||||
{
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
if (_control_status.flags.mag_hdg) {
|
||||
heading_innov_ratio = _aid_src_mag_heading.test_ratio;
|
||||
return;
|
||||
return _aid_src_mag_heading.test_ratio;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_3D) {
|
||||
heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
|
||||
return;
|
||||
if (_control_status.flags.mag_3D) {
|
||||
return Vector3f(_aid_src_mag.test_ratio).max();
|
||||
}
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
if (_control_status.flags.gps_yaw) {
|
||||
heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
|
||||
return;
|
||||
return _aid_src_gnss_yaw.test_ratio;
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
|
||||
return;
|
||||
return _aid_src_ev_yaw.test_ratio;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
|
||||
void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
|
||||
void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
const auto &aid_src_drag() const { return _aid_src_drag; }
|
||||
|
||||
void getDragInnov(float drag_innov[2]) const
|
||||
{
|
||||
drag_innov[0] = _aid_src_drag.innovation[0];
|
||||
drag_innov[1] = _aid_src_drag.innovation[1];
|
||||
}
|
||||
void getDragInnovVar(float drag_innov_var[2]) const
|
||||
{
|
||||
drag_innov_var[0] = _aid_src_drag.innovation_variance[0];
|
||||
drag_innov_var[1] = _aid_src_drag.innovation_variance[1];
|
||||
}
|
||||
void getDragInnovRatio(float drag_innov_ratio[2]) const
|
||||
{
|
||||
drag_innov_ratio[0] = _aid_src_drag.test_ratio[0];
|
||||
drag_innov_ratio[1] = _aid_src_drag.test_ratio[1];
|
||||
}
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
|
||||
void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
|
||||
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
|
||||
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
|
||||
const auto &aid_src_gravity() const { return _aid_src_gravity; }
|
||||
|
||||
void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
|
||||
void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
|
||||
void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); }
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
|
|
@ -497,12 +404,6 @@ public:
|
|||
|
||||
uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
|
||||
|
||||
const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
|
@ -519,13 +420,12 @@ public:
|
|||
const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
|
||||
const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
|
||||
const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
|
||||
|
||||
const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }
|
||||
const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
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;
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
bool collect_gps(const gpsMessage &gps) override;
|
||||
|
||||
|
|
|
|||
|
|
@ -295,86 +295,6 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const
|
|||
-CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad));
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
void Ekf::getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _aid_src_gnss_vel.innovation[0];
|
||||
hvel[1] = _aid_src_gnss_vel.innovation[1];
|
||||
vvel = _aid_src_gnss_vel.innovation[2];
|
||||
|
||||
hpos[0] = _aid_src_gnss_pos.innovation[0];
|
||||
hpos[1] = _aid_src_gnss_pos.innovation[1];
|
||||
vpos = _aid_src_gnss_hgt.innovation;
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _aid_src_gnss_vel.innovation_variance[0];
|
||||
hvel[1] = _aid_src_gnss_vel.innovation_variance[1];
|
||||
vvel = _aid_src_gnss_vel.innovation_variance[2];
|
||||
|
||||
hpos[0] = _aid_src_gnss_pos.innovation_variance[0];
|
||||
hpos[1] = _aid_src_gnss_pos.innovation_variance[1];
|
||||
vpos = _aid_src_gnss_hgt.innovation_variance;
|
||||
}
|
||||
|
||||
void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
||||
{
|
||||
hvel = fmaxf(_aid_src_gnss_vel.test_ratio[0], _aid_src_gnss_vel.test_ratio[1]);
|
||||
vvel = _aid_src_gnss_vel.test_ratio[2];
|
||||
|
||||
hpos = fmaxf(_aid_src_gnss_pos.test_ratio[0], _aid_src_gnss_pos.test_ratio[1]);
|
||||
vpos = _aid_src_gnss_hgt.test_ratio;
|
||||
}
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _aid_src_ev_vel.innovation[0];
|
||||
hvel[1] = _aid_src_ev_vel.innovation[1];
|
||||
vvel = _aid_src_ev_vel.innovation[2];
|
||||
|
||||
hpos[0] = _aid_src_ev_pos.innovation[0];
|
||||
hpos[1] = _aid_src_ev_pos.innovation[1];
|
||||
vpos = _aid_src_ev_hgt.innovation;
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const
|
||||
{
|
||||
hvel[0] = _aid_src_ev_vel.innovation_variance[0];
|
||||
hvel[1] = _aid_src_ev_vel.innovation_variance[1];
|
||||
vvel = _aid_src_ev_vel.innovation_variance[2];
|
||||
|
||||
hpos[0] = _aid_src_ev_pos.innovation_variance[0];
|
||||
hpos[1] = _aid_src_ev_pos.innovation_variance[1];
|
||||
vpos = _aid_src_ev_hgt.innovation_variance;
|
||||
}
|
||||
|
||||
void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const
|
||||
{
|
||||
hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]);
|
||||
vvel = _aid_src_ev_vel.test_ratio[2];
|
||||
|
||||
hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
|
||||
vpos = _aid_src_ev_hgt.test_ratio;
|
||||
}
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
||||
{
|
||||
aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
|
||||
aux_vel_innov[1] = _aid_src_aux_vel.innovation[1];
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
||||
{
|
||||
aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
|
||||
aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
|
||||
}
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
|
||||
{
|
||||
origin_time = _pos_ref.getProjectionReferenceTimestamp();
|
||||
|
|
|
|||
|
|
@ -1406,53 +1406,93 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp)
|
|||
// publish estimator innovation data
|
||||
estimator_innovations_s innovations{};
|
||||
innovations.timestamp_sample = _ekf.time_delayed_us();
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
|
||||
// GPS
|
||||
innovations.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation[0];
|
||||
innovations.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation[1];
|
||||
innovations.gps_vvel = _ekf.aid_src_gnss_vel().innovation[2];
|
||||
innovations.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation[0];
|
||||
innovations.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation[1];
|
||||
innovations.gps_vpos = _ekf.aid_src_gnss_hgt().innovation;
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
|
||||
// External Vision
|
||||
innovations.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation[0];
|
||||
innovations.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation[1];
|
||||
innovations.ev_vvel = _ekf.aid_src_ev_vel().innovation[2];
|
||||
innovations.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation[0];
|
||||
innovations.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation[1];
|
||||
innovations.ev_vpos = _ekf.aid_src_ev_hgt().innovation;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnov(innovations.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
// Height sensors
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnov(innovations.rng_vpos);
|
||||
innovations.rng_vpos = _ekf.aid_src_rng_hgt().innovation;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
innovations.baro_vpos = _ekf.aid_src_baro_hgt().innovation;
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnov(innovations.aux_hvel);
|
||||
// Auxiliary velocity
|
||||
innovations.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation[0];
|
||||
innovations.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation[1];
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnov(innovations.flow);
|
||||
// Optical flow
|
||||
innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0];
|
||||
innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1];
|
||||
# if defined(CONFIG_EKF2_TERRAIN)
|
||||
innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0];
|
||||
innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1];
|
||||
# endif // CONFIG_EKF2_TERRAIN
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnov(innovations.heading);
|
||||
|
||||
// heading
|
||||
innovations.heading = _ekf.getHeadingInnov();
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnov(innovations.mag_field);
|
||||
// mag_field
|
||||
innovations.mag_field[0] = _ekf.aid_src_mag().innovation[0];
|
||||
innovations.mag_field[1] = _ekf.aid_src_mag().innovation[1];
|
||||
innovations.mag_field[2] = _ekf.aid_src_mag().innovation[2];
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnov(innovations.drag);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
_ekf.getAirspeedInnov(innovations.airspeed);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnov(innovations.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
|
||||
_ekf.getGravityInnov(innovations.gravity);
|
||||
// gravity
|
||||
innovations.gravity[0] = _ekf.aid_src_gravity().innovation[0];
|
||||
innovations.gravity[1] = _ekf.aid_src_gravity().innovation[1];
|
||||
innovations.gravity[2] = _ekf.aid_src_gravity().innovation[2];
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnov(innovations.hagl);
|
||||
# endif //CONFIG_EKF2_RANGE_FINDER
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnov(innovations.terr_flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
// drag
|
||||
innovations.drag[0] = _ekf.aid_src_drag().innovation[0];
|
||||
innovations.drag[1] = _ekf.aid_src_drag().innovation[1];
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
// Not yet supported
|
||||
innovations.aux_vvel = NAN;
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
// airspeed
|
||||
innovations.airspeed = _ekf.aid_src_airspeed().innovation;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
// beta
|
||||
innovations.beta = _ekf.aid_src_sideslip().innovation;
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl
|
||||
innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation;
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl_rate
|
||||
innovations.hagl_rate = _ekf.getHaglRateInnov();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_innovations_pub.publish(innovations);
|
||||
|
|
@ -1500,55 +1540,93 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
|
|||
// publish estimator innovation test ratio data
|
||||
estimator_innovations_s test_ratios{};
|
||||
test_ratios.timestamp_sample = _ekf.time_delayed_us();
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
|
||||
test_ratios.gps_vpos);
|
||||
// GPS
|
||||
test_ratios.gps_hvel[0] = _ekf.aid_src_gnss_vel().test_ratio[0];
|
||||
test_ratios.gps_hvel[1] = _ekf.aid_src_gnss_vel().test_ratio[1];
|
||||
test_ratios.gps_vvel = _ekf.aid_src_gnss_vel().test_ratio[2];
|
||||
test_ratios.gps_hpos[0] = _ekf.aid_src_gnss_pos().test_ratio[0];
|
||||
test_ratios.gps_hpos[1] = _ekf.aid_src_gnss_pos().test_ratio[1];
|
||||
test_ratios.gps_vpos = _ekf.aid_src_gnss_hgt().test_ratio;
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
|
||||
// External Vision
|
||||
test_ratios.ev_hvel[0] = _ekf.aid_src_ev_vel().test_ratio[0];
|
||||
test_ratios.ev_hvel[1] = _ekf.aid_src_ev_vel().test_ratio[1];
|
||||
test_ratios.ev_vvel = _ekf.aid_src_ev_vel().test_ratio[2];
|
||||
test_ratios.ev_hpos[0] = _ekf.aid_src_ev_pos().test_ratio[0];
|
||||
test_ratios.ev_hpos[1] = _ekf.aid_src_ev_pos().test_ratio[1];
|
||||
test_ratios.ev_vpos = _ekf.aid_src_ev_hgt().test_ratio;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
// Height sensors
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
|
||||
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
|
||||
test_ratios.rng_vpos = _ekf.aid_src_rng_hgt().test_ratio;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
test_ratios.baro_vpos = _ekf.aid_src_baro_hgt().test_ratio;
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
|
||||
// Auxiliary velocity
|
||||
test_ratios.aux_hvel[0] = _ekf.aid_src_aux_vel().test_ratio[0];
|
||||
test_ratios.aux_hvel[1] = _ekf.aid_src_aux_vel().test_ratio[1];
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
|
||||
// Optical flow
|
||||
test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0];
|
||||
test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1];
|
||||
# if defined(CONFIG_EKF2_TERRAIN)
|
||||
test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0];
|
||||
test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1];
|
||||
# endif // CONFIG_EKF2_TERRAIN
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovRatio(test_ratios.heading);
|
||||
|
||||
// heading
|
||||
test_ratios.heading = _ekf.getHeadingInnovRatio();
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
|
||||
// mag_field
|
||||
test_ratios.mag_field[0] = _ekf.aid_src_mag().test_ratio[0];
|
||||
test_ratios.mag_field[1] = _ekf.aid_src_mag().test_ratio[1];
|
||||
test_ratios.mag_field[2] = _ekf.aid_src_mag().test_ratio[2];
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnovRatio(test_ratios.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
|
||||
_ekf.getGravityInnovRatio(test_ratios.gravity[0]);
|
||||
// gravity
|
||||
test_ratios.gravity[0] = _ekf.aid_src_gravity().test_ratio[0];
|
||||
test_ratios.gravity[1] = _ekf.aid_src_gravity().test_ratio[1];
|
||||
test_ratios.gravity[2] = _ekf.aid_src_gravity().test_ratio[2];
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnovRatio(test_ratios.hagl);
|
||||
# endif
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
// drag
|
||||
test_ratios.drag[0] = _ekf.aid_src_drag().test_ratio[0];
|
||||
test_ratios.drag[1] = _ekf.aid_src_drag().test_ratio[1];
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
// Not yet supported
|
||||
test_ratios.aux_vvel = NAN;
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
// airspeed
|
||||
test_ratios.airspeed = _ekf.aid_src_airspeed().test_ratio;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
// beta
|
||||
test_ratios.beta = _ekf.aid_src_sideslip().test_ratio;
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl
|
||||
test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio;
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl_rate
|
||||
test_ratios.hagl_rate = _ekf.getHaglRateInnovRatio();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_innovation_test_ratios_pub.publish(test_ratios);
|
||||
|
|
@ -1559,54 +1637,93 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
|
|||
// publish estimator innovation variance data
|
||||
estimator_innovations_s variances{};
|
||||
variances.timestamp_sample = _ekf.time_delayed_us();
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getRngHgtInnovVar(variances.rng_vpos);
|
||||
_ekf.getHaglRateInnovVar(variances.hagl_rate);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
_ekf.getAuxVelInnovVar(variances.aux_hvel);
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getFlowInnovVar(variances.flow);
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
_ekf.getHeadingInnovVar(variances.heading);
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
_ekf.getMagInnovVar(variances.mag_field);
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
_ekf.getDragInnovVar(variances.drag);
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
_ekf.getAirspeedInnovVar(variances.airspeed);
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnovVar(variances.beta);
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN)
|
||||
# if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
_ekf.getHaglInnovVar(variances.hagl);
|
||||
# endif // CONFIG_EKF2_RANGE_FINDER
|
||||
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
_ekf.getTerrainFlowInnovVar(variances.terr_flow);
|
||||
# endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
// GPS
|
||||
variances.gps_hvel[0] = _ekf.aid_src_gnss_vel().innovation_variance[0];
|
||||
variances.gps_hvel[1] = _ekf.aid_src_gnss_vel().innovation_variance[1];
|
||||
variances.gps_vvel = _ekf.aid_src_gnss_vel().innovation_variance[2];
|
||||
variances.gps_hpos[0] = _ekf.aid_src_gnss_pos().innovation_variance[0];
|
||||
variances.gps_hpos[1] = _ekf.aid_src_gnss_pos().innovation_variance[1];
|
||||
variances.gps_vpos = _ekf.aid_src_gnss_hgt().innovation_variance;
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
// External Vision
|
||||
variances.ev_hvel[0] = _ekf.aid_src_ev_vel().innovation_variance[0];
|
||||
variances.ev_hvel[1] = _ekf.aid_src_ev_vel().innovation_variance[1];
|
||||
variances.ev_vvel = _ekf.aid_src_ev_vel().innovation_variance[2];
|
||||
variances.ev_hpos[0] = _ekf.aid_src_ev_pos().innovation_variance[0];
|
||||
variances.ev_hpos[1] = _ekf.aid_src_ev_pos().innovation_variance[1];
|
||||
variances.ev_vpos = _ekf.aid_src_ev_hgt().innovation_variance;
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
// Height sensors
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
variances.rng_vpos = _ekf.aid_src_rng_hgt().innovation_variance;
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
#if defined(CONFIG_EKF2_BAROMETER)
|
||||
variances.baro_vpos = _ekf.aid_src_baro_hgt().innovation_variance;
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_AUXVEL)
|
||||
// Auxiliary velocity
|
||||
variances.aux_hvel[0] = _ekf.aid_src_aux_vel().innovation_variance[0];
|
||||
variances.aux_hvel[1] = _ekf.aid_src_aux_vel().innovation_variance[1];
|
||||
#endif // CONFIG_EKF2_AUXVEL
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
// Optical flow
|
||||
variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0];
|
||||
variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1];
|
||||
# if defined(CONFIG_EKF2_TERRAIN)
|
||||
variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0];
|
||||
variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1];
|
||||
# endif // CONFIG_EKF2_TERRAIN
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
// heading
|
||||
variances.heading = _ekf.getHeadingInnovVar();
|
||||
|
||||
#if defined(CONFIG_EKF2_MAGNETOMETER)
|
||||
// mag_field
|
||||
variances.mag_field[0] = _ekf.aid_src_mag().innovation_variance[0];
|
||||
variances.mag_field[1] = _ekf.aid_src_mag().innovation_variance[1];
|
||||
variances.mag_field[2] = _ekf.aid_src_mag().innovation_variance[2];
|
||||
#endif // CONFIG_EKF2_MAGNETOMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GRAVITY_FUSION)
|
||||
_ekf.getGravityInnovVar(variances.gravity);
|
||||
// gravity
|
||||
variances.gravity[0] = _ekf.aid_src_gravity().innovation_variance[0];
|
||||
variances.gravity[1] = _ekf.aid_src_gravity().innovation_variance[1];
|
||||
variances.gravity[2] = _ekf.aid_src_gravity().innovation_variance[2];
|
||||
#endif // CONFIG_EKF2_GRAVITY_FUSION
|
||||
|
||||
// Not yet supported
|
||||
variances.aux_vvel = NAN;
|
||||
#if defined(CONFIG_EKF2_DRAG_FUSION)
|
||||
// drag
|
||||
variances.drag[0] = _ekf.aid_src_drag().innovation_variance[0];
|
||||
variances.drag[1] = _ekf.aid_src_drag().innovation_variance[1];
|
||||
#endif // CONFIG_EKF2_DRAG_FUSION
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
// airspeed
|
||||
variances.airspeed = _ekf.aid_src_airspeed().innovation_variance;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
// beta
|
||||
variances.beta = _ekf.aid_src_sideslip().innovation_variance;
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl
|
||||
variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance;
|
||||
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
// hagl_rate
|
||||
variances.hagl_rate = _ekf.getHaglRateInnovVar();
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
|
||||
_estimator_innovation_variances_pub.publish(variances);
|
||||
|
|
@ -2043,12 +2160,12 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
|
|||
const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
|
||||
|
||||
#if defined(CONFIG_EKF2_AIRSPEED)
|
||||
_ekf.getAirspeedInnov(wind.tas_innov);
|
||||
_ekf.getAirspeedInnovVar(wind.tas_innov_var);
|
||||
wind.tas_innov = _ekf.aid_src_airspeed().innovation;
|
||||
wind.tas_innov_var = _ekf.aid_src_airspeed().innovation_variance;
|
||||
#endif // CONFIG_EKF2_AIRSPEED
|
||||
#if defined(CONFIG_EKF2_SIDESLIP)
|
||||
_ekf.getBetaInnov(wind.beta_innov);
|
||||
_ekf.getBetaInnovVar(wind.beta_innov_var);
|
||||
wind.beta_innov = _ekf.aid_src_sideslip().innovation;
|
||||
wind.beta_innov = _ekf.aid_src_sideslip().innovation_variance;
|
||||
#endif // CONFIG_EKF2_SIDESLIP
|
||||
|
||||
wind.windspeed_north = wind_vel(0);
|
||||
|
|
|
|||
|
|
@ -245,22 +245,16 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized)
|
|||
|
||||
_sensor_simulator.runSeconds(1);
|
||||
|
||||
float hpos = 0.f;
|
||||
float vpos = 0.f;
|
||||
float hvel = 0.f;
|
||||
float vvel = 0.f;
|
||||
float baro_vpos = 0.f;
|
||||
|
||||
// After the change of origin, the pos and vel innovations should stay small
|
||||
_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
|
||||
_ekf->getBaroHgtInnovRatio(baro_vpos);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
|
||||
|
||||
EXPECT_NEAR(hpos, 0.f, 0.05f);
|
||||
EXPECT_NEAR(vpos, 0.f, 0.05f);
|
||||
EXPECT_NEAR(baro_vpos, 0.f, 0.05f);
|
||||
EXPECT_NEAR(_ekf->aid_src_baro_hgt().test_ratio, 0.f, 0.05f);
|
||||
|
||||
EXPECT_NEAR(hvel, 0.f, 0.02f);
|
||||
EXPECT_NEAR(vvel, 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
|
||||
}
|
||||
|
||||
TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
|
||||
|
|
@ -291,19 +285,14 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized)
|
|||
|
||||
_sensor_simulator.runSeconds(1);
|
||||
|
||||
float hpos = 0.f;
|
||||
float vpos = 0.f;
|
||||
float hvel = 0.f;
|
||||
float vvel = 0.f;
|
||||
|
||||
// After the change of origin, the pos and vel innovations should stay small
|
||||
_ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[0], 0.f, 0.05f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_pos().test_ratio[1], 0.f, 0.05f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_hgt().test_ratio, 0.f, 0.05f);
|
||||
|
||||
EXPECT_NEAR(hpos, 0.f, 0.05f);
|
||||
EXPECT_NEAR(vpos, 0.f, 0.05f);
|
||||
|
||||
EXPECT_NEAR(hvel, 0.f, 0.02f);
|
||||
EXPECT_NEAR(vvel, 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[0], 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[1], 0.f, 0.02f);
|
||||
EXPECT_NEAR(_ekf->aid_src_gnss_vel().test_ratio[2], 0.f, 0.02f);
|
||||
}
|
||||
|
||||
TEST_F(EkfBasicsTest, global_position_from_local_ev)
|
||||
|
|
|
|||
|
|
@ -222,8 +222,7 @@ TEST_F(EkfGpsTest, altitudeDrift)
|
|||
_sensor_simulator.runSeconds(dt);
|
||||
}
|
||||
|
||||
float baro_innov;
|
||||
_ekf->getBaroHgtInnov(baro_innov);
|
||||
float baro_innov = _ekf->aid_src_baro_hgt().innovation;
|
||||
BiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
|
||||
|
||||
printf("baro innov = %f\n", (double)baro_innov);
|
||||
|
|
|
|||
|
|
@ -198,13 +198,8 @@ TEST_F(EkfHeightFusionTest, gpsRef)
|
|||
EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f);
|
||||
|
||||
// and the innovations are close to zero
|
||||
float baro_innov = NAN;
|
||||
_ekf->getBaroHgtInnov(baro_innov);
|
||||
EXPECT_NEAR(baro_innov, 0.f, 0.2f);
|
||||
|
||||
float rng_innov = NAN;
|
||||
_ekf->getRngHgtInnov(rng_innov);
|
||||
EXPECT_NEAR(rng_innov, 0.f, 0.2f);
|
||||
EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f);
|
||||
EXPECT_NEAR(_ekf->aid_src_rng_hgt().innovation, 0.f, 0.2f);
|
||||
}
|
||||
|
||||
TEST_F(EkfHeightFusionTest, baroRefFailOver)
|
||||
|
|
|
|||
Loading…
Reference in New Issue