mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 22:32:11 +08:00
EKF: inline simple getters
This commit is contained in:
Daniel Agar
@@ -70,82 +70,57 @@ public:
|
||||
bool update() override;
|
||||
|
||||
void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
|
||||
|
||||
void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
|
||||
|
||||
void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override;
|
||||
|
||||
void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
|
||||
|
||||
void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const override;
|
||||
|
||||
void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const override;
|
||||
|
||||
void getBaroHgtInnov(float &baro_hgt_innov) const override;
|
||||
void 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 getBaroHgtInnovVar(float &baro_hgt_innov_var) const override;
|
||||
|
||||
void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const override;
|
||||
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const override;
|
||||
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const override;
|
||||
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override;
|
||||
void getRngHgtInnov(float &rng_hgt_innov) const override { rng_hgt_innov = _rng_hgt_innov(2); }
|
||||
void getRngHgtInnovVar(float &rng_hgt_innov_var) const override { rng_hgt_innov_var = _rng_hgt_innov_var(2); }
|
||||
void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const override { rng_hgt_innov_ratio = _rng_hgt_test_ratio(1); }
|
||||
|
||||
void getAuxVelInnov(float aux_vel_innov[2]) const override;
|
||||
|
||||
void getAuxVelInnovVar(float aux_vel_innov[2]) const override;
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override { aux_vel_innov_ratio = _aux_vel_test_ratio(0); }
|
||||
|
||||
void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const override;
|
||||
void getFlowInnov(float flow_innov[2]) const override { _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 override;
|
||||
void getHeadingInnov(float &heading_innov) const override { heading_innov = _heading_innov; }
|
||||
void getHeadingInnovVar(float &heading_innov_var) const override { heading_innov_var = _heading_innov_var; }
|
||||
|
||||
void getFlowInnovVar(float flow_innov_var[2]) const override;
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const override { heading_innov_ratio = _yaw_test_ratio; }
|
||||
void getMagInnov(float mag_innov[3]) const override { _mag_innov.copyTo(mag_innov); }
|
||||
void getMagInnovVar(float mag_innov_var[3]) const override { _mag_innov_var.copyTo(mag_innov_var); }
|
||||
void getMagInnovRatio(float &mag_innov_ratio) const override { mag_innov_ratio = _mag_test_ratio.max(); }
|
||||
|
||||
void getFlowInnovRatio(float &flow_innov_ratio) const override;
|
||||
void getDragInnov(float drag_innov[2]) const override { _drag_innov.copyTo(drag_innov); }
|
||||
void getDragInnovVar(float drag_innov_var[2]) const override { _drag_innov_var.copyTo(drag_innov_var); }
|
||||
void getDragInnovRatio(float drag_innov_ratio[2]) const override { _drag_test_ratio.copyTo(drag_innov_ratio); }
|
||||
|
||||
Vector2f getFlowVelBody() const override;
|
||||
Vector2f getFlowVelNE() const override;
|
||||
Vector2f getFlowCompensated() const override;
|
||||
Vector2f getFlowUncompensated() const override;
|
||||
Vector3f getFlowGyro() const override;
|
||||
void getAirspeedInnov(float &airspeed_innov) const override { airspeed_innov = _airspeed_innov; }
|
||||
void getAirspeedInnovVar(float &airspeed_innov_var) const override { airspeed_innov_var = _airspeed_innov_var; }
|
||||
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override { airspeed_innov_ratio = _tas_test_ratio; }
|
||||
|
||||
void getHeadingInnov(float &heading_innov) const override;
|
||||
void getBetaInnov(float &beta_innov) const override { beta_innov = _beta_innov; }
|
||||
void getBetaInnovVar(float &beta_innov_var) const override { beta_innov_var = _beta_innov_var; }
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const override { beta_innov_ratio = _beta_test_ratio; }
|
||||
|
||||
void getHeadingInnovVar(float &heading_innov_var) const override;
|
||||
|
||||
void getHeadingInnovRatio(float &heading_innov_ratio) const override;
|
||||
|
||||
void getMagInnov(float mag_innov[3]) const override;
|
||||
|
||||
void getMagInnovVar(float mag_innov_var[3]) const override;
|
||||
|
||||
void getMagInnovRatio(float &mag_innov_ratio) const override;
|
||||
|
||||
void getDragInnov(float drag_innov[2]) const override;
|
||||
|
||||
void getDragInnovVar(float drag_innov_var[2]) const override;
|
||||
|
||||
void getDragInnovRatio(float drag_innov_ratio[2]) const override;
|
||||
|
||||
void getAirspeedInnov(float &airspeed_innov) const override;
|
||||
|
||||
void getAirspeedInnovVar(float &airspeed_innov_var) const override;
|
||||
|
||||
void getAirspeedInnovRatio(float &airspeed_innov_ratio) const override;
|
||||
|
||||
void getBetaInnov(float &beta_innov) const override;
|
||||
|
||||
void getBetaInnovVar(float &beta_innov_var) const override;
|
||||
|
||||
void getBetaInnovRatio(float &beta_innov_ratio) const override;
|
||||
|
||||
void getHaglInnov(float &hagl_innov) const override;
|
||||
|
||||
void getHaglInnovVar(float &hagl_innov_var) const override;
|
||||
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const override;
|
||||
void getHaglInnov(float &hagl_innov) const override { hagl_innov = _hagl_innov; }
|
||||
void getHaglInnovVar(float &hagl_innov_var) const override { hagl_innov_var = _hagl_innov_var; }
|
||||
void getHaglInnovRatio(float &hagl_innov_ratio) const override { hagl_innov_ratio = _hagl_test_ratio; }
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> getStateAtFusionHorizonAsVector() const override;
|
||||
@@ -206,7 +181,7 @@ public:
|
||||
|
||||
// return an array containing the output predictor angular, velocity and position tracking
|
||||
// error magnitudes (rad), (m/sec), (m)
|
||||
Vector3f getOutputTrackingError() const override;
|
||||
Vector3f getOutputTrackingError() const override { return _output_tracking_error; }
|
||||
|
||||
/*
|
||||
Returns following IMU vibration metrics in the following array locations
|
||||
@@ -214,7 +189,7 @@ public:
|
||||
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
*/
|
||||
Vector3f getImuVibrationMetrics() const override;
|
||||
Vector3f getImuVibrationMetrics() const override { return _vibe_metrics; }
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
@@ -245,13 +220,13 @@ public:
|
||||
float get_terrain_var() const { return _terrain_var; }
|
||||
|
||||
// get the accelerometer bias in m/s**2
|
||||
Vector3f getAccelBias() const override;
|
||||
Vector3f getAccelBias() const override { return _state.delta_vel_bias / _dt_ekf_avg; }
|
||||
|
||||
// get the gyroscope bias in rad/s
|
||||
Vector3f getGyroBias() const override;
|
||||
Vector3f getGyroBias() const override { return _state.delta_ang_bias / _dt_ekf_avg; }
|
||||
|
||||
// get GPS check status
|
||||
void get_gps_check_status(uint16_t *val) override;
|
||||
void get_gps_check_status(uint16_t *val) override { *val = _gps_check_fail_status.value; }
|
||||
|
||||
// return the amount the local vertical position changed in the last reset and the number of reset events
|
||||
void get_posD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
|
||||
@@ -301,13 +276,14 @@ 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;
|
||||
bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) override;
|
||||
|
||||
// Request the EKF reset the yaw to the estimate from the internal EKF-GSF filter
|
||||
// and reset the velocity and position states to the GPS. This will cause the EKF
|
||||
// to ignore the magnetometer for the remainder of flight.
|
||||
// This should only be used as a last resort before activating a loss of navigation failsafe
|
||||
void requestEmergencyNavReset() override;
|
||||
void requestEmergencyNavReset() override { _do_ekfgsf_yaw_reset = true; }
|
||||
|
||||
private:
|
||||
struct {
|
||||
@@ -528,7 +504,7 @@ private:
|
||||
// and the correction step
|
||||
void calculateOutputStates();
|
||||
void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction);
|
||||
void applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Vector3f& pos_correction);
|
||||
void applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction);
|
||||
|
||||
// initialise filter states of both the delayed ekf and the real time complementary filter
|
||||
bool initialiseFilter(void);
|
||||
@@ -565,7 +541,7 @@ private:
|
||||
// variance : observaton variance
|
||||
// gate_sigma : innovation consistency check gate size (Sigma)
|
||||
// jacobian : 4x1 vector of partial derivatives of observation wrt each quaternion state
|
||||
void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f& yaw_jacobian);
|
||||
void updateQuaternion(const float innovation, const float variance, const float gate_sigma, const Vector4f &yaw_jacobian);
|
||||
|
||||
// fuse the yaw angle obtained from a dual antenna GPS unit
|
||||
void fuseGpsYaw();
|
||||
@@ -678,31 +654,37 @@ private:
|
||||
// matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24>
|
||||
// that is optimized by exploring the sparsity in H
|
||||
template <size_t ...Idxs>
|
||||
SquareMatrix24f computeKHP(const Vector24f& K, const SparseVector24f<Idxs...>& H) const {
|
||||
SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
|
||||
{
|
||||
SquareMatrix24f KHP;
|
||||
constexpr size_t non_zeros = sizeof...(Idxs);
|
||||
float KH[non_zeros];
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for(unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
for (unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
KH[i] = K(row) * H.atCompressedIndex(i);
|
||||
}
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = 0.f;
|
||||
for(unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
|
||||
for (unsigned i = 0; i < H.non_zeros(); i++) {
|
||||
const size_t index = H.index(i);
|
||||
tmp += KH[i] * P(index, column);
|
||||
}
|
||||
KHP(row,column) = tmp;
|
||||
|
||||
KHP(row, column) = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
return KHP;
|
||||
}
|
||||
|
||||
// measurement update with a single measurement
|
||||
// returns true if fusion is performed
|
||||
template <size_t ...Idxs>
|
||||
bool measurementUpdate(const Vector24f& K, const SparseVector24f<Idxs...>& H, float innovation) {
|
||||
bool measurementUpdate(const Vector24f &K, const SparseVector24f<Idxs...> &H, float innovation)
|
||||
{
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
@@ -719,12 +701,13 @@ private:
|
||||
// apply the state corrections
|
||||
fuse(K, innovation);
|
||||
}
|
||||
|
||||
return is_healthy;
|
||||
}
|
||||
|
||||
// if the covariance correction will result in a negative variance, then
|
||||
// the covariance matrix is unhealthy and must be corrected
|
||||
bool checkAndFixCovarianceUpdate(const SquareMatrix24f& KHP);
|
||||
bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP);
|
||||
|
||||
// limit the diagonal of the covariance matrix
|
||||
// force symmetry when the argument is true
|
||||
@@ -735,7 +718,7 @@ private:
|
||||
|
||||
// generic function which will perform a fusion step given a kalman gain K
|
||||
// and a scalar innovation value
|
||||
void fuse(const Vector24f& K, float innovation);
|
||||
void fuse(const Vector24f &K, float innovation);
|
||||
|
||||
float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override;
|
||||
|
||||
|
||||
@@ -646,36 +646,6 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
|
||||
vpos = _ev_pos_test_ratio(1);
|
||||
}
|
||||
|
||||
void Ekf::getBaroHgtInnov(float &baro_hgt_innov) const
|
||||
{
|
||||
baro_hgt_innov = _baro_hgt_innov(2);
|
||||
}
|
||||
|
||||
void Ekf::getBaroHgtInnovVar(float &baro_hgt_innov_var) const
|
||||
{
|
||||
baro_hgt_innov_var = _baro_hgt_innov_var(2);
|
||||
}
|
||||
|
||||
void Ekf::getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const
|
||||
{
|
||||
baro_hgt_innov_ratio = _baro_hgt_test_ratio(1);
|
||||
}
|
||||
|
||||
void Ekf::getRngHgtInnov(float &rng_hgt_innov) const
|
||||
{
|
||||
rng_hgt_innov = _rng_hgt_innov(2);
|
||||
}
|
||||
|
||||
void Ekf::getRngHgtInnovVar(float &rng_hgt_innov_var) const
|
||||
{
|
||||
rng_hgt_innov_var = _rng_hgt_innov_var(2);
|
||||
}
|
||||
|
||||
void Ekf::getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const
|
||||
{
|
||||
rng_hgt_innov_ratio = _rng_hgt_test_ratio(1);
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
|
||||
{
|
||||
aux_vel_innov[0] = _aux_vel_innov(0);
|
||||
@@ -688,147 +658,6 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
|
||||
aux_vel_innov_var[1] = _aux_vel_innov_var(1);
|
||||
}
|
||||
|
||||
void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) const
|
||||
{
|
||||
aux_vel_innov_ratio = _aux_vel_test_ratio(0);
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnov(float flow_innov[2]) const
|
||||
{
|
||||
_flow_innov.copyTo(flow_innov);
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
|
||||
{
|
||||
_flow_innov_var.copyTo(flow_innov_var);
|
||||
}
|
||||
|
||||
void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const
|
||||
{
|
||||
flow_innov_ratio = _optflow_test_ratio;
|
||||
}
|
||||
|
||||
Vector2f Ekf::getFlowVelBody() const
|
||||
{
|
||||
return _flow_vel_body;
|
||||
}
|
||||
|
||||
Vector2f Ekf::getFlowVelNE() const
|
||||
{
|
||||
return _flow_vel_ne;
|
||||
}
|
||||
|
||||
Vector2f Ekf::getFlowCompensated() const
|
||||
{
|
||||
return _flow_compensated_XY_rad;
|
||||
}
|
||||
|
||||
Vector2f Ekf::getFlowUncompensated() const
|
||||
{
|
||||
return _flow_sample_delayed.flow_xy_rad;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getFlowGyro() const
|
||||
{
|
||||
return _flow_sample_delayed.gyro_xyz;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnov(float &heading_innov) const
|
||||
{
|
||||
heading_innov = _heading_innov;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnovVar(float &heading_innov_var) const
|
||||
{
|
||||
heading_innov_var = _heading_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getHeadingInnovRatio(float &heading_innov_ratio) const
|
||||
{
|
||||
heading_innov_ratio = _yaw_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getMagInnov(float mag_innov[3]) const
|
||||
{
|
||||
_mag_innov.copyTo(mag_innov);
|
||||
}
|
||||
|
||||
void Ekf::getMagInnovVar(float mag_innov_var[3]) const
|
||||
{
|
||||
_mag_innov_var.copyTo(mag_innov_var);
|
||||
}
|
||||
|
||||
void Ekf::getMagInnovRatio(float &mag_innov_ratio) const
|
||||
{
|
||||
mag_innov_ratio = _mag_test_ratio.max();
|
||||
}
|
||||
|
||||
void Ekf::getDragInnov(float drag_innov[2]) const
|
||||
{
|
||||
_drag_innov.copyTo(drag_innov);
|
||||
}
|
||||
|
||||
void Ekf::getDragInnovVar(float drag_innov_var[2]) const
|
||||
{
|
||||
_drag_innov_var.copyTo(drag_innov_var);
|
||||
}
|
||||
|
||||
void Ekf::getDragInnovRatio(float drag_innov_ratio[2]) const
|
||||
{
|
||||
_drag_test_ratio.copyTo(drag_innov_ratio);
|
||||
}
|
||||
|
||||
void Ekf::getAirspeedInnov(float &airspeed_innov) const
|
||||
{
|
||||
airspeed_innov = _airspeed_innov;
|
||||
}
|
||||
|
||||
void Ekf::getAirspeedInnovVar(float &airspeed_innov_var) const
|
||||
{
|
||||
airspeed_innov_var = _airspeed_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getAirspeedInnovRatio(float &airspeed_innov_ratio) const
|
||||
{
|
||||
airspeed_innov_ratio = _tas_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnov(float &beta_innov) const
|
||||
{
|
||||
beta_innov = _beta_innov;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnovVar(float &beta_innov_var) const
|
||||
{
|
||||
beta_innov_var = _beta_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getBetaInnovRatio(float &beta_innov_ratio) const
|
||||
{
|
||||
beta_innov_ratio = _beta_test_ratio;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnov(float &hagl_innov) const
|
||||
{
|
||||
hagl_innov = _hagl_innov;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnovVar(float &hagl_innov_var) const
|
||||
{
|
||||
hagl_innov_var = _hagl_innov_var;
|
||||
}
|
||||
|
||||
void Ekf::getHaglInnovRatio(float &hagl_innov_ratio) const
|
||||
{
|
||||
hagl_innov_ratio = _hagl_test_ratio;
|
||||
}
|
||||
|
||||
// get GPS check status
|
||||
void Ekf::get_gps_check_status(uint16_t *val)
|
||||
{
|
||||
*val = _gps_check_fail_status.value;
|
||||
}
|
||||
|
||||
// get the state vector at the delayed time horizon
|
||||
matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
{
|
||||
@@ -844,16 +673,6 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
||||
return state;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getAccelBias() const
|
||||
{
|
||||
return _state.delta_vel_bias / _dt_ekf_avg;
|
||||
}
|
||||
|
||||
Vector3f Ekf::getGyroBias() const
|
||||
{
|
||||
return _state.delta_ang_bias / _dt_ekf_avg;
|
||||
}
|
||||
|
||||
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
|
||||
// return true if the origin is valid
|
||||
bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
|
||||
@@ -864,24 +683,6 @@ bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
|
||||
return _NED_origin_initialised;
|
||||
}
|
||||
|
||||
// return a vector containing the output predictor angular, velocity and position tracking
|
||||
// error magnitudes (rad), (m/s), (m)
|
||||
Vector3f Ekf::getOutputTrackingError() const
|
||||
{
|
||||
return _output_tracking_error;
|
||||
}
|
||||
|
||||
/*
|
||||
Returns following IMU vibration metrics in the following array locations
|
||||
0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
|
||||
1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
|
||||
2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
|
||||
*/
|
||||
Vector3f Ekf::getImuVibrationMetrics() const
|
||||
{
|
||||
return _vibe_metrics;
|
||||
}
|
||||
|
||||
/*
|
||||
First argument returns GPS drift metrics in the following array locations
|
||||
0 : Horizontal position drift rate (m/s)
|
||||
@@ -1738,11 +1539,6 @@ bool Ekf::resetYawToEKFGSF()
|
||||
return true;
|
||||
}
|
||||
|
||||
void Ekf::requestEmergencyNavReset()
|
||||
{
|
||||
_do_ekfgsf_yaw_reset = true;
|
||||
}
|
||||
|
||||
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||
{
|
||||
return yawEstimator.getLogData(yaw_composite,yaw_variance,yaw,innov_VN,innov_VE,weight);
|
||||
|
||||
+15
-13
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
|
||||
* Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -60,7 +60,7 @@ class EstimatorInterface
|
||||
{
|
||||
|
||||
public:
|
||||
EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){};
|
||||
EstimatorInterface(): _imu_down_sampler(FILTER_UPDATE_PERIOD_S) {};
|
||||
virtual ~EstimatorInterface() = default;
|
||||
|
||||
virtual bool init(uint64_t timestamp) = 0;
|
||||
@@ -91,11 +91,11 @@ public:
|
||||
virtual void getFlowInnov(float flow_innov[2]) const = 0;
|
||||
virtual void getFlowInnovVar(float flow_innov_var[2]) const = 0;
|
||||
virtual void getFlowInnovRatio(float &flow_innov_ratio) const = 0;
|
||||
virtual Vector2f getFlowVelBody() const = 0;
|
||||
virtual Vector2f getFlowVelNE() const = 0;
|
||||
virtual Vector2f getFlowCompensated() const = 0;
|
||||
virtual Vector2f getFlowUncompensated() const = 0;
|
||||
virtual Vector3f getFlowGyro() const = 0;
|
||||
virtual const Vector2f &getFlowVelBody() const = 0;
|
||||
virtual const Vector2f &getFlowVelNE() const = 0;
|
||||
virtual const Vector2f &getFlowCompensated() const = 0;
|
||||
virtual const Vector2f &getFlowUncompensated() const = 0;
|
||||
virtual const Vector3f &getFlowGyro() const = 0;
|
||||
|
||||
virtual void getHeadingInnov(float &heading_innov) const = 0;
|
||||
virtual void getHeadingInnovVar(float &heading_innov_var) const = 0;
|
||||
@@ -181,15 +181,15 @@ public:
|
||||
|
||||
void setAirspeedData(const airspeedSample &airspeed_sample);
|
||||
|
||||
void setRangeData(const rangeSample& range_sample);
|
||||
void setRangeData(const rangeSample &range_sample);
|
||||
|
||||
// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
|
||||
void setOpticalFlowData(const flowSample& flow);
|
||||
void setOpticalFlowData(const flowSample &flow);
|
||||
|
||||
// set external vision position and attitude data
|
||||
void setExtVisionData(const extVisionSample& evdata);
|
||||
void setExtVisionData(const extVisionSample &evdata);
|
||||
|
||||
void setAuxVelData(const auxVelSample& auxvel_sample);
|
||||
void setAuxVelData(const auxVelSample &auxvel_sample);
|
||||
|
||||
// return a address to the parameters struct
|
||||
// in order to give access to the application
|
||||
@@ -328,6 +328,7 @@ public:
|
||||
bool get_mag_decl_deg(float *val)
|
||||
{
|
||||
*val = 0.0f;
|
||||
|
||||
if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
|
||||
*val = math::degrees(_mag_declination_gps);
|
||||
return true;
|
||||
@@ -407,7 +408,8 @@ public:
|
||||
virtual void requestEmergencyNavReset() = 0;
|
||||
|
||||
// get ekf-gsf debug data
|
||||
virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
|
||||
virtual bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) = 0;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -591,5 +593,5 @@ protected:
|
||||
|
||||
virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;
|
||||
|
||||
void printBufferAllocationFailed(const char * buffer_name);
|
||||
void printBufferAllocationFailed(const char *buffer_name);
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user