52 virtual ~Ekf() =
default;
55 bool init(uint64_t timestamp)
override;
58 void reset(uint64_t timestamp)
override;
162 void get_ekf_ctrl_limits(
float *vxy_max,
float *vz_max,
float *hagl_min,
float *hagl_max)
override;
602 void fuse(
float *K,
float innovation);
695 static constexpr
float sq(
float var) {
return var * var; }
716 void zeroRows(
float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
719 void zeroCols(
float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
722 void zeroOffDiag(
float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
726 void setDiag(
float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last,
float variance);
768 float kahanSummation(
float sum_previous,
float input,
float &accumulator)
const;
bool canUse3DMagFusion() const
void fuseDeclination(float decl_sigma)
Vector3f _vel_err_integ
integral of velocity tracking error (m)
float _cos_tilt_rng
cosine of the range finder tilt rotation about the Y body axis
uint64_t _time_bad_vert_accel
last time a bad vertical accel was detected (uSec)
void setControlBaroHeight()
float _posInnovGateNE
Number of standard deviations used for the NE position fusion innovation consistency check...
float _beta_innov
synthetic sideslip measurement innovation (rad)
void calculateOutputStates()
bool _fuse_height
true when baro height data should be fused
float _gpsDriftVelN
GPS north position derivative (m/sec)
stateSample _state
state struct of the ekf running at the delayed time horizon
matrix::SquareMatrix< float, 24 > covariances() const
static struct vehicle_status_s status
float _vel_pos_innov_var[6]
NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) ...
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override
float _flow_innov[2]
flow measurement innovation (rad/sec)
uint64_t _time_last_mov_3d_mag_suitable
last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) ...
bool initialiseFilter(void)
void initialiseQuatCovariances(Vector3f &rot_vec_var)
float _dt_update
delta time since last ekf update. This time can be used for filters which run at the same rate as the...
float velD_change
Down velocity change due to last reset (m/sec)
float _saved_mag_ef_covmat[2][2]
NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) ...
void setControlGPSHeight()
uint64_t _ev_rot_last_time_us
previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) ...
Quatf calculate_quaternion() const
float _yaw_rate_lpf_ef
Filtered angular rate about earth frame D axis (rad/sec)
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)
void checkMagBiasObservability()
float _saved_mag_bf_variance[4]
magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) ...
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
void runOnGroundYawReset()
bool shouldInhibitMag() const
void get_state_delayed(float *state) override
void get_heading_innov(float *heading_innov) override
void get_pos_var(Vector3f &pos_var) override
float _last_static_yaw
last yaw angle recorded when on ground motion checks were passing (rad)
bool _mag_yaw_reset_req
true when a reset of the yaw using the magnetometer data has been requested
Vector2f _hpos_pred_prev
previous value of NE position state used by odometry fusion (m)
bool _synthetic_mag_z_active
true if we are generating synthetic magnetometer Z measurements
void updateTerrainValidity()
float _gps_alt_ref
WGS-84 height (m)
uint8_t posNE_counter
number of horizontal position reset events (allow to wrap if count exceeds 255)
void get_true_airspeed(float *tas) override
Vector3f _delta_vel_bias_var_accum
kahan summation algorithm accumulator for delta velocity bias variance
uint64_t _last_imu_bias_cov_reset_us
time the last reset of IMU delta angle and velocity state covariances was performed (uSec) ...
float _yaw_delta_ef
Recent change in yaw angle measured about the earth frame D axis (rad)
void controlHeightSensorTimeouts()
uint32_t _min_gps_health_time_us
GPS is marked as healthy only after this amount of time.
float get_terrain_var() const
void get_gyro_bias(float bias[3]) override
Vector3f _imu_del_ang_of
bias corrected delta angle measurements accumulated across the same time frame as the optical flow ra...
void updateRangeDataContinuity()
float _heading_innov_var
heading measurement innovation variance (rad**2)
void controlOpticalFlowFusion()
Vector2f _accel_lpf_NE
Low pass filtered horizontal earth frame acceleration (m/sec**2)
bool canRunMagFusion() const
bool _flow_for_terrain_data_ready
void get_output_tracking_error(float error[3]) override
bool _ev_rot_mat_initialised
_ev_rot_mat should only be initialised once in the beginning through the reset function ...
uint64_t _time_bad_rng_signal_quality
timestamp at which range finder signal quality was 0 (used for hysteresis)
uint64_t _last_gps_pass_us
last system time in usec that the GPS passed it's checks
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
float _drag_innov[2]
multirotor drag measurement innovation (m/sec**2)
uint64_t _last_gps_origin_time_us
true when all active GPS checks have passed
void get_drag_innov_var(float drag_innov_var[2]) override
Vector3f _flow_gyro_bias
bias errors in optical flow sensor rate gyro outputs (rad/sec)
void get_mag_innov_var(float mag_innov_var[3]) override
void initialiseCovariance()
bool _fuse_hpos_as_odom
true when the NE position data is being fused using an odometry assumption
float _rng_stuck_max_val
maximum value for new rng measurement when being stuck
Vector3f _delta_angle_bias_var_accum
kahan summation algorithm accumulator for delta angle bias variance
float kahanSummation(float sum_previous, float input, float &accumulator) const
float _terrain_var
variance of terrain position estimate (m**2)
bool _mag_decl_cov_reset
true after the fuseDeclination() function has been used to modify the earth field covariances after a...
uint64_t _last_gps_fail_us
last system time in usec that the GPS failed it's checks
bool _fuse_vert_vel
true when gps vertical velocity measurement should be fused
bool gps_is_good(const gps_message &gps)
float _gps_velE_filt
GPS filtered East velocity (m/sec)
void get_imu_vibe_metrics(float vibe[3]) override
void checkRangeAidSuitability()
float _mag_innov[3]
earth magnetic field innovations (Gauss)
void get_flow_innov_var(float flow_innov_var[2]) override
bool _range_aid_mode_selected
true when range finder is being used as the height reference instead of the primary height sensor ...
imuSample _imu_down_sampled
down sampled imu data (sensor rate -> filter update rate)
bool collect_gps(const gps_message &gps) override
Vector2f posNE_change
North, East position change due to last reset (m)
float P[_k_num_states][_k_num_states]
state covariance matrix
uint64_t _time_good_vert_accel
last time a good vertical accel was detected (uSec)
uint64_t _time_bad_motion_us
last system time that on-ground motion exceeded limits (uSec)
void zeroCols(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
uint64_t _time_last_mag
measurement time of last magnetomter sample (uSec)
static bool isMeasuredMatchingExpected(float measured, float expected, float gate)
void get_wind_velocity(float *wind) override
uint64_t _time_last_rng_ready
time the last range finder measurement was ready (uSec)
bool _mag_inhibit_yaw_reset_req
true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions...
void checkMagFieldStrength()
bool collect_imu(const imuSample &imu) override
float _terrain_vpos
estimated vertical position of the terrain underneath the vehicle in local NED frame (m) ...
float _rng_stuck_min_val
minimum value for new rng measurement when being stuck
bool _accel_bias_inhibit
true when the accel bias learning is being inhibited
float _heading_innov
heading measurement innovation (rad)
uint64_t _time_last_of_fuse
time the last fusion of optical flow measurements were performed (uSec)
uint64_t _time_last_hgt_fuse
time the last fusion of height measurements was performed (uSec)
float _imu_collection_time_adj
the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PE...
float _hagl_innov_var
innovation variance for the last height above terrain measurement (m**2)
void resetMagRelatedCovariances()
float posD_change
Down position change due to last reset (m)
float getMagDeclination()
bool _baro_data_ready
true when new baro height data has fallen behind the fusion time horizon and is available to be fused...
gps_check_fail_status_u _gps_check_fail_status
void controlFusionModes()
bool _fuse_hor_vel
true when gps horizontal velocity measurement should be fused
void fuse(float *K, float innovation)
void getHaglInnovVar(float *hagl_innov_var) override
AlphaFilterVector3f _mag_lpf
filtered magnetometer measurement (Gauss)
Vector3f _prev_dvel_bias_var
saved delta velocity XYZ bias variances (m/sec)**2
bool isStrongMagneticDisturbance() const
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
void get_airspeed_innov_var(float *airspeed_innov_var) override
float _hvelInnovGate
Number of standard deviations used for the horizontal velocity fusion innovation consistency check...
void calcEarthRateNED(Vector3f &omega, float lat_rad) const
void get_accel_bias(float bias[3]) override
void controlExternalVisionFusion()
float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted)
void runMagAndMagDeclFusions()
void update_deadreckoning_status()
bool _inhibit_flow_use
true when use of optical flow and range finder is being inhibited
void controlHeightFusion()
void get_heading_innov_var(float *heading_innov_var) override
bool _ev_data_ready
true when new external vision system data has fallen behind the fusion time horizon and is available ...
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
void zeroOffDiag(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
int _primary_hgt_source
specifies primary source of height data
bool _mag_use_inhibit_prev
true when magnetometer use was being inhibited the previous frame
Vector3f calcRotVecVariances()
float _beta_innov_var
synthetic sideslip measurement innovation variance (rad**2)
bool isYawResetAuthorized() const
bool _flt_mag_align_converging
true when the in-flight mag field post alignment convergence is being performd
void get_velD_reset(float *delta, uint8_t *counter) override
bool _terrain_initialised
true when the terrain estimator has been initialized
bool isMeasuredMatchingGpsMagStrength() const
void controlRangeFinderFusion()
void get_posNE_reset(float delta[2], uint8_t *counter) override
void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override
bool reset_imu_bias() override
Vector2f _flowRadXYcomp
measured delta angle of the image about the X and Y body axes after removal of body rotation (rad)...
bool canResetMagHeading() const
float getTerrainVPos() const
bool _baro_hgt_faulty
true if valid baro data is unavailable for use
float _output_tracking_error[3]
contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override
bool _flow_data_ready
true when the leading edge of the optical flow integration period has fallen behind the fusion time h...
void reset(uint64_t timestamp) override
uint32_t _mag_counter
number of magnetometer samples read during initialisation
bool get_gps_drift_metrics(float drift[3], bool *blocked) override
uint8_t posD_counter
number of vertical position reset events (allow to wrap if count exceeds 255)
Vector2< float > Vector2f
float _drag_innov_var[2]
multirotor drag measurement innovation variance ((m/sec**2)**2)
void fuseFlowForTerrain()
uint8_t quat_counter
number of quaternion reset events (allow to wrap if count exceeds 255)
uint32_t _ev_counter
number of external vision samples read during initialisation
void get_airspeed_innov(float *airspeed_innov) override
Vector3f _earth_rate_NED
earth rotation vector (NED) in rad/s
void setDiag(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)
float _aux_vel_innov[2]
NE auxiliary velocity innovations: (m/sec)
void checkMagInhibition()
float _gps_drift_velD
GPS down position derivative (m/sec)
static constexpr float sq(float var)
bool _tas_data_ready
true when new true airspeed data has fallen behind the fusion time horizon and is available to be fus...
float _mag_innov_var[3]
earth magnetic field innovation variance (Gauss**2)
void get_ev2ekf_quaternion(float *quat) override
void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override
void get_ekf_soln_status(uint16_t *status) override
float _delta_time_of
time in sec that _imu_del_ang_of was accumulated over (sec)
bool _velpos_reset_request
true when a large yaw error has been fixed and a velocity and position state reset is required ...
uint64_t _time_last_fake_gps
same flag as "_flow_data_ready" but used for separate terrain estimator
float _airspeed_innov_var
airspeed measurement innovation variance ((m/sec)**2)
void get_posD_reset(float *delta, uint8_t *counter) override
void increaseQuatYawErrVariance(float yaw_variance)
float _posObsNoiseNE
1-STD observation noise used for the fusion of NE position data (m)
void controlAuxVelFusion()
bool _hpos_prev_available
true when previous values of the estimate and measurement are available for use
void fixCovarianceErrors()
uint64_t _time_acc_bias_check
last time the accel bias check passed (uSec)
bool canRealignYawUsingGps() const
Dcmf _ev_rot_mat
transformation matrix that rotates observations from the EV to the EKF navigation frame...
void checkMagDeclRequired()
uint64_t _time_ins_deadreckon_start
amount of time we have been doing inertial only deadreckoning (uSec)
bool _rng_hgt_valid
true if range finder sample retrieved from buffer is valid
void setControlRangeHeight()
void get_flow_innov(float flow_innov[2]) override
void setControlEVHeight()
uint8_t velD_counter
number of vertical velocity reset events (allow to wrap if count exceeds 255)
void get_mag_innov(float mag_innov[3]) override
void resetWindCovariance()
void get_aux_vel_innov(float aux_vel_innov[2]) override
void runTerrainEstimator()
bool _mag_use_inhibit
true when magnetometer use is being inhibited
void getTerrainVertPos(float *ret) override
void get_vel_var(Vector3f &vel_var) override
Vector3< float > Vector3f
void makeSymmetrical(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
bool _hagl_valid
true when the height above ground estimate is valid
float _accel_mag_filt
acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _R_rng_to_earth_2_2
2,2 element of the rotation matrix from sensor frame to earth frame
float _hagl_innov
innovation of the last height above terrain measurement (m)
matrix::SquareMatrix< float, 4 > orientation_covariances() const
float _vvelInnovGate
Number of standard deviations used for the vertical velocity fusion innovation consistency check...
#define VDIST_SENSOR_BARO
Use baro height.
matrix::Vector< float, 24 > covariances_diagonal() const
bool _filter_initialised
true when the EKF sttes and covariances been initialised
uint64_t _time_last_arsp_fuse
time the last fusion of airspeed measurements were performed (uSec)
bool _gps_hgt_intermittent
true if gps height into the buffer is intermittent
void get_velNE_reset(float delta[2], uint8_t *counter) override
float _gps_velD_diff_filt
GPS filtered Down velocity (m/sec)
uint64_t _flt_mag_align_start_time
time that inflight magnetic field alignment started (uSec)
float _sin_tilt_rng
sine of the range finder tilt rotation about the Y body axis
uint64_t _time_yaw_started
last system time in usec that a yaw rotation manoeuvre was detected
float _flow_innov_var[2]
flow innovation variance ((rad/sec)**2)
Quaternion< float > Quatf
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Quatf quat_change
quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaterni...
void set_min_required_gps_health_time(uint32_t time_us)
void controlVelPosFusion()
Definition of base class for attitude estimators.
struct Ekf::@62 _state_reset_status
reset event monitoring structure containing velocity, position, height and yaw reset information ...
uint64_t _time_good_motion_us
last system time that on-ground motion was within limits (uSec)
void get_vel_pos_innov(float vel_pos_innov[6]) override
Vector3f _pos_meas_prev
previous value of NED position measurement fused using odometry assumption (m)
bool _fuse_pos
true when gps position data should be fused
bool isYawAngleObservable() const
bool _yaw_angle_observable
true when there is enough horizontal acceleration to make yaw observable
Vector2f _last_known_posNE
last known local NE position vector (m)
Vector3f _velObsVarNED
1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 ...
float _dt_last_range_update_filt_us
filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)...
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
Vector3f _delVel_sum
summed delta velocity (m/sec)
bool isRangeDataContinuous()
void get_gps_check_status(uint16_t *val) override
void run3DMagAndDeclFusions()
float _airspeed_innov
airspeed measurement innovation (m/sec)
uint64_t _delta_time_baro_us
delta time between two consecutive delayed baro samples from the buffer (uSec)
float _last_on_ground_posD
last vertical position when the in_air status was false (m)
void checkHaglYawResetReq()
uint8_t _num_bad_flight_yaw_events
number of times a bad heading has been detected in flight and required a yaw reset ...
uint64_t _time_last_vel_fuse
time the last fusion of velocity measurements was performed (uSec)
float _ang_rate_mag_filt
angular rate magnitude after application of a decaying envelope filter (rad/sec)
bool isRangeAidSuitable()
void getHaglInnov(float *hagl_innov) override
float _baro_hgt_offset
baro height reading at the local NED origin (m)
float _gpsDriftVelE
GPS east position derivative (m/sec)
void get_wind_velocity_var(float *wind_var) override
uint32_t _hgt_counter
number of height samples read during initialisation
void controlAirDataFusion()
bool init(uint64_t timestamp) override
static constexpr float FILTER_UPDATE_PERIOD_S
void check3DMagFusionSuitability()
Quatf _q_down_sampled
down sampled quaternion (tracking delta angles between ekf update steps)
bool _bad_vert_accel_detected
true when bad vertical accelerometer data has been detected
void uncorrelateQuatStates()
matrix::SquareMatrix< float, 3 > position_covariances() const
matrix::SquareMatrix< float, 3 > velocity_covariances() const
void get_quat_reset(float delta_quat[4], uint8_t *counter) override
float _gps_velN_filt
GPS filtered North velocity (m/sec)
bool _earth_rate_initialised
true when we know the earth rotatin rate (requires GPS)
Vector3f _ev_rot_vec_filt
filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (ra...
void updateRangeDataStuck()
void updateRangeDataValidity()
Vector3f _delta_angle_corr
delta angle correction vector (rad)
void get_beta_innov(float *beta_innov) override
static constexpr uint8_t _k_num_states
number of EKF states
void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) override
uint64_t _time_last_beta_fuse
time the last fusion of synthetic sideslip measurements were performed (uSec)
Vector3f _accel_vec_filt
acceleration vector after application of a low pass filter (m/sec**2)
uint64_t _time_last_hagl_fuse
last system time that the hagl measurement failed it's checks (uSec)
float _dt_ekf_avg
average update rate of the ekf
uint8_t velNE_counter
number of horizontal position reset events (allow to wrap if count exceeds 255)
bool _gps_data_ready
true when new GPS data has fallen behind the fusion time horizon and is available to be fused ...
bool calcOptFlowBodyRateComp()
bool isMagBiasObservable() const
void get_drag_innov(float drag_innov[2]) override
bool isTerrainEstimateValid() const override
Vector3f _pos_err_integ
integral of position tracking error (m.s)
void get_beta_innov_var(float *beta_innov_var) override
Vector2f velNE_change
North East velocity change due to last reset (m)
bool _using_synthetic_position
true if we are using a synthetic position to constrain drift
uint64_t _mag_use_not_inhibit_us
last system time in usec before magnetometer use was inhibited
bool _mag_data_ready
true when new magnetometer data has fallen behind the fusion time horizon and is available to be fuse...
float _gps_error_norm
normalised gps error
bool global_position_is_valid() override
float calcOptFlowMeasVar()
bool _range_data_ready
true when new range finder data has fallen behind the fusion time horizon and is available to be fuse...
bool isMeasuredMatchingAverageMagStrength() const
float _rng_filt_state
filtered height measurement (m)
bool _mag_bias_observable
true when there is enough rotation to make magnetometer bias errors observable
bool _fuse_hor_vel_aux
true when auxiliary horizontal velocity measurement should be fused
void resetStates() override
bool _is_range_aid_suitable
true when range finder can be used in flight as the height reference instead of the primary height se...
void resetStatesAndCovariances() override
void checkYawAngleObservability()