forked from Archive/PX4-Autopilot
EKF: remove virtual getters from estimator_interface
This commit is contained in:
Daniel Agar
parent
48a8992caf
commit
a21092804a
|
|
@ -161,12 +161,7 @@ void Ekf::fuseAirspeed()
|
|||
}
|
||||
}
|
||||
|
||||
Vector2f Ekf::getWindVelocityVariance() const
|
||||
{
|
||||
return P.slice<2, 2>(22,22).diag();
|
||||
}
|
||||
|
||||
void Ekf::get_true_airspeed(float *tas)
|
||||
void Ekf::get_true_airspeed(float *tas) const
|
||||
{
|
||||
const float tempvar = sqrtf(sq(_state.vel(0) - _state.wind_vel(0)) + sq(_state.vel(1) - _state.wind_vel(1)) + sq(_state.vel(2)));
|
||||
memcpy(tas, &tempvar, sizeof(float));
|
||||
|
|
|
|||
|
|
@ -77,7 +77,7 @@ void Ekf::controlFusionModes()
|
|||
height_source = "unknown";
|
||||
|
||||
}
|
||||
if(height_source){
|
||||
if (height_source){
|
||||
ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)",
|
||||
(unsigned long long)_imu_sample_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1034,7 +1034,6 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP) {
|
|||
return healthy;
|
||||
}
|
||||
|
||||
|
||||
void Ekf::resetMagRelatedCovariances()
|
||||
{
|
||||
resetQuatCov();
|
||||
|
|
@ -1119,6 +1118,5 @@ void Ekf::resetWindCovariance()
|
|||
} else {
|
||||
// without airspeed, start with a small initial uncertainty to improve the initial estimate
|
||||
P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
201
EKF/ekf.h
201
EKF/ekf.h
|
|
@ -44,6 +44,8 @@
|
|||
|
||||
#include "estimator_interface.h"
|
||||
|
||||
#include "EKFGSF_yaw.h"
|
||||
|
||||
class Ekf final : public EstimatorInterface
|
||||
{
|
||||
public:
|
||||
|
|
@ -61,78 +63,73 @@ public:
|
|||
// initialise variables to sane values (also interface class)
|
||||
bool init(uint64_t timestamp) override;
|
||||
|
||||
// set the internal states and status to their default value
|
||||
void reset() override;
|
||||
|
||||
bool initialiseTilt();
|
||||
|
||||
// should be called every time new data is pushed into the filter
|
||||
bool update() override;
|
||||
bool update();
|
||||
|
||||
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 getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
|
||||
|
||||
void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const 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 getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
|
||||
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
|
||||
|
||||
void getBaroHgtInnov(float &baro_hgt_innov) const 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 getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _baro_hgt_innov(2); }
|
||||
void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _baro_hgt_innov_var(2); }
|
||||
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _baro_hgt_test_ratio(1); }
|
||||
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const 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 getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _rng_hgt_innov(2); }
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
|
||||
|
||||
void getAuxVelInnov(float aux_vel_innov[2]) const 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 getAuxVelInnov(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovVar(float aux_vel_innov[2]) const;
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
|
||||
|
||||
void getFlowInnov(float flow_innov[2]) const 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 getFlowInnov(float flow_innov[2]) const { _flow_innov.copyTo(flow_innov); }
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const { _flow_innov_var.copyTo(flow_innov_var); }
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = _optflow_test_ratio; }
|
||||
const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
|
||||
const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
|
||||
const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; }
|
||||
const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; }
|
||||
const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; }
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const override { heading_innov = _heading_innov; }
|
||||
void getHeadingInnovVar(float &heading_innov_var) const override { heading_innov_var = _heading_innov_var; }
|
||||
void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; }
|
||||
void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; }
|
||||
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const 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 getHeadingInnovRatio(float &heading_innov_ratio) const { heading_innov_ratio = _yaw_test_ratio; }
|
||||
void getMagInnov(float mag_innov[3]) const { _mag_innov.copyTo(mag_innov); }
|
||||
void getMagInnovVar(float mag_innov_var[3]) const { _mag_innov_var.copyTo(mag_innov_var); }
|
||||
void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = _mag_test_ratio.max(); }
|
||||
|
||||
void getDragInnov(float drag_innov[2]) const 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 { _drag_innov.copyTo(drag_innov); }
|
||||
void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
|
||||
void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
|
||||
|
||||
void getAirspeedInnov(float &airspeed_innov) const 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 getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _airspeed_innov; }
|
||||
void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _airspeed_innov_var; }
|
||||
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _tas_test_ratio; }
|
||||
|
||||
void getBetaInnov(float &beta_innov) const 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 getBetaInnov(float &beta_innov) const { beta_innov = _beta_innov; }
|
||||
void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _beta_innov_var; }
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _beta_test_ratio; }
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const 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; }
|
||||
void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
|
||||
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const;
|
||||
|
||||
// get the wind velocity in m/s
|
||||
const Vector2f &getWindVelocity() const { return _state.wind_vel; };
|
||||
|
||||
// get the wind velocity var
|
||||
Vector2f getWindVelocityVariance() const override;
|
||||
Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22,22).diag(); }
|
||||
|
||||
// get the true airspeed in m/s
|
||||
void get_true_airspeed(float *tas) override;
|
||||
void get_true_airspeed(float *tas) const;
|
||||
|
||||
// get the full covariance matrix
|
||||
const matrix::SquareMatrix<float, 24> &covariances() const { return P; }
|
||||
|
|
@ -154,26 +151,26 @@ public:
|
|||
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
// return true if the origin is valid
|
||||
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override;
|
||||
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const;
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
||||
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override;
|
||||
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
|
||||
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override;
|
||||
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const;
|
||||
|
||||
// get the 1-sigma horizontal and vertical velocity uncertainty
|
||||
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override;
|
||||
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;
|
||||
|
||||
// get the vehicle control limits required by the estimator to keep within sensor limitations
|
||||
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override;
|
||||
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
|
||||
|
||||
/*
|
||||
Reset all IMU bias states and covariances to initial alignment values.
|
||||
Use when the IMU sensor has changed.
|
||||
Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
|
||||
*/
|
||||
bool reset_imu_bias() override;
|
||||
bool reset_imu_bias();
|
||||
|
||||
Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); };
|
||||
|
||||
|
|
@ -183,14 +180,6 @@ public:
|
|||
// error magnitudes (rad), (m/sec), (m)
|
||||
const Vector3f &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)
|
||||
*/
|
||||
const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
0 : Horizontal position drift rate (m/s)
|
||||
|
|
@ -199,22 +188,22 @@ public:
|
|||
Second argument returns true when IMU movement is blocking the drift calculation
|
||||
Function returns true if the metrics have been updated and not returned previously by this function
|
||||
*/
|
||||
bool get_gps_drift_metrics(float drift[3], bool *blocked) override;
|
||||
bool get_gps_drift_metrics(float drift[3], bool *blocked);
|
||||
|
||||
// return true if the global position estimate is valid
|
||||
bool global_position_is_valid() override;
|
||||
// return true if the origin is set we are not doing unconstrained free inertial navigation
|
||||
// and have not started using synthetic position observations to constrain drift
|
||||
bool global_position_is_valid() const
|
||||
{
|
||||
return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
|
||||
}
|
||||
|
||||
// check if the EKF is dead reckoning horizontal velocity using inertial data only
|
||||
void update_deadreckoning_status();
|
||||
bool isTerrainEstimateValid() const { return _hagl_valid; };
|
||||
|
||||
bool isTerrainEstimateValid() const override;
|
||||
|
||||
uint8_t getTerrainEstimateSensorBitfield() const override {return _hagl_sensor_status.value;}
|
||||
|
||||
void updateTerrainValidity();
|
||||
uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
float getTerrainVertPos() const override;
|
||||
float getTerrainVertPos() const { return _terrain_vpos; };
|
||||
|
||||
// get the terrain variance
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
|
@ -226,30 +215,38 @@ public:
|
|||
Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; }
|
||||
|
||||
// get GPS check status
|
||||
void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; }
|
||||
void get_gps_check_status(uint16_t *val) const { *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;}
|
||||
void get_posD_reset(float *delta, uint8_t *counter) const
|
||||
{
|
||||
*delta = _state_reset_status.posD_change;
|
||||
*counter = _state_reset_status.posD_counter;
|
||||
}
|
||||
|
||||
// return the amount the local vertical velocity changed in the last reset and the number of reset events
|
||||
void get_velD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
|
||||
void get_velD_reset(float *delta, uint8_t *counter) const
|
||||
{
|
||||
*delta = _state_reset_status.velD_change;
|
||||
*counter = _state_reset_status.velD_counter;
|
||||
}
|
||||
|
||||
// return the amount the local horizontal position changed in the last reset and the number of reset events
|
||||
void get_posNE_reset(float delta[2], uint8_t *counter) override
|
||||
void get_posNE_reset(float delta[2], uint8_t *counter) const
|
||||
{
|
||||
_state_reset_status.posNE_change.copyTo(delta);
|
||||
*counter = _state_reset_status.posNE_counter;
|
||||
}
|
||||
|
||||
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
|
||||
void get_velNE_reset(float delta[2], uint8_t *counter) override
|
||||
void get_velNE_reset(float delta[2], uint8_t *counter) const
|
||||
{
|
||||
_state_reset_status.velNE_change.copyTo(delta);
|
||||
*counter = _state_reset_status.velNE_counter;
|
||||
}
|
||||
|
||||
// return the amount the quaternion has changed in the last reset and the number of reset events
|
||||
void get_quat_reset(float delta_quat[4], uint8_t *counter) override
|
||||
void get_quat_reset(float delta_quat[4], uint8_t *counter) const
|
||||
{
|
||||
_state_reset_status.quat_change.copyTo(delta_quat);
|
||||
*counter = _state_reset_status.quat_counter;
|
||||
|
|
@ -261,10 +258,10 @@ public:
|
|||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
|
||||
float &hagl, float &beta) override;
|
||||
float &hagl, float &beta) const;
|
||||
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
void get_ekf_soln_status(uint16_t *status) override;
|
||||
void get_ekf_soln_status(uint16_t *status) const;
|
||||
|
||||
// return the quaternion defining the rotation from the External Vision to the EKF reference frame
|
||||
matrix::Quatf getVisionAlignmentQuaternion() const { return Quatf(_R_ev_to_ekf); };
|
||||
|
|
@ -278,15 +275,26 @@ public:
|
|||
// 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;
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
|
||||
|
||||
private:
|
||||
|
||||
// set the internal states and status to their default value
|
||||
void reset();
|
||||
|
||||
bool initialiseTilt();
|
||||
|
||||
// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
|
||||
// and reset the velocity and position states to the GPS. This will cause the EKF
|
||||
// to ignore the magnetometer for the remainder of flight.
|
||||
// This should only be used as a last resort before activating a loss of navigation failsafe
|
||||
void requestEmergencyNavReset() override { _do_ekfgsf_yaw_reset = true; }
|
||||
void requestEmergencyNavReset() { _do_ekfgsf_yaw_reset = true; }
|
||||
|
||||
// check if the EKF is dead reckoning horizontal velocity using inertial data only
|
||||
void update_deadreckoning_status();
|
||||
|
||||
void updateTerrainValidity();
|
||||
|
||||
private:
|
||||
struct {
|
||||
uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
|
||||
uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
|
||||
|
|
@ -620,8 +628,8 @@ private:
|
|||
// return true if the initialisation is successful
|
||||
bool initHagl();
|
||||
|
||||
bool shouldUseRangeFinderForHagl() const;
|
||||
bool shouldUseOpticalFlowForHagl() const;
|
||||
bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); }
|
||||
bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); }
|
||||
|
||||
// run the terrain estimator
|
||||
void runTerrainEstimator();
|
||||
|
|
@ -722,7 +730,7 @@ private:
|
|||
// and a scalar innovation value
|
||||
void fuse(const Vector24f &K, float innovation);
|
||||
|
||||
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override;
|
||||
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const override;
|
||||
|
||||
// calculate the earth rotation vector from a given latitude
|
||||
Vector3f calcEarthRateNED(float lat_rad) const;
|
||||
|
|
@ -750,28 +758,28 @@ private:
|
|||
bool noOtherYawAidingThanMag() const;
|
||||
|
||||
void checkHaglYawResetReq();
|
||||
float getTerrainVPos() const;
|
||||
float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
|
||||
|
||||
void runOnGroundYawReset();
|
||||
bool isYawResetAuthorized() const;
|
||||
bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
|
||||
bool canResetMagHeading() const;
|
||||
void runInAirYawReset();
|
||||
bool canRealignYawUsingGps() const;
|
||||
bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
|
||||
void runVelPosReset();
|
||||
|
||||
void selectMagAuto();
|
||||
void check3DMagFusionSuitability();
|
||||
void checkYawAngleObservability();
|
||||
void checkMagBiasObservability();
|
||||
bool isYawAngleObservable() const;
|
||||
bool isMagBiasObservable() const;
|
||||
bool isYawAngleObservable() const { return _yaw_angle_observable; }
|
||||
bool isMagBiasObservable() const { return _mag_bias_observable; }
|
||||
bool canUse3DMagFusion() const;
|
||||
|
||||
void checkMagDeclRequired();
|
||||
void checkMagInhibition();
|
||||
bool shouldInhibitMag() const;
|
||||
void checkMagFieldStrength();
|
||||
bool isStrongMagneticDisturbance() const;
|
||||
bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
|
||||
bool isMeasuredMatchingGpsMagStrength() const;
|
||||
bool isMeasuredMatchingAverageMagStrength() const;
|
||||
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
|
||||
|
|
@ -809,7 +817,7 @@ private:
|
|||
|
||||
// determine if flight condition is suitable to use range finder instead of the primary height sensor
|
||||
void checkRangeAidSuitability();
|
||||
bool isRangeAidSuitable() { return _is_range_aid_suitable; }
|
||||
bool isRangeAidSuitable() const { return _is_range_aid_suitable; }
|
||||
|
||||
// set control flags to use baro height
|
||||
void setControlBaroHeight();
|
||||
|
|
@ -918,6 +926,9 @@ private:
|
|||
|
||||
// Declarations used to control use of the EKF-GSF yaw estimator
|
||||
|
||||
// yaw estimator instance
|
||||
EKFGSF_yaw _yawEstimator;
|
||||
|
||||
int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec)
|
||||
uint64_t _time_last_on_ground_us{0}; ///< last tine we were on the ground (uSec)
|
||||
bool _do_ekfgsf_yaw_reset{false}; // true when an emergency yaw reset has been requested
|
||||
|
|
|
|||
|
|
@ -153,7 +153,6 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel)
|
|||
_state_reset_status.velD_counter++;
|
||||
}
|
||||
|
||||
|
||||
void Ekf::resetHorizontalPosition()
|
||||
{
|
||||
// let the next odometry update know that the previous value of states cannot be used to calculate the change in position
|
||||
|
|
@ -300,7 +299,6 @@ void Ekf::resetHeight()
|
|||
} else {
|
||||
_state.pos(2) = _ev_sample_delayed.pos(2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// reset the vertical velocity state
|
||||
|
|
@ -574,7 +572,7 @@ void Ekf::constrainStates()
|
|||
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
|
||||
}
|
||||
|
||||
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
|
||||
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
|
||||
{
|
||||
// calculate static pressure error = Pmeas - Ptruth
|
||||
// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
|
||||
|
|
@ -696,7 +694,7 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
|||
|
||||
// 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)
|
||||
bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const
|
||||
{
|
||||
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
|
||||
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
|
||||
|
|
@ -726,7 +724,7 @@ bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
|
|||
}
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
||||
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
|
||||
void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
{
|
||||
// report absolute accuracy taking into account the uncertainty in location of the origin
|
||||
// If not aiding, return 0 for horizontal position estimate as no estimate is available
|
||||
|
|
@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
|
|||
}
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
|
||||
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
|
||||
void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
|
||||
{
|
||||
// TODO - allow for baro drift in vertical position error
|
||||
float hpos_err = sqrtf(P(7, 7) + P(8, 8));
|
||||
|
|
@ -765,7 +763,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
|
|||
}
|
||||
|
||||
// get the 1-sigma horizontal and vertical velocity uncertainty
|
||||
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
|
||||
void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
|
||||
{
|
||||
float hvel_err = sqrtf(P(4, 4) + P(5, 5));
|
||||
|
||||
|
|
@ -805,7 +803,7 @@ vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting
|
|||
hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
|
||||
hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
|
||||
*/
|
||||
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
|
||||
void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
|
||||
{
|
||||
// Calculate range finder limits
|
||||
const float rangefinder_hagl_min = _range_sensor.getValidMinVal();
|
||||
|
|
@ -845,14 +843,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
|
|||
*hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
|
||||
*hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool Ekf::reset_imu_bias()
|
||||
{
|
||||
if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
|
||||
return false;
|
||||
|
||||
}
|
||||
|
||||
// Zero the delta angle and delta velocity bias states
|
||||
|
|
@ -877,7 +873,7 @@ bool Ekf::reset_imu_bias()
|
|||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
|
||||
float &hagl, float &beta)
|
||||
float &hagl, float &beta) const
|
||||
{
|
||||
// return the integer bitmask containing the consistency check pass/fail status
|
||||
status = _innov_check_fail_status.value;
|
||||
|
|
@ -900,7 +896,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
|
|||
}
|
||||
|
||||
// return a bitmask integer that describes which state estimates are valid
|
||||
void Ekf::get_ekf_soln_status(uint16_t *status)
|
||||
void Ekf::get_ekf_soln_status(uint16_t *status) const
|
||||
{
|
||||
ekf_solution_status soln_status;
|
||||
// TODO: Is this accurate enough?
|
||||
|
|
@ -941,13 +937,6 @@ void Ekf::uncorrelateQuatFromOtherStates()
|
|||
P.slice<4, _k_num_states - 4>(0, 4) = 0.f;
|
||||
}
|
||||
|
||||
bool Ekf::global_position_is_valid()
|
||||
{
|
||||
// return true if the origin is set we are not doing unconstrained free inertial navigation
|
||||
// and have not started using synthetic position observations to constrain drift
|
||||
return (_NED_origin_initialised && !_deadreckon_time_exceeded && !_using_synthetic_position);
|
||||
}
|
||||
|
||||
// return true if we are totally reliant on inertial dead-reckoning for position
|
||||
void Ekf::update_deadreckoning_status()
|
||||
{
|
||||
|
|
@ -1533,7 +1522,7 @@ bool Ekf::resetYawToEKFGSF()
|
|||
// data and the yaw estimate has converged
|
||||
float new_yaw, new_yaw_variance;
|
||||
|
||||
if (!yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) {
|
||||
if (!_yawEstimator.getYawData(&new_yaw, &new_yaw_variance)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1569,7 +1558,7 @@ bool Ekf::resetYawToEKFGSF()
|
|||
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);
|
||||
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
|
||||
}
|
||||
|
||||
void Ekf::runYawEKFGSF()
|
||||
|
|
@ -1584,12 +1573,12 @@ void Ekf::runYawEKFGSF()
|
|||
}
|
||||
|
||||
const Vector3f imu_gyro_bias = getGyroBias();
|
||||
yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
|
||||
_yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
|
||||
|
||||
// basic sanity check on GPS velocity data
|
||||
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON &&
|
||||
ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) {
|
||||
yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
|
||||
_yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -567,13 +567,6 @@ void EstimatorInterface::unallocate_buffers()
|
|||
_output_vert_buffer.unallocate();
|
||||
_drag_buffer.unallocate();
|
||||
_auxvel_buffer.unallocate();
|
||||
|
||||
}
|
||||
|
||||
bool EstimatorInterface::local_position_is_valid()
|
||||
{
|
||||
// return true if we are not doing unconstrained free inertial navigation
|
||||
return !_deadreckon_time_exceeded;
|
||||
}
|
||||
|
||||
bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const
|
||||
|
|
@ -610,7 +603,6 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
|
|||
void EstimatorInterface::print_status()
|
||||
{
|
||||
ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
|
||||
ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");
|
||||
|
||||
ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size());
|
||||
ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size());
|
||||
|
|
|
|||
|
|
@ -46,7 +46,6 @@
|
|||
#include "RingBuffer.h"
|
||||
#include <AlphaFilter/AlphaFilter.hpp>
|
||||
#include "imu_down_sampler.hpp"
|
||||
#include "EKFGSF_yaw.h"
|
||||
#include "sensor_range_finder.hpp"
|
||||
#include "utils.hpp"
|
||||
|
||||
|
|
@ -58,106 +57,19 @@ using namespace estimator;
|
|||
|
||||
class EstimatorInterface
|
||||
{
|
||||
|
||||
public:
|
||||
EstimatorInterface(): _imu_down_sampler(FILTER_UPDATE_PERIOD_S) {};
|
||||
virtual ~EstimatorInterface() = default;
|
||||
|
||||
virtual bool init(uint64_t timestamp) = 0;
|
||||
virtual void reset() = 0;
|
||||
|
||||
virtual bool update() = 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) 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) 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) 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]) 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]) const = 0;
|
||||
virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0;
|
||||
virtual void getFlowInnovRatio(float &flow_innov_ratio) 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;
|
||||
virtual void getHeadingInnovRatio(float &heading_innov_ratio) const = 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]) 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) 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) 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 matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const = 0;
|
||||
|
||||
virtual Vector2f getWindVelocityVariance() const = 0;
|
||||
|
||||
virtual void get_true_airspeed(float *tas) = 0;
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
0 : Horizontal position drift rate (m/s)
|
||||
1 : Vertical position drift rate (m/s)
|
||||
2 : Filtered horizontal velocity (m/s)
|
||||
Second argument returns true when IMU movement is blocking the drift calculation
|
||||
Function returns true if the metrics have been updated and not returned previously by this function
|
||||
*/
|
||||
virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0;
|
||||
|
||||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
// return true if the origin is valid
|
||||
virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
|
||||
virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
|
||||
|
||||
// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
|
||||
virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
|
||||
|
||||
// get the 1-sigma horizontal and vertical velocity uncertainty
|
||||
virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0;
|
||||
|
||||
// get the vehicle control limits required by the estimator to keep within sensor limitations
|
||||
virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0;
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
virtual bool collect_gps(const gps_message &gps) = 0;
|
||||
|
||||
void setIMUData(const imuSample &imu_sample);
|
||||
|
||||
/*
|
||||
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)
|
||||
*/
|
||||
const Vector3f &getImuVibrationMetrics() const { return _vibe_metrics; }
|
||||
|
||||
void setMagData(const magSample &mag_sample);
|
||||
|
||||
|
|
@ -182,29 +94,22 @@ public:
|
|||
parameters *getParamHandle() { return &_params; }
|
||||
|
||||
// set vehicle landed status data
|
||||
void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
|
||||
|
||||
/*
|
||||
Reset all IMU bias states and covariances to initial alignment values.
|
||||
Use when the IMU sensor has changed.
|
||||
Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
|
||||
*/
|
||||
virtual bool reset_imu_bias() = 0;
|
||||
void set_in_air_status(bool in_air) { _control_status.flags.in_air = in_air; }
|
||||
|
||||
// return true if the attitude is usable
|
||||
bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
|
||||
bool attitude_valid() const { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
|
||||
|
||||
// get vehicle landed status data
|
||||
bool get_in_air_status() {return _control_status.flags.in_air;}
|
||||
bool get_in_air_status() const { return _control_status.flags.in_air; }
|
||||
|
||||
// get wind estimation status
|
||||
bool get_wind_status() { return _control_status.flags.wind; }
|
||||
bool get_wind_status() const { return _control_status.flags.wind; }
|
||||
|
||||
// set vehicle is fixed wing status
|
||||
void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
|
||||
void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }
|
||||
|
||||
// set flag if synthetic sideslip measurement should be fused
|
||||
void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
|
||||
void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
|
||||
|
||||
// set flag if static pressure rise due to ground effect is expected
|
||||
// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
|
||||
|
|
@ -216,7 +121,7 @@ public:
|
|||
}
|
||||
|
||||
// set air density used by the multi-rotor specific drag force fusion
|
||||
void set_air_density(float air_density) {_air_density = air_density;}
|
||||
void set_air_density(float air_density) { _air_density = air_density; }
|
||||
|
||||
// set sensor limitations reported by the rangefinder
|
||||
void set_rangefinder_limits(float min_distance, float max_distance)
|
||||
|
|
@ -232,9 +137,6 @@ public:
|
|||
_flow_max_distance = max_distance;
|
||||
}
|
||||
|
||||
// return true if the global position estimate is valid
|
||||
virtual bool global_position_is_valid() = 0;
|
||||
|
||||
// the flags considered are opt_flow, gps, ev_vel and ev_pos
|
||||
bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;
|
||||
|
||||
|
|
@ -256,29 +158,16 @@ public:
|
|||
|
||||
int getNumberOfActiveHorizontalAidingSources() const;
|
||||
|
||||
// return true if the EKF is dead reckoning the position using inertial data only
|
||||
bool inertial_dead_reckoning() {return _is_dead_reckoning;}
|
||||
|
||||
virtual bool isTerrainEstimateValid() const = 0;
|
||||
//[[deprecated("Replaced by isTerrainEstimateValid")]]
|
||||
bool get_terrain_valid() { return isTerrainEstimateValid(); }
|
||||
|
||||
virtual uint8_t getTerrainEstimateSensorBitfield() const = 0;
|
||||
|
||||
// get the estimated terrain vertical position relative to the NED origin
|
||||
virtual float getTerrainVertPos() const = 0;
|
||||
|
||||
// return true if the local position estimate is valid
|
||||
bool local_position_is_valid();
|
||||
bool local_position_is_valid() const { return !_deadreckon_time_exceeded; }
|
||||
|
||||
// return true if the EKF is dead reckoning the position using inertial data only
|
||||
bool inertial_dead_reckoning() const { return _is_dead_reckoning; }
|
||||
|
||||
const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
|
||||
|
||||
// get the velocity of the body frame origin in local NED earth frame
|
||||
Vector3f getVelocity() const
|
||||
{
|
||||
const Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
|
||||
return vel_earth;
|
||||
}
|
||||
Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; }
|
||||
|
||||
// get the velocity derivative in earth frame
|
||||
const Vector3f &getVelocityDerivative() const { return _vel_deriv; }
|
||||
|
|
@ -298,7 +187,7 @@ public:
|
|||
// Get the value of magnetic declination in degrees to be saved for use at the next startup
|
||||
// Returns true when the declination can be saved
|
||||
// At the next startup, set param.mag_declination_deg to the value saved
|
||||
bool get_mag_decl_deg(float *val)
|
||||
bool get_mag_decl_deg(float *val) const
|
||||
{
|
||||
if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
|
||||
*val = math::degrees(_mag_declination_gps);
|
||||
|
|
@ -310,69 +199,35 @@ public:
|
|||
}
|
||||
|
||||
// get EKF mode status
|
||||
void get_control_mode(uint32_t *val) { *val = _control_status.value; }
|
||||
void get_control_mode(uint32_t *val) const { *val = _control_status.value; }
|
||||
|
||||
// get EKF internal fault status
|
||||
void get_filter_fault_status(uint16_t *val) { *val = _fault_status.value; }
|
||||
void get_filter_fault_status(uint16_t *val) const { *val = _fault_status.value; }
|
||||
|
||||
bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
|
||||
|
||||
// get GPS check status
|
||||
virtual void get_gps_check_status(uint16_t *val) = 0;
|
||||
|
||||
// return the amount the local vertical position changed in the last reset and the number of reset events
|
||||
virtual void get_posD_reset(float *delta, uint8_t *counter) = 0;
|
||||
|
||||
// return the amount the local vertical velocity changed in the last reset and the number of reset events
|
||||
virtual void get_velD_reset(float *delta, uint8_t *counter) = 0;
|
||||
|
||||
// return the amount the local horizontal position changed in the last reset and the number of reset events
|
||||
virtual void get_posNE_reset(float delta[2], uint8_t *counter) = 0;
|
||||
|
||||
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
|
||||
virtual void get_velNE_reset(float delta[2], uint8_t *counter) = 0;
|
||||
|
||||
// return the amount the quaternion has changed in the last reset and the number of reset events
|
||||
virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0;
|
||||
|
||||
// get EKF innovation consistency check status information comprising of:
|
||||
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
||||
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
||||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
virtual void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
|
||||
float &hagl, float &beta) = 0;
|
||||
|
||||
// return a bitmask integer that describes which state estimates can be used for flight control
|
||||
virtual void get_ekf_soln_status(uint16_t *status) = 0;
|
||||
|
||||
// Getter for the average imu update period in s
|
||||
float get_dt_imu_avg() const { return _dt_imu_avg; }
|
||||
|
||||
// Getter for the imu sample on the delayed time horizon
|
||||
const imuSample &get_imu_sample_delayed() { return _imu_sample_delayed; }
|
||||
const imuSample &get_imu_sample_delayed() const { return _imu_sample_delayed; }
|
||||
|
||||
// Getter for the baro sample on the delayed time horizon
|
||||
const baroSample &get_baro_sample_delayed() { return _baro_sample_delayed; }
|
||||
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
|
||||
|
||||
void print_status();
|
||||
|
||||
static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
|
||||
static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
|
||||
|
||||
// request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
|
||||
// argment should be incremented only when a new reset is required
|
||||
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;
|
||||
|
||||
protected:
|
||||
|
||||
parameters _params; // filter parameters
|
||||
EstimatorInterface() = default;
|
||||
virtual ~EstimatorInterface() = default;
|
||||
|
||||
ImuDownSampler _imu_down_sampler;
|
||||
virtual bool init(uint64_t timestamp) = 0;
|
||||
|
||||
parameters _params; // filter parameters
|
||||
|
||||
/*
|
||||
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
|
||||
|
|
@ -391,8 +246,6 @@ protected:
|
|||
*/
|
||||
uint8_t _imu_buffer_length{0};
|
||||
|
||||
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
|
||||
|
||||
float _dt_imu_avg{0.0f}; // average imu update period in s
|
||||
|
||||
imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon
|
||||
|
|
@ -409,9 +262,6 @@ protected:
|
|||
dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
|
||||
auxVelSample _auxvel_sample_delayed{};
|
||||
|
||||
// Used by the multi-rotor specific drag force fusion
|
||||
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
|
||||
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
|
||||
|
||||
// Sensor limitations
|
||||
|
|
@ -460,13 +310,6 @@ protected:
|
|||
bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid
|
||||
bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements
|
||||
|
||||
// IMU vibration and movement monitoring
|
||||
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
|
||||
Vector3f _vibe_metrics; // IMU vibration metrics
|
||||
// [0] Level of coning vibration in the IMU delta angles (rad^2)
|
||||
// [1] high frequency vibration level in the IMU delta angle data (rad)
|
||||
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics
|
||||
// [0] Horizontal position drift rate (m/s)
|
||||
// [1] Vertical position drift rate (m/s)
|
||||
|
|
@ -488,20 +331,6 @@ protected:
|
|||
RingBuffer<dragSample> _drag_buffer;
|
||||
RingBuffer<auxVelSample> _auxvel_buffer;
|
||||
|
||||
// yaw estimator instance
|
||||
EKFGSF_yaw yawEstimator;
|
||||
|
||||
// observation buffer final allocation failed
|
||||
bool _gps_buffer_fail{false};
|
||||
bool _mag_buffer_fail{false};
|
||||
bool _baro_buffer_fail{false};
|
||||
bool _range_buffer_fail{false};
|
||||
bool _airspeed_buffer_fail{false};
|
||||
bool _flow_buffer_fail{false};
|
||||
bool _ev_buffer_fail{false};
|
||||
bool _drag_buffer_fail{false};
|
||||
bool _auxvel_buffer_fail{false};
|
||||
|
||||
// timestamps of latest in buffer saved measurement in microseconds
|
||||
uint64_t _time_last_imu{0};
|
||||
uint64_t _time_last_gps{0};
|
||||
|
|
@ -515,24 +344,11 @@ protected:
|
|||
//last time the baro ground effect compensation was turned on externally (uSec)
|
||||
uint64_t _time_last_gnd_effect_on{0};
|
||||
|
||||
// Used to downsample magnetometer data
|
||||
Vector3f _mag_data_sum;
|
||||
uint8_t _mag_sample_count {0};
|
||||
uint64_t _mag_timestamp_sum {0};
|
||||
|
||||
// Used to down sample barometer data
|
||||
float _baro_alt_sum {0.0f}; // summed pressure altitude readings (m)
|
||||
uint8_t _baro_sample_count {0}; // number of barometric altitude measurements summed
|
||||
uint64_t _baro_timestamp_sum {0}; // summed timestamp to provide the timestamp of the averaged sample
|
||||
|
||||
fault_status_u _fault_status{};
|
||||
|
||||
// allocate data buffers and initialize interface variables
|
||||
bool initialise_interface(uint64_t timestamp);
|
||||
|
||||
// free buffer memory
|
||||
void unallocate_buffers();
|
||||
|
||||
float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
|
||||
float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T)
|
||||
|
|
@ -543,12 +359,55 @@ protected:
|
|||
// this is the previous status of the filter control modes - used to detect mode transitions
|
||||
filter_control_status_u _control_status_prev{};
|
||||
|
||||
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const = 0;
|
||||
|
||||
private:
|
||||
|
||||
inline void setDragData(const imuSample &imu);
|
||||
|
||||
inline void computeVibrationMetric(const imuSample &imu);
|
||||
inline bool checkIfVehicleAtRest(float dt, const imuSample &imu);
|
||||
|
||||
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;
|
||||
|
||||
void printBufferAllocationFailed(const char *buffer_name);
|
||||
|
||||
// free buffer memory
|
||||
void unallocate_buffers();
|
||||
|
||||
ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S};
|
||||
|
||||
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
|
||||
|
||||
// IMU vibration and movement monitoring
|
||||
Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
|
||||
Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
|
||||
Vector3f _vibe_metrics; // IMU vibration metrics
|
||||
// [0] Level of coning vibration in the IMU delta angles (rad^2)
|
||||
// [1] high frequency vibration level in the IMU delta angle data (rad)
|
||||
// [2] high frequency vibration level in the IMU delta velocity data (m/s)
|
||||
|
||||
// Used to down sample barometer data
|
||||
uint64_t _baro_timestamp_sum{0}; // summed timestamp to provide the timestamp of the averaged sample
|
||||
float _baro_alt_sum{0.0f}; // summed pressure altitude readings (m)
|
||||
uint8_t _baro_sample_count{0}; // number of barometric altitude measurements summed
|
||||
|
||||
// Used by the multi-rotor specific drag force fusion
|
||||
uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
|
||||
float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
|
||||
|
||||
// Used to downsample magnetometer data
|
||||
uint64_t _mag_timestamp_sum{0};
|
||||
Vector3f _mag_data_sum;
|
||||
uint8_t _mag_sample_count{0};
|
||||
|
||||
// observation buffer final allocation failed
|
||||
bool _gps_buffer_fail{false};
|
||||
bool _mag_buffer_fail{false};
|
||||
bool _baro_buffer_fail{false};
|
||||
bool _range_buffer_fail{false};
|
||||
bool _airspeed_buffer_fail{false};
|
||||
bool _flow_buffer_fail{false};
|
||||
bool _ev_buffer_fail{false};
|
||||
bool _drag_buffer_fail{false};
|
||||
bool _auxvel_buffer_fail{false};
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -138,11 +138,6 @@ void Ekf::checkHaglYawResetReq()
|
|||
}
|
||||
}
|
||||
|
||||
float Ekf::getTerrainVPos() const
|
||||
{
|
||||
return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
|
||||
}
|
||||
|
||||
void Ekf::runOnGroundYawReset()
|
||||
{
|
||||
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
||||
|
|
@ -154,11 +149,6 @@ void Ekf::runOnGroundYawReset()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::isYawResetAuthorized() const
|
||||
{
|
||||
return !_is_yaw_fusion_inhibited;
|
||||
}
|
||||
|
||||
bool Ekf::canResetMagHeading() const
|
||||
{
|
||||
return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
|
||||
|
|
@ -177,11 +167,6 @@ void Ekf::runInAirYawReset()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::canRealignYawUsingGps() const
|
||||
{
|
||||
return _control_status.flags.fixed_wing;
|
||||
}
|
||||
|
||||
void Ekf::runVelPosReset()
|
||||
{
|
||||
if (_velpos_reset_request) {
|
||||
|
|
@ -237,16 +222,6 @@ void Ekf::checkMagBiasObservability()
|
|||
_time_yaw_started = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
bool Ekf::isYawAngleObservable() const
|
||||
{
|
||||
return _yaw_angle_observable;
|
||||
}
|
||||
|
||||
bool Ekf::isMagBiasObservable() const
|
||||
{
|
||||
return _mag_bias_observable;
|
||||
}
|
||||
|
||||
bool Ekf::canUse3DMagFusion() const
|
||||
{
|
||||
// Use of 3D fusion requires an in-air heading alignment but it should not
|
||||
|
|
@ -308,11 +283,6 @@ void Ekf::checkMagFieldStrength()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::isStrongMagneticDisturbance() const
|
||||
{
|
||||
return _control_status.flags.mag_field_disturbed;
|
||||
}
|
||||
|
||||
bool Ekf::isMeasuredMatchingGpsMagStrength() const
|
||||
{
|
||||
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
||||
|
|
|
|||
|
|
@ -83,16 +83,6 @@ bool Ekf::initHagl()
|
|||
return initialized;
|
||||
}
|
||||
|
||||
bool Ekf::shouldUseRangeFinderForHagl() const
|
||||
{
|
||||
return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder);
|
||||
}
|
||||
|
||||
bool Ekf::shouldUseOpticalFlowForHagl() const
|
||||
{
|
||||
return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow);
|
||||
}
|
||||
|
||||
void Ekf::runTerrainEstimator()
|
||||
{
|
||||
// If we are on ground, store the local position and time to use as a reference
|
||||
|
|
@ -286,11 +276,6 @@ void Ekf::fuseFlowForTerrain()
|
|||
}
|
||||
}
|
||||
|
||||
bool Ekf::isTerrainEstimateValid() const
|
||||
{
|
||||
return _hagl_valid;
|
||||
}
|
||||
|
||||
void Ekf::updateTerrainValidity()
|
||||
{
|
||||
// we have been fusing range finder measurements in the last 5 seconds
|
||||
|
|
@ -306,12 +291,6 @@ void Ekf::updateTerrainValidity()
|
|||
_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
|
||||
&& recent_range_fusion
|
||||
&& (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
|
||||
_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl()
|
||||
&& recent_flow_for_terrain_fusion;
|
||||
}
|
||||
|
||||
// get the estimated vertical position of the terrain relative to the NED origin
|
||||
float Ekf::getTerrainVertPos() const
|
||||
{
|
||||
return _terrain_vpos;
|
||||
_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue