PX4 Firmware
PX4 Autopilot Software http://px4.io
ekf.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name ECL nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file ekf.h
36  * Class for core functions for ekf attitude and position estimator.
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  * @author Paul Riseborough <p_riseborough@live.com.au>
40  *
41  */
42 
43 #pragma once
44 
45 #include "estimator_interface.h"
46 
47 class Ekf : public EstimatorInterface
48 {
49 public:
50 
51  Ekf() = default;
52  virtual ~Ekf() = default;
53 
54  // initialise variables to sane values (also interface class)
55  bool init(uint64_t timestamp) override;
56 
57  // set the internal states and status to their default value
58  void reset(uint64_t timestamp) override;
59 
60  void resetStatesAndCovariances() override;
61  void resetStates() override;
62 
63  // should be called every time new data is pushed into the filter
64  bool update() override;
65 
66  // gets the innovations of velocity and position measurements
67  // 0-2 vel, 3-5 pos
68  void get_vel_pos_innov(float vel_pos_innov[6]) override;
69 
70  // gets the innovations for of the NE auxiliary velocity measurement
71  void get_aux_vel_innov(float aux_vel_innov[2]) override;
72 
73  // gets the innovations of the earth magnetic field measurements
74  void get_mag_innov(float mag_innov[3]) override;
75 
76  // gets the innovations of the heading measurement
77  void get_heading_innov(float *heading_innov) override;
78 
79  // gets the innovation variances of velocity and position measurements
80  // 0-2 vel, 3-5 pos
81  void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override;
82 
83  // gets the innovation variances of the earth magnetic field measurements
84  void get_mag_innov_var(float mag_innov_var[3]) override;
85 
86  // gets the innovations of airspeed measurement
87  void get_airspeed_innov(float *airspeed_innov) override;
88 
89  // gets the innovation variance of the airspeed measurement
90  void get_airspeed_innov_var(float *airspeed_innov_var) override;
91 
92  // gets the innovations of synthetic sideslip measurement
93  void get_beta_innov(float *beta_innov) override;
94 
95  // gets the innovation variance of the synthetic sideslip measurement
96  void get_beta_innov_var(float *beta_innov_var) override;
97 
98  // gets the innovation variance of the heading measurement
99  void get_heading_innov_var(float *heading_innov_var) override;
100 
101  // gets the innovation variance of the flow measurement
102  void get_flow_innov_var(float flow_innov_var[2]) override;
103 
104  // gets the innovation of the flow measurement
105  void get_flow_innov(float flow_innov[2]) override;
106 
107  // gets the innovation variance of the drag specific force measurement
108  void get_drag_innov_var(float drag_innov_var[2]) override;
109 
110  // gets the innovation of the drag specific force measurement
111  void get_drag_innov(float drag_innov[2]) override;
112 
113  void getHaglInnovVar(float *hagl_innov_var) override;
114  void getHaglInnov(float *hagl_innov) override;
115 
116  // get the state vector at the delayed time horizon
117  void get_state_delayed(float *state) override;
118 
119  // get the wind velocity in m/s
120  void get_wind_velocity(float *wind) override;
121 
122  // get the wind velocity var
123  void get_wind_velocity_var(float *wind_var) override;
124 
125  // get the true airspeed in m/s
126  void get_true_airspeed(float *tas) override;
127 
128  // get the full covariance matrix
130 
131  // get the diagonal elements of the covariance matrix
133 
134  // get the orientation (quaterion) covariances
136 
137  // get the linear velocity covariances
138  matrix::SquareMatrix<float, 3> velocity_covariances() const { return covariances().slice<3, 3>(4, 4); }
139 
140  // get the position covariances
141  matrix::SquareMatrix<float, 3> position_covariances() const { return covariances().slice<3, 3>(7, 7); }
142 
143  // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
144  bool collect_gps(const gps_message &gps) override;
145 
146  bool collect_imu(const imuSample &imu) override;
147 
148  // get the ekf WGS-84 origin position and height and the system time it was last set
149  // return true if the origin is valid
150  bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override;
151 
152  // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
153  void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) override;
154 
155  // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
156  void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override;
157 
158  // get the 1-sigma horizontal and vertical velocity uncertainty
159  void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override;
160 
161  // get the vehicle control limits required by the estimator to keep within sensor limitations
162  void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) override;
163 
164  /*
165  Reset all IMU bias states and covariances to initial alignment values.
166  Use when the IMU sensor has changed.
167  Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
168  */
169  bool reset_imu_bias() override;
170 
171  void get_vel_var(Vector3f &vel_var) override;
172 
173  void get_pos_var(Vector3f &pos_var) override;
174 
175  // return an array containing the output predictor angular, velocity and position tracking
176  // error magnitudes (rad), (m/sec), (m)
177  void get_output_tracking_error(float error[3]) override;
178 
179  /*
180  Returns following IMU vibration metrics in the following array locations
181  0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
182  1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
183  2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
184  */
185  void get_imu_vibe_metrics(float vibe[3]) override;
186 
187  /*
188  First argument returns GPS drift metrics in the following array locations
189  0 : Horizontal position drift rate (m/s)
190  1 : Vertical position drift rate (m/s)
191  2 : Filtered horizontal velocity (m/s)
192  Second argument returns true when IMU movement is blocking the drift calculation
193  Function returns true if the metrics have been updated and not returned previously by this function
194  */
195  bool get_gps_drift_metrics(float drift[3], bool *blocked) override;
196 
197  // return true if the global position estimate is valid
198  bool global_position_is_valid() override;
199 
200  // check if the EKF is dead reckoning horizontal velocity using inertial data only
202 
203  bool isTerrainEstimateValid() const override;
204 
205  void updateTerrainValidity();
206 
207  // get the estimated terrain vertical position relative to the NED origin
208  void getTerrainVertPos(float *ret) override;
209 
210  // get the terrain variance
211  float get_terrain_var() const { return _terrain_var; }
212 
213  // get the accelerometer bias in m/s/s
214  void get_accel_bias(float bias[3]) override;
215 
216  // get the gyroscope bias in rad/s
217  void get_gyro_bias(float bias[3]) override;
218 
219  // get GPS check status
220  void get_gps_check_status(uint16_t *val) override;
221 
222  // return the amount the local vertical position changed in the last reset and the number of reset events
223  void get_posD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
224 
225  // return the amount the local vertical velocity changed in the last reset and the number of reset events
226  void get_velD_reset(float *delta, uint8_t *counter) override {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
227 
228  // return the amount the local horizontal position changed in the last reset and the number of reset events
229  void get_posNE_reset(float delta[2], uint8_t *counter) override
230  {
231  _state_reset_status.posNE_change.copyTo(delta);
232  *counter = _state_reset_status.posNE_counter;
233  }
234 
235  // return the amount the local horizontal velocity changed in the last reset and the number of reset events
236  void get_velNE_reset(float delta[2], uint8_t *counter) override
237  {
238  _state_reset_status.velNE_change.copyTo(delta);
239  *counter = _state_reset_status.velNE_counter;
240  }
241 
242  // return the amount the quaternion has changed in the last reset and the number of reset events
243  void get_quat_reset(float delta_quat[4], uint8_t *counter) override
244  {
245  _state_reset_status.quat_change.copyTo(delta_quat);
246  *counter = _state_reset_status.quat_counter;
247  }
248 
249  // get EKF innovation consistency check status information comprising of:
250  // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
251  // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
252  // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
253  // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
254  void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) override;
255 
256  // return a bitmask integer that describes which state estimates can be used for flight control
257  void get_ekf_soln_status(uint16_t *status) override;
258 
259  // return the quaternion defining the rotation from the External Vision to the EKF reference frame
260  void get_ev2ekf_quaternion(float *quat) override;
261 
262  // use the latest IMU data at the current time horizon.
263  Quatf calculate_quaternion() const;
264 
265  // set minimum continuous period without GPS fail required to mark a healthy GPS status
266  void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; }
267 
268 private:
269 
270  static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
271 
272  struct {
273  uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
274  uint8_t velD_counter; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
275  uint8_t posNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
276  uint8_t posD_counter; ///< number of vertical position reset events (allow to wrap if count exceeds 255)
277  uint8_t quat_counter; ///< number of quaternion reset events (allow to wrap if count exceeds 255)
278  Vector2f velNE_change; ///< North East velocity change due to last reset (m)
279  float velD_change; ///< Down velocity change due to last reset (m/sec)
280  Vector2f posNE_change; ///< North, East position change due to last reset (m)
281  float posD_change; ///< Down position change due to last reset (m)
282  Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
283  } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information
284 
285  float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
286  float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
287 
288  stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
289 
290  bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised
291  bool _earth_rate_initialised{false}; ///< true when we know the earth rotatin rate (requires GPS)
292 
293  bool _fuse_height{false}; ///< true when baro height data should be fused
294  bool _fuse_pos{false}; ///< true when gps position data should be fused
295  bool _fuse_hor_vel{false}; ///< true when gps horizontal velocity measurement should be fused
296  bool _fuse_vert_vel{false}; ///< true when gps vertical velocity measurement should be fused
297  bool _fuse_hor_vel_aux{false}; ///< true when auxiliary horizontal velocity measurement should be fused
298 
299  float _posObsNoiseNE{0.0f}; ///< 1-STD observation noise used for the fusion of NE position data (m)
300  float _posInnovGateNE{1.0f}; ///< Number of standard deviations used for the NE position fusion innovation consistency check
301 
302  Vector3f _velObsVarNED; ///< 1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2
303  float _hvelInnovGate{1.0f}; ///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check
304  float _vvelInnovGate{1.0f}; ///< Number of standard deviations used for the vertical velocity fusion innovation consistency check
305 
306  // variables used when position data is being fused using a relative position odometry model
307  bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption
308  Vector3f _pos_meas_prev; ///< previous value of NED position measurement fused using odometry assumption (m)
309  Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m)
310  bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use
311  Vector3f _ev_rot_vec_filt; ///< filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (rad)
312  Dcmf _ev_rot_mat; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity
313  uint64_t _ev_rot_last_time_us{0}; ///< previous time that the calculation of the EV to EKF rotation matrix was updated (uSec)
314  bool _ev_rot_mat_initialised{0}; ///< _ev_rot_mat should only be initialised once in the beginning through the reset function
315 
316  // booleans true when fresh sensor data is available at the fusion time horizon
317  bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
318  bool _mag_data_ready{false}; ///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
319  bool _baro_data_ready{false}; ///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
320  bool _range_data_ready{false}; ///< true when new range finder data has fallen behind the fusion time horizon and is available to be fused
321  bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
322  bool _ev_data_ready{false}; ///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
323  bool _tas_data_ready{false}; ///< true when new true airspeed data has fallen behind the fusion time horizon and is available to be fused
324  bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator
325 
326  uint64_t _time_last_fake_gps{0}; ///< last time we faked GPS position measurements to constrain tilt errors during operation without external aiding (uSec)
327  uint64_t _time_ins_deadreckon_start{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
328  bool _using_synthetic_position{false}; ///< true if we are using a synthetic position to constrain drift
329 
330  uint64_t _time_last_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec)
331  uint64_t _time_last_delpos_fuse{0}; ///< time the last fusion of incremental horizontal position measurements was performed (uSec)
332  uint64_t _time_last_vel_fuse{0}; ///< time the last fusion of velocity measurements was performed (uSec)
333  uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of height measurements was performed (uSec)
334  uint64_t _time_last_of_fuse{0}; ///< time the last fusion of optical flow measurements were performed (uSec)
335  uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
336  uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
337  uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec)
338  Vector2f _last_known_posNE; ///< last known local NE position vector (m)
339  float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
340 
341  uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec)
342  uint64_t _delta_time_baro_us{0}; ///< delta time between two consecutive delayed baro samples from the buffer (uSec)
343 
344  uint64_t _last_imu_bias_cov_reset_us{0}; ///< time the last reset of IMU delta angle and velocity state covariances was performed (uSec)
345 
346  Vector3f _earth_rate_NED; ///< earth rotation vector (NED) in rad/s
347 
348  Dcmf _R_to_earth; ///< transformation matrix from body frame to earth frame from last EKF prediction
349 
350  // used by magnetometer fusion mode selection
351  Vector2f _accel_lpf_NE; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
352  float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
353  float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
354  bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
355  bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
356  uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected
357  uint8_t _num_bad_flight_yaw_events{0}; ///< number of times a bad heading has been detected in flight and required a yaw reset
358  uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited
359  bool _mag_use_inhibit{false}; ///< true when magnetometer use is being inhibited
360  bool _mag_use_inhibit_prev{false}; ///< true when magnetometer use was being inhibited the previous frame
361  bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
362  float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad)
363  bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested
364  bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
365  bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
366 
367  float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix
368 
369  Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance
370  Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance
371 
372  float _vel_pos_innov[6] {}; ///< NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
373  float _vel_pos_innov_var[6] {}; ///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
374  float _aux_vel_innov[2] {}; ///< NE auxiliary velocity innovations: (m/sec)
375 
376  float _mag_innov[3] {}; ///< earth magnetic field innovations (Gauss)
377  float _mag_innov_var[3] {}; ///< earth magnetic field innovation variance (Gauss**2)
378 
379  float _airspeed_innov{0.0f}; ///< airspeed measurement innovation (m/sec)
380  float _airspeed_innov_var{0.0f}; ///< airspeed measurement innovation variance ((m/sec)**2)
381 
382  float _beta_innov{0.0f}; ///< synthetic sideslip measurement innovation (rad)
383  float _beta_innov_var{0.0f}; ///< synthetic sideslip measurement innovation variance (rad**2)
384 
385  float _drag_innov[2] {}; ///< multirotor drag measurement innovation (m/sec**2)
386  float _drag_innov_var[2] {}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2)
387 
388  float _heading_innov{0.0f}; ///< heading measurement innovation (rad)
389  float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2)
390 
391  // optical flow processing
392  float _flow_innov[2] {}; ///< flow measurement innovation (rad/sec)
393  float _flow_innov_var[2] {}; ///< flow innovation variance ((rad/sec)**2)
394  Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
395  Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
396  float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
397  uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec)
398  uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec)
399  bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
400  Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
401 
402  // output predictor states
403  Vector3f _delta_angle_corr; ///< delta angle correction vector (rad)
404  imuSample _imu_down_sampled{}; ///< down sampled imu data (sensor rate -> filter update rate)
405  Quatf _q_down_sampled; ///< down sampled quaternion (tracking delta angles between ekf update steps)
406  Vector3f _vel_err_integ; ///< integral of velocity tracking error (m)
407  Vector3f _pos_err_integ; ///< integral of position tracking error (m.s)
408  float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
409 
410  // variables used for the GPS quality checks
411  float _gpsDriftVelN{0.0f}; ///< GPS north position derivative (m/sec)
412  float _gpsDriftVelE{0.0f}; ///< GPS east position derivative (m/sec)
413  float _gps_drift_velD{0.0f}; ///< GPS down position derivative (m/sec)
414  float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec)
415  float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec)
416  float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec)
417  uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
418  uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
419  float _gps_error_norm{1.0f}; ///< normalised gps error
420  uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
421  bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
422 
423  // Variables used to publish the WGS-84 location of the EKF local NED origin
424  uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec)
425  float _gps_alt_ref{0.0f}; ///< WGS-84 height (m)
426 
427  // Variables used to initialise the filter states
428  uint32_t _hgt_counter{0}; ///< number of height samples read during initialisation
429  float _rng_filt_state{0.0f}; ///< filtered height measurement (m)
430  uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
431  uint32_t _ev_counter{0}; ///< number of external vision samples read during initialisation
432  uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
433  AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement (Gauss)
434  Vector3f _delVel_sum; ///< summed delta velocity (m/sec)
435  float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m)
436  float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)
437 
438  // Variables used to control activation of post takeoff functionality
439  float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
440  bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd
441  uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
442  uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
443  float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
444  float _saved_mag_ef_covmat[2][2] {}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
445  bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
446 
448 
449  // variables used to inhibit accel bias learning
450  bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited
451  Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
452  float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
453  float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
454  Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2
455 
456  // Terrain height state estimation
457  float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
458  float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2)
459  float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m)
460  float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
461  uint64_t _time_last_hagl_fuse{0}; ///< last system time that the hagl measurement failed it's checks (uSec)
462  bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized
463  float _sin_tilt_rng{0.0f}; ///< sine of the range finder tilt rotation about the Y body axis
464  float _cos_tilt_rng{0.0f}; ///< cosine of the range finder tilt rotation about the Y body axis
465  float _R_rng_to_earth_2_2{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame
466  float _dt_last_range_update_filt_us{0.0f}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
467  bool _hagl_valid{false}; ///< true when the height above ground estimate is valid
468 
469  // height sensor status
470  bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use
471  bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent
472  bool _rng_hgt_valid{false}; ///< true if range finder sample retrieved from buffer is valid
473  int _primary_hgt_source{VDIST_SENSOR_BARO}; ///< specifies primary source of height data
474  uint64_t _time_bad_rng_signal_quality{0}; ///< timestamp at which range finder signal quality was 0 (used for hysteresis)
475 
476  // imu fault status
477  uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec)
478  uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec)
479  bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected
480 
481  // variables used to control range aid functionality
482  bool _is_range_aid_suitable{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor
483  bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor
484 
485  // variables used to check range finder validity data
486  float _rng_stuck_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck
487  float _rng_stuck_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck
488 
489  float _height_rate_lpf{0.0f};
490 
491  // update the real time complementary filter states. This includes the prediction
492  // and the correction step
493  void calculateOutputStates();
494 
495  // initialise filter states of both the delayed ekf and the real time complementary filter
496  bool initialiseFilter(void);
497 
498  // initialise ekf covariance matrix
499  void initialiseCovariance();
500 
501  // predict ekf state
502  void predictState();
503 
504  // predict ekf covariance
505  void predictCovariance();
506 
507  // ekf sequential fusion of magnetometer measurements
508  void fuseMag();
509 
510  // fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
511  void fuseHeading();
512 
513  // fuse the yaw angle obtained from a dual antenna GPS unit
514  void fuseGpsAntYaw();
515 
516  // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
517  // return true if the reset was successful
518  bool resetGpsAntYaw();
519 
520  // fuse magnetometer declination measurement
521  // argument passed in is the declination uncertainty in radians
522  void fuseDeclination(float decl_sigma);
523 
524  // apply sensible limits to the declination and length of the NE mag field states estimates
525  void limitDeclination();
526 
527  // fuse airspeed measurement
528  void fuseAirspeed();
529 
530  // fuse synthetic zero sideslip measurement
531  void fuseSideslip();
532 
533  // fuse body frame drag specific forces for multi-rotor wind estimation
534  void fuseDrag();
535 
536  // fuse velocity and position measurements (also barometer height)
537  void fuseVelPosHeight();
538 
539  // reset velocity states of the ekf
540  bool resetVelocity();
541 
542  // fuse optical flow line of sight rate measurements
543  void fuseOptFlow();
544 
545  // calculate optical flow body angular rate compensation
546  // returns false if bias corrected body rate data is unavailable
548 
549  // initialise the terrain vertical position estimator
550  // return true if the initialisation is successful
551  bool initHagl();
552 
553  // run the terrain estimator
554  void runTerrainEstimator();
555 
556  // update the terrain vertical position estimate using a height above ground measurement from the range finder
557  void fuseHagl();
558 
559  // update the terrain vertical position estimate using an optical flow measurement
560  void fuseFlowForTerrain();
561 
562  // reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
563  // return true if successful
564  bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
565 
566  // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
567  // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
568  bool realignYawGPS();
569 
570  // Return the magnetic declination in radians to be used by the alignment and fusion processing
571  float getMagDeclination();
572 
573  // reset position states of the ekf (only horizontal position)
574  bool resetPosition();
575 
576  // reset height state of the ekf
577  void resetHeight();
578 
579  // modify output filter to match the the EKF state at the fusion time horizon
580  void alignOutputFilter();
581 
582  // update the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
583  // and update the rotation matrix which transforms EV navigation frame measurements into NED
584  void calcExtVisRotMat();
585 
586 
587  // reset the estimated angular misalignment vector between the EV naigration frame and the EKF navigation frame
588  // and reset the rotation matrix which transforms EV navigation frame measurements into NED
589  void resetExtVisRotMat();
590 
591  // limit the diagonal of the covariance matrix
592  void fixCovarianceErrors();
593 
594  // make ekf covariance matrix symmetric between a nominated state indexe range
595  void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
596 
597  // constrain the ekf states
598  void constrainStates();
599 
600  // generic function which will perform a fusion step given a kalman gain K
601  // and a scalar innovation value
602  void fuse(float *K, float innovation);
603 
604  // calculate the earth rotation vector from a given latitude
605  void calcEarthRateNED(Vector3f &omega, float lat_rad) const;
606 
607  // return true id the GPS quality is good enough to set an origin and start aiding
608  bool gps_is_good(const gps_message &gps);
609 
610  // Control the filter fusion modes
611  void controlFusionModes();
612 
613  // control fusion of external vision observations
615 
616  // control fusion of optical flow observations
618 
619  // control fusion of GPS observations
620  void controlGpsFusion();
621 
622  // control fusion of magnetometer observations
623  void controlMagFusion();
624  void updateMagFilter();
625 
626  bool canRunMagFusion() const;
627 
628  void checkHaglYawResetReq();
629  float getTerrainVPos() const;
630 
631  void runOnGroundYawReset();
632  bool isYawResetAuthorized() const;
633  bool canResetMagHeading() const;
634  void runInAirYawReset();
635  bool canRealignYawUsingGps() const;
636  void runVelPosReset();
637 
638  void selectMagAuto();
642  bool isYawAngleObservable() const;
643  bool isMagBiasObservable() const;
644  bool canUse3DMagFusion() const;
645 
646  void checkMagDeclRequired();
647  void checkMagInhibition();
648  bool shouldInhibitMag() const;
649  void checkMagFieldStrength();
650  bool isStrongMagneticDisturbance() const;
653  static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
655  void run3DMagAndDeclFusions();
656 
657  // control fusion of range finder observations
659 
660  // control fusion of air data observations
661  void controlAirDataFusion();
662 
663  // control fusion of synthetic sideslip observations
664  void controlBetaFusion();
665 
666  // control fusion of multi-rotor drag specific force observations
667  void controlDragFusion();
668 
669  // control fusion of pressure altitude observations
670  void controlBaroFusion();
671 
672  // control fusion of velocity and position observations
673  void controlVelPosFusion();
674 
675  // control fusion of auxiliary velocity observations
676  void controlAuxVelFusion();
677 
678  // control for height sensor timeouts, sensor changes and state resets
680 
681  // control for combined height fusion mode (implemented for switching between baro and range height)
682  void controlHeightFusion();
683 
684  // determine if flight condition is suitable to use range finder instead of the primary height sensor
687 
688  // update _rng_hgt_valid, which indicates if the current range sample has passed validity checks
690 
691  // check for "stuck" range finder measurements when rng was not valid for certain period
692  void updateRangeDataStuck();
693 
694  // return the square of two floating point numbers - used in auto coded sections
695  static constexpr float sq(float var) { return var * var; }
696 
697  // set control flags to use baro height
698  void setControlBaroHeight();
699 
700  // set control flags to use range height
701  void setControlRangeHeight();
702 
703  // set control flags to use GPS height
704  void setControlGPSHeight();
705 
706  // set control flags to use external vision height
707  void setControlEVHeight();
708 
709  void stopMagFusion();
710  void stopMag3DFusion();
711  void stopMagHdgFusion();
712  void startMagHdgFusion();
713  void startMag3DFusion();
714 
715  // zero the specified range of rows in the state covariance matrix
716  void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
717 
718  // zero the specified range of columns in the state covariance matrix
719  void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
720 
721  // zero the specified range of off diagonals in the state covariance matrix
722  void zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
723 
724  // zero the specified range of off diagonals in the state covariance matrix
725  // set the diagonals to the supplied value
726  void setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance);
727 
728  // calculate the measurement variance for the optical flow sensor
729  float calcOptFlowMeasVar();
730 
731  // rotate quaternion covariances into variances for an equivalent rotation vector
733 
734  // initialise the quaternion covariances using rotation vector variances
735  void initialiseQuatCovariances(Vector3f &rot_vec_var);
736 
737  // perform a limited reset of the magnetic field state covariances
739 
740  // perform a limited reset of the wind state covariances
741  void resetWindCovariance();
742 
743  // perform a reset of the wind states
744  void resetWindStates();
745 
746  // check that the range finder data is continuous
748 
750 
751  // Increase the yaw error variance of the quaternions
752  // Argument is additional yaw variance in rad**2
753  void increaseQuatYawErrVariance(float yaw_variance);
754 
755  // load and save mag field state covariance data for re-use
756  void loadMagCovData();
757  void saveMagCovData();
758  void clearMagCov();
759  void zeroMagCov();
760 
761  // uncorrelate quaternion states from other states
762  void uncorrelateQuatStates();
763 
764  // Use Kahan summation algorithm to get the sum of "sum_previous" and "input".
765  // This function relies on the caller to be responsible for keeping a copy of
766  // "accumulator" and passing this value at the next iteration.
767  // Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm
768  float kahanSummation(float sum_previous, float input, float &accumulator) const;
769 
770  // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
771  // sensor measurement
772  float calculate_synthetic_mag_z_measurement(Vector3f mag_meas, Vector3f mag_earth_predicted);
773 
774 };
bool canUse3DMagFusion() const
void fuseDeclination(float decl_sigma)
Definition: mag_fusion.cpp:815
Vector3f _vel_err_integ
integral of velocity tracking error (m)
Definition: ekf.h:406
void loadMagCovData()
float _cos_tilt_rng
cosine of the range finder tilt rotation about the Y body axis
Definition: ekf.h:464
uint64_t _time_bad_vert_accel
last time a bad vertical accel was detected (uSec)
Definition: ekf.h:477
void setControlBaroHeight()
float _posInnovGateNE
Number of standard deviations used for the NE position fusion innovation consistency check...
Definition: ekf.h:300
float _beta_innov
synthetic sideslip measurement innovation (rad)
Definition: ekf.h:382
void calculateOutputStates()
Definition: ekf.cpp:458
void clearMagCov()
Definition: covariance.cpp:917
bool _fuse_height
true when baro height data should be fused
Definition: ekf.h:293
float _gpsDriftVelN
GPS north position derivative (m/sec)
Definition: ekf.h:411
stateSample _state
state struct of the ekf running at the delayed time horizon
Definition: ekf.h:288
matrix::SquareMatrix< float, 24 > covariances() const
Definition: ekf.h:129
static struct vehicle_status_s status
Definition: Commander.cpp:138
float _vel_pos_innov_var[6]
NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) ...
Definition: ekf.h:373
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override
float _flow_innov[2]
flow measurement innovation (rad/sec)
Definition: ekf.h:392
uint64_t _time_last_mov_3d_mag_suitable
last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) ...
Definition: ekf.h:442
bool initialiseFilter(void)
Definition: ekf.cpp:152
void initialiseQuatCovariances(Vector3f &rot_vec_var)
bool resetGpsAntYaw()
float _dt_update
delta time since last ekf update. This time can be used for filters which run at the same rate as the...
Definition: ekf.h:286
float velD_change
Down velocity change due to last reset (m/sec)
Definition: ekf.h:279
float _saved_mag_ef_covmat[2][2]
NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) ...
Definition: ekf.h:444
static enum @74 state
void setControlGPSHeight()
uint64_t _ev_rot_last_time_us
previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) ...
Definition: ekf.h:313
Quatf calculate_quaternion() const
Definition: ekf.cpp:680
float _yaw_rate_lpf_ef
Filtered angular rate about earth frame D axis (rad/sec)
Definition: ekf.h:353
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)
Definition: ekf_helper.cpp:554
void checkMagBiasObservability()
float _saved_mag_bf_variance[4]
magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) ...
Definition: ekf.h:443
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override
Definition: ekf_helper.cpp:959
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
Definition: ekf.h:372
void controlBaroFusion()
void runOnGroundYawReset()
bool shouldInhibitMag() const
void get_state_delayed(float *state) override
Definition: ekf_helper.cpp:902
void get_heading_innov(float *heading_innov) override
Definition: ekf_helper.cpp:859
Dcm< float > Dcmf
Definition: Dcm.hpp:185
void get_pos_var(Vector3f &pos_var) override
Definition: covariance.cpp:121
float _last_static_yaw
last yaw angle recorded when on ground motion checks were passing (rad)
Definition: ekf.h:362
bool _mag_yaw_reset_req
true when a reset of the yaw using the magnetometer data has been requested
Definition: ekf.h:363
Vector2f _hpos_pred_prev
previous value of NE position state used by odometry fusion (m)
Definition: ekf.h:309
bool _synthetic_mag_z_active
true if we are generating synthetic magnetometer Z measurements
Definition: ekf.h:365
void updateTerrainValidity()
float _gps_alt_ref
WGS-84 height (m)
Definition: ekf.h:425
uint8_t posNE_counter
number of horizontal position reset events (allow to wrap if count exceeds 255)
Definition: ekf.h:275
void get_true_airspeed(float *tas) override
Vector3f _delta_vel_bias_var_accum
kahan summation algorithm accumulator for delta velocity bias variance
Definition: ekf.h:369
uint64_t _last_imu_bias_cov_reset_us
time the last reset of IMU delta angle and velocity state covariances was performed (uSec) ...
Definition: ekf.h:344
float _yaw_delta_ef
Recent change in yaw angle measured about the earth frame D axis (rad)
Definition: ekf.h:352
void controlHeightSensorTimeouts()
Definition: control.cpp:762
uint32_t _min_gps_health_time_us
GPS is marked as healthy only after this amount of time.
Definition: ekf.h:420
float get_terrain_var() const
Definition: ekf.h:211
void get_gyro_bias(float bias[3]) override
Definition: ekf_helper.cpp:948
bool resetVelocity()
Definition: ekf_helper.cpp:50
Vector3f _imu_del_ang_of
bias corrected delta angle measurements accumulated across the same time frame as the optical flow ra...
Definition: ekf.h:395
void updateRangeDataContinuity()
float _heading_innov_var
heading measurement innovation variance (rad**2)
Definition: ekf.h:389
void controlOpticalFlowFusion()
Definition: control.cpp:422
void selectMagAuto()
void runVelPosReset()
Vector2f _accel_lpf_NE
Low pass filtered horizontal earth frame acceleration (m/sec**2)
Definition: ekf.h:351
bool canRunMagFusion() const
bool _flow_for_terrain_data_ready
Definition: ekf.h:324
void get_output_tracking_error(float error[3]) override
Definition: ekf_helper.cpp:969
bool _ev_rot_mat_initialised
_ev_rot_mat should only be initialised once in the beginning through the reset function ...
Definition: ekf.h:314
uint64_t _time_bad_rng_signal_quality
timestamp at which range finder signal quality was 0 (used for hysteresis)
Definition: ekf.h:474
uint64_t _last_gps_pass_us
last system time in usec that the GPS passed it&#39;s checks
Definition: ekf.h:418
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
Definition: ekf.h:435
float _drag_innov[2]
multirotor drag measurement innovation (m/sec**2)
Definition: ekf.h:385
uint64_t _last_gps_origin_time_us
true when all active GPS checks have passed
Definition: ekf.h:424
void fuseSideslip()
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)
Definition: ekf.h:394
void get_mag_innov_var(float mag_innov_var[3]) override
Definition: ekf_helper.cpp:872
void controlBetaFusion()
Definition: control.cpp:1242
void initialiseCovariance()
Definition: covariance.cpp:49
bool _fuse_hpos_as_odom
true when the NE position data is being fused using an odometry assumption
Definition: ekf.h:307
void fuseAirspeed()
float _rng_stuck_max_val
maximum value for new rng measurement when being stuck
Definition: ekf.h:487
Vector3f _delta_angle_bias_var_accum
kahan summation algorithm accumulator for delta angle bias variance
Definition: ekf.h:370
float kahanSummation(float sum_previous, float input, float &accumulator) const
float _terrain_var
variance of terrain position estimate (m**2)
Definition: ekf.h:458
bool _mag_decl_cov_reset
true after the fuseDeclination() function has been used to modify the earth field covariances after a...
Definition: ekf.h:364
void resetWindStates()
uint64_t _last_gps_fail_us
last system time in usec that the GPS failed it&#39;s checks
Definition: ekf.h:417
void zeroMagCov()
Definition: covariance.cpp:923
void controlDragFusion()
Definition: control.cpp:1276
bool _fuse_vert_vel
true when gps vertical velocity measurement should be fused
Definition: ekf.h:296
bool gps_is_good(const gps_message &gps)
Definition: gps_checks.cpp:116
float _gps_velE_filt
GPS filtered East velocity (m/sec)
Definition: ekf.h:416
void get_imu_vibe_metrics(float vibe[3]) override
Definition: ekf_helper.cpp:980
void fuseGpsAntYaw()
void checkRangeAidSuitability()
Definition: control.cpp:1166
float _mag_innov[3]
earth magnetic field innovations (Gauss)
Definition: ekf.h:376
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 ...
Definition: ekf.h:483
imuSample _imu_down_sampled
down sampled imu data (sensor rate -> filter update rate)
Definition: ekf.h:404
bool collect_gps(const gps_message &gps) override
Definition: gps_checks.cpp:59
Vector2f posNE_change
North, East position change due to last reset (m)
Definition: ekf.h:280
float P[_k_num_states][_k_num_states]
state covariance matrix
Definition: ekf.h:367
uint64_t _time_good_vert_accel
last time a good vertical accel was detected (uSec)
Definition: ekf.h:478
uint64_t _time_bad_motion_us
last system time that on-ground motion exceeded limits (uSec)
Definition: ekf.h:397
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)
Definition: ekf.h:432
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)
Definition: ekf.h:337
bool _mag_inhibit_yaw_reset_req
true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions...
Definition: ekf.h:361
void calcExtVisRotMat()
void checkMagFieldStrength()
bool collect_imu(const imuSample &imu) override
Definition: ekf.cpp:378
float _terrain_vpos
estimated vertical position of the terrain underneath the vehicle in local NED frame (m) ...
Definition: ekf.h:457
float _rng_stuck_min_val
minimum value for new rng measurement when being stuck
Definition: ekf.h:486
bool _accel_bias_inhibit
true when the accel bias learning is being inhibited
Definition: ekf.h:450
float _heading_innov
heading measurement innovation (rad)
Definition: ekf.h:388
uint64_t _time_last_of_fuse
time the last fusion of optical flow measurements were performed (uSec)
Definition: ekf.h:334
uint64_t _time_last_hgt_fuse
time the last fusion of height measurements was performed (uSec)
Definition: ekf.h:333
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...
Definition: ekf.h:339
float _hagl_innov_var
innovation variance for the last height above terrain measurement (m**2)
Definition: ekf.h:460
void resetMagRelatedCovariances()
Definition: covariance.cpp:899
void saveMagCovData()
float posD_change
Down position change due to last reset (m)
Definition: ekf.h:281
float getMagDeclination()
Definition: ekf_helper.cpp:750
bool _baro_data_ready
true when new baro height data has fallen behind the fusion time horizon and is available to be fused...
Definition: ekf.h:319
void resetExtVisRotMat()
void predictState()
Definition: ekf.cpp:318
gps_check_fail_status_u _gps_check_fail_status
Definition: ekf.h:447
void controlFusionModes()
Definition: control.cpp:46
bool _fuse_hor_vel
true when gps horizontal velocity measurement should be fused
Definition: ekf.h:295
void fuse(float *K, float innovation)
void getHaglInnovVar(float *hagl_innov_var) override
AlphaFilterVector3f _mag_lpf
filtered magnetometer measurement (Gauss)
Definition: ekf.h:433
Vector3f _prev_dvel_bias_var
saved delta velocity XYZ bias variances (m/sec)**2
Definition: ekf.h:454
bool isStrongMagneticDisturbance() const
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
Definition: ekf.h:348
void get_airspeed_innov_var(float *airspeed_innov_var) override
Definition: ekf_helper.cpp:878
float _hvelInnovGate
Number of standard deviations used for the horizontal velocity fusion innovation consistency check...
Definition: ekf.h:303
void calcEarthRateNED(Vector3f &omega, float lat_rad) const
Definition: ekf_helper.cpp:820
void get_accel_bias(float bias[3]) override
Definition: ekf_helper.cpp:938
void controlExternalVisionFusion()
Definition: control.cpp:174
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
Definition: ekf.h:399
void controlHeightFusion()
Definition: control.cpp:996
void updateMagFilter()
virtual ~Ekf()=default
bool realignYawGPS()
Definition: ekf_helper.cpp:405
void get_heading_innov_var(float *heading_innov_var) override
Definition: ekf_helper.cpp:890
bool _ev_data_ready
true when new external vision system data has fallen behind the fusion time horizon and is available ...
Definition: ekf.h:322
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
Definition: ekf.h:330
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
Definition: ekf.h:473
bool _gps_checks_passed
Definition: ekf.h:421
bool _mag_use_inhibit_prev
true when magnetometer use was being inhibited the previous frame
Definition: ekf.h:360
Vector3f calcRotVecVariances()
float _beta_innov_var
synthetic sideslip measurement innovation variance (rad**2)
Definition: ekf.h:383
bool isYawResetAuthorized() const
bool _flt_mag_align_converging
true when the in-flight mag field post alignment convergence is being performd
Definition: ekf.h:440
void get_velD_reset(float *delta, uint8_t *counter) override
Definition: ekf.h:226
bool _terrain_initialised
true when the terrain estimator has been initialized
Definition: ekf.h:462
bool isMeasuredMatchingGpsMagStrength() const
void controlRangeFinderFusion()
void get_posNE_reset(float delta[2], uint8_t *counter) override
Definition: ekf.h:229
void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override
Definition: ekf_helper.cpp:866
void limitDeclination()
Definition: mag_fusion.cpp:955
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)...
Definition: ekf.h:400
bool canResetMagHeading() const
float getTerrainVPos() const
bool _baro_hgt_faulty
true if valid baro data is unavailable for use
Definition: ekf.h:470
float _output_tracking_error[3]
contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
Definition: ekf.h:408
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...
Definition: ekf.h:321
void reset(uint64_t timestamp) override
Definition: ekf.cpp:55
void stopMag3DFusion()
uint32_t _mag_counter
number of magnetometer samples read during initialisation
Definition: ekf.h:430
bool get_gps_drift_metrics(float drift[3], bool *blocked) override
Definition: ekf_helper.cpp:993
uint8_t posD_counter
number of vertical position reset events (allow to wrap if count exceeds 255)
Definition: ekf.h:276
Vector2< float > Vector2f
Definition: Vector2.hpp:69
float _drag_innov_var[2]
multirotor drag measurement innovation variance ((m/sec**2)**2)
Definition: ekf.h:386
void fuseFlowForTerrain()
uint8_t quat_counter
number of quaternion reset events (allow to wrap if count exceeds 255)
Definition: ekf.h:277
uint32_t _ev_counter
number of external vision samples read during initialisation
Definition: ekf.h:431
void get_airspeed_innov(float *airspeed_innov) override
Definition: ekf_helper.cpp:847
Vector3f _earth_rate_NED
earth rotation vector (NED) in rad/s
Definition: ekf.h:346
void setDiag(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)
void fuseHagl()
float _aux_vel_innov[2]
NE auxiliary velocity innovations: (m/sec)
Definition: ekf.h:374
void checkMagInhibition()
static unsigned counter
Definition: safety.c:56
float _gps_drift_velD
GPS down position derivative (m/sec)
Definition: ekf.h:413
void stopMagHdgFusion()
static constexpr float sq(float var)
Definition: ekf.h:695
bool _tas_data_ready
true when new true airspeed data has fallen behind the fusion time horizon and is available to be fus...
Definition: ekf.h:323
float _mag_innov_var[3]
earth magnetic field innovation variance (Gauss**2)
Definition: ekf.h:377
void controlMagFusion()
Definition: mag_control.cpp:42
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)
Definition: ekf.h:396
bool _velpos_reset_request
true when a large yaw error has been fixed and a velocity and position state reset is required ...
Definition: ekf.h:445
uint64_t _time_last_fake_gps
same flag as "_flow_data_ready" but used for separate terrain estimator
Definition: ekf.h:326
float _airspeed_innov_var
airspeed measurement innovation variance ((m/sec)**2)
Definition: ekf.h:380
void get_posD_reset(float *delta, uint8_t *counter) override
Definition: ekf.h:223
void increaseQuatYawErrVariance(float yaw_variance)
float _posObsNoiseNE
1-STD observation noise used for the fusion of NE position data (m)
Definition: ekf.h:299
void controlAuxVelFusion()
Definition: control.cpp:1364
bool _hpos_prev_available
true when previous values of the estimate and measurement are available for use
Definition: ekf.h:310
void fixCovarianceErrors()
Definition: covariance.cpp:728
uint64_t _time_acc_bias_check
last time the accel bias check passed (uSec)
Definition: ekf.h:341
bool canRealignYawUsingGps() const
Dcmf _ev_rot_mat
transformation matrix that rotates observations from the EV to the EKF navigation frame...
Definition: ekf.h:312
void checkMagDeclRequired()
uint64_t _time_ins_deadreckon_start
amount of time we have been doing inertial only deadreckoning (uSec)
Definition: ekf.h:327
bool _rng_hgt_valid
true if range finder sample retrieved from buffer is valid
Definition: ekf.h:472
void setControlRangeHeight()
void get_flow_innov(float flow_innov[2]) override
void fuseMag()
Definition: mag_fusion.cpp:47
void setControlEVHeight()
uint8_t velD_counter
number of vertical velocity reset events (allow to wrap if count exceeds 255)
Definition: ekf.h:274
void resetHeight()
Definition: ekf_helper.cpp:214
void alignOutputFilter()
Definition: ekf_helper.cpp:378
void get_mag_innov(float mag_innov[3]) override
Definition: ekf_helper.cpp:841
void resetWindCovariance()
Definition: covariance.cpp:929
void get_aux_vel_innov(float aux_vel_innov[2]) override
Definition: ekf_helper.cpp:835
void fuseVelPosHeight()
void runTerrainEstimator()
bool _mag_use_inhibit
true when magnetometer use is being inhibited
Definition: ekf.h:359
void getTerrainVertPos(float *ret) override
void get_vel_var(Vector3f &vel_var) override
Definition: covariance.cpp:128
Vector3< float > Vector3f
Definition: Vector3.hpp:136
void makeSymmetrical(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:773
bool _hagl_valid
true when the height above ground estimate is valid
Definition: ekf.h:467
float _accel_mag_filt
acceleration magnitude after application of a decaying envelope filter (rad/sec)
Definition: ekf.h:452
float _R_rng_to_earth_2_2
2,2 element of the rotation matrix from sensor frame to earth frame
Definition: ekf.h:465
void constrainStates()
Definition: ekf_helper.cpp:784
float _hagl_innov
innovation of the last height above terrain measurement (m)
Definition: ekf.h:459
matrix::SquareMatrix< float, 4 > orientation_covariances() const
Definition: ekf.h:135
float _vvelInnovGate
Number of standard deviations used for the vertical velocity fusion innovation consistency check...
Definition: ekf.h:304
#define VDIST_SENSOR_BARO
Use baro height.
Definition: common.h:178
matrix::Vector< float, 24 > covariances_diagonal() const
Definition: ekf.h:132
bool _filter_initialised
true when the EKF sttes and covariances been initialised
Definition: ekf.h:290
uint64_t _time_last_arsp_fuse
time the last fusion of airspeed measurements were performed (uSec)
Definition: ekf.h:335
bool _gps_hgt_intermittent
true if gps height into the buffer is intermittent
Definition: ekf.h:471
void get_velNE_reset(float delta[2], uint8_t *counter) override
Definition: ekf.h:236
float _gps_velD_diff_filt
GPS filtered Down velocity (m/sec)
Definition: ekf.h:414
Definition: ekf.h:47
uint64_t _flt_mag_align_start_time
time that inflight magnetic field alignment started (uSec)
Definition: ekf.h:441
float _sin_tilt_rng
sine of the range finder tilt rotation about the Y body axis
Definition: ekf.h:463
uint64_t _time_yaw_started
last system time in usec that a yaw rotation manoeuvre was detected
Definition: ekf.h:356
float _flow_innov_var[2]
flow innovation variance ((rad/sec)**2)
Definition: ekf.h:393
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
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...
Definition: ekf.h:282
void set_min_required_gps_health_time(uint32_t time_us)
Definition: ekf.h:266
void controlVelPosFusion()
Definition: control.cpp:1299
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)
Definition: ekf.h:398
void get_vel_pos_innov(float vel_pos_innov[6]) override
Definition: ekf_helper.cpp:829
void controlGpsFusion()
Definition: control.cpp:576
Vector3f _pos_meas_prev
previous value of NED position measurement fused using odometry assumption (m)
Definition: ekf.h:308
bool _fuse_pos
true when gps position data should be fused
Definition: ekf.h:294
bool isYawAngleObservable() const
bool _yaw_angle_observable
true when there is enough horizontal acceleration to make yaw observable
Definition: ekf.h:355
Vector2f _last_known_posNE
last known local NE position vector (m)
Definition: ekf.h:338
Vector3f _velObsVarNED
1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 ...
Definition: ekf.h:302
float _dt_last_range_update_filt_us
filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)...
Definition: ekf.h:466
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
Definition: ekf.h:331
Vector3f _delVel_sum
summed delta velocity (m/sec)
Definition: ekf.h:434
bool isRangeDataContinuous()
Definition: ekf.h:749
void startMag3DFusion()
Ekf()=default
bool initHagl()
void get_gps_check_status(uint16_t *val) override
Definition: ekf_helper.cpp:896
void run3DMagAndDeclFusions()
float _airspeed_innov
airspeed measurement innovation (m/sec)
Definition: ekf.h:379
void fuseDrag()
Definition: drag_fusion.cpp:46
uint64_t _delta_time_baro_us
delta time between two consecutive delayed baro samples from the buffer (uSec)
Definition: ekf.h:342
float _last_on_ground_posD
last vertical position when the in_air status was false (m)
Definition: ekf.h:439
void checkHaglYawResetReq()
void predictCovariance()
Definition: covariance.cpp:135
uint8_t _num_bad_flight_yaw_events
number of times a bad heading has been detected in flight and required a yaw reset ...
Definition: ekf.h:357
void runInAirYawReset()
uint64_t _time_last_vel_fuse
time the last fusion of velocity measurements was performed (uSec)
Definition: ekf.h:332
float _ang_rate_mag_filt
angular rate magnitude after application of a decaying envelope filter (rad/sec)
Definition: ekf.h:453
bool isRangeAidSuitable()
Definition: ekf.h:686
void fuseHeading()
Definition: mag_fusion.cpp:438
void getHaglInnov(float *hagl_innov) override
float _baro_hgt_offset
baro height reading at the local NED origin (m)
Definition: ekf.h:436
float _gpsDriftVelE
GPS east position derivative (m/sec)
Definition: ekf.h:412
void get_wind_velocity_var(float *wind_var) override
uint32_t _hgt_counter
number of height samples read during initialisation
Definition: ekf.h:428
void controlAirDataFusion()
Definition: control.cpp:1199
bool init(uint64_t timestamp) override
Definition: ekf.cpp:47
static constexpr float FILTER_UPDATE_PERIOD_S
void check3DMagFusionSuitability()
Quatf _q_down_sampled
down sampled quaternion (tracking delta angles between ekf update steps)
Definition: ekf.h:405
bool _bad_vert_accel_detected
true when bad vertical accelerometer data has been detected
Definition: ekf.h:479
void uncorrelateQuatStates()
matrix::SquareMatrix< float, 3 > position_covariances() const
Definition: ekf.h:141
matrix::SquareMatrix< float, 3 > velocity_covariances() const
Definition: ekf.h:138
void get_quat_reset(float delta_quat[4], uint8_t *counter) override
Definition: ekf.h:243
void stopMagFusion()
float _gps_velN_filt
GPS filtered North velocity (m/sec)
Definition: ekf.h:415
bool _earth_rate_initialised
true when we know the earth rotatin rate (requires GPS)
Definition: ekf.h:291
Vector3f _ev_rot_vec_filt
filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (ra...
Definition: ekf.h:311
void updateRangeDataStuck()
void updateRangeDataValidity()
void fuseOptFlow()
Vector3f _delta_angle_corr
delta angle correction vector (rad)
Definition: ekf.h:403
void get_beta_innov(float *beta_innov) override
Definition: ekf_helper.cpp:853
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
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)
Definition: ekf.h:336
Vector3f _accel_vec_filt
acceleration vector after application of a low pass filter (m/sec**2)
Definition: ekf.h:451
uint64_t _time_last_hagl_fuse
last system time that the hagl measurement failed it&#39;s checks (uSec)
Definition: ekf.h:461
float _dt_ekf_avg
average update rate of the ekf
Definition: ekf.h:285
uint8_t velNE_counter
number of horizontal position reset events (allow to wrap if count exceeds 255)
Definition: ekf.h:273
bool _gps_data_ready
true when new GPS data has fallen behind the fusion time horizon and is available to be fused ...
Definition: ekf.h:317
bool resetPosition()
Definition: ekf_helper.cpp:139
bool calcOptFlowBodyRateComp()
bool isMagBiasObservable() const
void get_drag_innov(float drag_innov[2]) override
bool update() override
Definition: ekf.cpp:117
bool isTerrainEstimateValid() const override
Vector3f _pos_err_integ
integral of position tracking error (m.s)
Definition: ekf.h:407
void startMagHdgFusion()
float _height_rate_lpf
Definition: ekf.h:489
void get_beta_innov_var(float *beta_innov_var) override
Definition: ekf_helper.cpp:884
Vector2f velNE_change
North East velocity change due to last reset (m)
Definition: ekf.h:278
bool _using_synthetic_position
true if we are using a synthetic position to constrain drift
Definition: ekf.h:328
uint64_t _mag_use_not_inhibit_us
last system time in usec before magnetometer use was inhibited
Definition: ekf.h:358
bool _mag_data_ready
true when new magnetometer data has fallen behind the fusion time horizon and is available to be fuse...
Definition: ekf.h:318
float _gps_error_norm
normalised gps error
Definition: ekf.h:419
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...
Definition: ekf.h:320
bool isMeasuredMatchingAverageMagStrength() const
float _rng_filt_state
filtered height measurement (m)
Definition: ekf.h:429
bool _mag_bias_observable
true when there is enough rotation to make magnetometer bias errors observable
Definition: ekf.h:354
bool _fuse_hor_vel_aux
true when auxiliary horizontal velocity measurement should be fused
Definition: ekf.h:297
void resetStates() override
Definition: ekf.cpp:99
bool _is_range_aid_suitable
true when range finder can be used in flight as the height reference instead of the primary height se...
Definition: ekf.h:482
void resetStatesAndCovariances() override
Definition: ekf.cpp:93
void checkYawAngleObservability()