PX4 Firmware
PX4 Autopilot Software http://px4.io
ekf_helper.cpp
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_helper.cpp
36  * Definition of ekf helper functions.
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  *
40  */
41 
42 #include "ekf.h"
43 
44 #include <ecl.h>
45 #include <mathlib/mathlib.h>
46 #include <cstdlib>
47 
48 // Reset the velocity states. If we have a recent and valid
49 // gps measurement then use for velocity initialisation
51 {
52  // used to calculate the velocity change due to the reset
53  Vector3f vel_before_reset = _state.vel;
54 
55  // reset EKF states
57  // this reset is only called if we have new gps data at the fusion time horizon
59 
60  // use GPS accuracy to reset variances
62 
63  } else if (_control_status.flags.opt_flow) {
64  // constrain height above ground to be above minimum possible
65  float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
66 
67  // calculate absolute distance from focal point to centre of frame assuming a flat earth
68  float range = heightAboveGndEst / _R_rng_to_earth_2_2;
69 
70  if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
71  // we should have reliable OF measurements so
72  // calculate X and Y body relative velocities from OF measurements
73  Vector3f vel_optflow_body;
74  vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt;
75  vel_optflow_body(1) = range * _flowRadXYcomp(0) / _flow_sample_delayed.dt;
76  vel_optflow_body(2) = 0.0f;
77 
78  // rotate from body to earth frame
79  Vector3f vel_optflow_earth;
80  vel_optflow_earth = _R_to_earth * vel_optflow_body;
81 
82  // take x and Y components
83  _state.vel(0) = vel_optflow_earth(0);
84  _state.vel(1) = vel_optflow_earth(1);
85 
86  } else {
87  _state.vel(0) = 0.0f;
88  _state.vel(1) = 0.0f;
89  }
90 
91  // reset the velocity covariance terms
92  zeroRows(P, 4, 5);
93  zeroCols(P, 4, 5);
94 
95  // reset the horizontal velocity variance using the optical flow noise variance
96  P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
97  } else if (_control_status.flags.ev_vel) {
101  }
102  _state.vel(0) = _ev_vel(0);
103  _state.vel(1) = _ev_vel(1);
104  _state.vel(2) = _ev_vel(2);
106  } else if (_control_status.flags.ev_pos) {
107  _state.vel.setZero();
108  zeroOffDiag(P, 4, 6);
109  } else {
110  // Used when falling back to non-aiding mode of operation
111  _state.vel(0) = 0.0f;
112  _state.vel(1) = 0.0f;
113  setDiag(P, 4, 5, 25.0f);
114  }
115 
116  // calculate the change in velocity and apply to the output predictor state history
117  const Vector3f velocity_change = _state.vel - vel_before_reset;
118 
119  for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
120  _output_buffer[index].vel += velocity_change;
121  }
122 
123  // apply the change in velocity to our newest velocity estimate
124  // which was already taken out from the output buffer
125  _output_new.vel += velocity_change;
126 
127  // capture the reset event
128  _state_reset_status.velNE_change(0) = velocity_change(0);
129  _state_reset_status.velNE_change(1) = velocity_change(1);
130  _state_reset_status.velD_change = velocity_change(2);
131  _state_reset_status.velNE_counter++;
132  _state_reset_status.velD_counter++;
133 
134  return true;
135 }
136 
137 // Reset position states. If we have a recent and valid
138 // gps measurement then use for position initialisation
140 {
141  // used to calculate the position change due to the reset
142  Vector2f posNE_before_reset;
143  posNE_before_reset(0) = _state.pos(0);
144  posNE_before_reset(1) = _state.pos(1);
145 
146  // let the next odometry update know that the previous value of states cannot be used to calculate the change in position
147  _hpos_prev_available = false;
148 
149  if (_control_status.flags.gps) {
150  // this reset is only called if we have new gps data at the fusion time horizon
153 
154  // use GPS accuracy to reset variances
156 
157  } else if (_control_status.flags.ev_pos) {
158  // this reset is only called if we have new ev data at the fusion time horizon
159  Vector3f _ev_pos = _ev_sample_delayed.pos;
162  }
163  _state.pos(0) = _ev_pos(0);
164  _state.pos(1) = _ev_pos(1);
165 
166  // use EV accuracy to reset variances
168 
169  } else if (_control_status.flags.opt_flow) {
171  // we are likely starting OF for the first time so reset the horizontal position
172  _state.pos(0) = 0.0f;
173  _state.pos(1) = 0.0f;
174 
175  } else {
176  // set to the last known position
177  _state.pos(0) = _last_known_posNE(0);
178  _state.pos(1) = _last_known_posNE(1);
179 
180  }
181 
182  // estimate is relative to initial position in this mode, so we start with zero error.
183  zeroCols(P,7,8);
184  zeroRows(P,7,8);
185 
186  } else {
187  // Used when falling back to non-aiding mode of operation
188  _state.pos(0) = _last_known_posNE(0);
189  _state.pos(1) = _last_known_posNE(1);
191  }
192 
193  // calculate the change in position and apply to the output predictor state history
194  const Vector2f posNE_change{_state.pos(0) - posNE_before_reset(0), _state.pos(1) - posNE_before_reset(1)};
195 
196  for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
197  _output_buffer[index].pos(0) += posNE_change(0);
198  _output_buffer[index].pos(1) += posNE_change(1);
199  }
200 
201  // apply the change in position to our newest position estimate
202  // which was already taken out from the output buffer
203  _output_new.pos(0) += posNE_change(0);
204  _output_new.pos(1) += posNE_change(1);
205 
206  // capture the reset event
207  _state_reset_status.posNE_change = posNE_change;
208  _state_reset_status.posNE_counter++;
209 
210  return true;
211 }
212 
213 // Reset height state using the last height measurement
215 {
216  // Get the most recent GPS data
217  const gpsSample &gps_newest = _gps_buffer.get_newest();
218 
219  // store the current vertical position and velocity for reference so we can calculate and publish the reset amount
220  float old_vert_pos = _state.pos(2);
221  bool vert_pos_reset = false;
222  float old_vert_vel = _state.vel(2);
223  bool vert_vel_reset = false;
224 
225  // reset the vertical position
227 
229 
230  // update the state and associated variance
231  _state.pos(2) = new_pos_down;
232 
233  // reset the associated covariance values
234  zeroRows(P, 9, 9);
235  zeroCols(P, 9, 9);
236 
237  // the state variance is the same as the observation
238  P[9][9] = sq(_params.range_noise);
239 
240  vert_pos_reset = true;
241 
242  // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
243  const baroSample &baro_newest = _baro_buffer.get_newest();
244  _baro_hgt_offset = baro_newest.hgt + _state.pos(2);
245 
246  } else if (_control_status.flags.baro_hgt) {
247  // initialize vertical position with newest baro measurement
248  const baroSample &baro_newest = _baro_buffer.get_newest();
249 
250  if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
251  _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
252 
253  // reset the associated covariance values
254  zeroRows(P, 9, 9);
255  zeroCols(P, 9, 9);
256 
257  // the state variance is the same as the observation
258  P[9][9] = sq(_params.baro_noise);
259 
260  vert_pos_reset = true;
261 
262  } else {
263  // TODO: reset to last known baro based estimate
264  }
265 
266  } else if (_control_status.flags.gps_hgt) {
267  // initialize vertical position and velocity with newest gps measurement
268  if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
269  _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
270 
271  // reset the associated covariance values
272  zeroRows(P, 9, 9);
273  zeroCols(P, 9, 9);
274 
275  // the state variance is the same as the observation
276  P[9][9] = sq(gps_newest.hacc);
277 
278  vert_pos_reset = true;
279 
280  // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
281  const baroSample &baro_newest = _baro_buffer.get_newest();
282  _baro_hgt_offset = baro_newest.hgt + _state.pos(2);
283 
284  } else {
285  // TODO: reset to last known gps based estimate
286  }
287 
288  } else if (_control_status.flags.ev_hgt) {
289  // initialize vertical position with newest measurement
290  const extVisionSample &ev_newest = _ext_vision_buffer.get_newest();
291 
292  // use the most recent data if it's time offset from the fusion time horizon is smaller
293  int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
294  int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us;
295 
296  vert_pos_reset = true;
297 
298  if (std::abs(dt_newest) < std::abs(dt_delayed)) {
299  _state.pos(2) = ev_newest.pos(2);
300 
301  } else {
303  }
304 
305  }
306 
307  // reset the vertical velocity covariance values
308  zeroRows(P, 6, 6);
309  zeroCols(P, 6, 6);
310 
311  // reset the vertical velocity state
312  if (_control_status.flags.gps && (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL)) {
313  // If we are using GPS, then use it to reset the vertical velocity
314  _state.vel(2) = gps_newest.vel(2);
315 
316  // the state variance is the same as the observation
317  P[6][6] = sq(1.5f * gps_newest.sacc);
318 
319  } else {
320  // we don't know what the vertical velocity is, so set it to zero
321  _state.vel(2) = 0.0f;
322 
323  // Set the variance to a value large enough to allow the state to converge quickly
324  // that does not destabilise the filter
325  P[6][6] = 10.0f;
326 
327  }
328 
329  vert_vel_reset = true;
330 
331  // store the reset amount and time to be published
332  if (vert_pos_reset) {
333  _state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
334  _state_reset_status.posD_counter++;
335  }
336 
337  if (vert_vel_reset) {
338  _state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
339  _state_reset_status.velD_counter++;
340  }
341 
342  // apply the change in height / height rate to our newest height / height rate estimate
343  // which have already been taken out from the output buffer
344  if (vert_pos_reset) {
345  _output_new.pos(2) += _state_reset_status.posD_change;
346  }
347 
348  if (vert_vel_reset) {
349  _output_new.vel(2) += _state_reset_status.velD_change;
350  }
351 
352  // add the reset amount to the output observer buffered data
353  for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
354  if (vert_pos_reset) {
355  _output_buffer[i].pos(2) += _state_reset_status.posD_change;
356  _output_vert_buffer[i].vel_d_integ += _state_reset_status.posD_change;
357  }
358 
359  if (vert_vel_reset) {
360  _output_buffer[i].vel(2) += _state_reset_status.velD_change;
361  _output_vert_buffer[i].vel_d += _state_reset_status.velD_change;
362  }
363  }
364 
365  // add the reset amount to the output observer vertical position state
366  if (vert_pos_reset) {
369  }
370 
371  if (vert_vel_reset) {
374  }
375 }
376 
377 // align output filter states to match EKF states at the fusion time horizon
379 {
380  // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
382  q_delta.normalize();
383 
384  // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
385  const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
386  const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
387 
388  // loop through the output filter state history and add the deltas
389  for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
390  _output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal;
391  _output_buffer[i].quat_nominal.normalize();
392  _output_buffer[i].vel += vel_delta;
393  _output_buffer[i].pos += pos_delta;
394  }
395 
397  _output_new.quat_nominal.normalize();
398 
401 }
402 
403 // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
404 // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
406 {
407  // Need at least 5 m/s of GPS horizontal speed and ratio of velocity error to velocity < 0.15 for a reliable alignment
408  float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
409 
410  if ((gpsSpeed > 5.0f) && (_gps_sample_delayed.sacc < (0.15f * gpsSpeed))) {
411  // check for excessive GPS velocity innovations
412  bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
413 
414  // calculate GPS course over ground angle
415  float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
416 
417  // calculate course yaw angle
418  float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
419 
420  // Check the EKF and GPS course over ground for consistency
421  float courseYawError = gpsCOG - ekfGOG;
422 
423  // If the angles disagree and horizontal GPS velocity innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
424  bool badYawErr = fabsf(courseYawError) > 0.5f;
425  bool badMagYaw = (badYawErr && badVelInnov);
426 
427  if (badMagYaw) {
429  }
430 
431  // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned
432  if (badMagYaw || !_control_status.flags.yaw_align) {
433  ECL_WARN_TIMESTAMPED("EKF bad yaw corrected using GPS course");
434 
435  // declare the magnetometer as failed if a bad yaw has occurred more than once
437  ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use");
439  }
440 
441  // save a copy of the quaternion state for later use in calculating the amount of reset change
442  Quatf quat_before_reset = _state.quat_nominal;
443 
444  // update transformation matrix from body to world frame using the current state estimate
446 
447  // get quaternion from existing filter states and calculate roll, pitch and yaw angles
448  Eulerf euler321(_state.quat_nominal);
449 
450  // apply yaw correction
452  // This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
453  // forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
454  euler321(2) += courseYawError;
456 
457  } else if (_control_status.flags.wind) {
458  // we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
459  // aligned with the wind relative GPS velocity vector
460  euler321(2) = atan2f((_gps_sample_delayed.vel(1) - _state.wind_vel(1)),
462 
463  } else {
464  // we don't have wind estimates, so align yaw to the GPS velocity vector
465  euler321(2) = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
466 
467  }
468 
469  // calculate new filter quaternion states using corrected yaw angle
470  _state.quat_nominal = Quatf(euler321);
472 
473  // If heading was bad, then we also need to reset the velocity and position states
474  _velpos_reset_request = badMagYaw;
475 
476  // update transformation matrix from body to world frame using the current state estimate
478 
479  // Use the last magnetometer measurements to reset the field states
480  _state.mag_B.zero();
482 
483  // use the combined EKF and GPS speed variance to calculate a rough estimate of the yaw error after alignment
484  float SpdErrorVariance = sq(_gps_sample_delayed.sacc) + P[4][4] + P[5][5];
485  float sineYawError = math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0f, 1.0f);
486 
487  // adjust the quaternion covariances estimated yaw error
488  increaseQuatYawErrVariance(sq(asinf(sineYawError)));
489 
490  // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
491  clearMagCov();
492 
494  for (uint8_t index = 16; index <= 21; index ++) {
495  P[index][index] = sq(_params.mag_noise);
496  }
497 
498  // save covariance data for re-use when auto-switching between heading and 3-axis fusion
499  saveMagCovData();
500  }
501 
502  // record the start time for the magnetic field alignment
504 
505  // calculate the amount that the quaternion has changed by
506  _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
507 
508  // add the reset amount to the output observer buffered data
509  for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
510  _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
511  }
512 
513  // apply the change in attitude quaternion to our newest quaternion estimate
514  // which was already taken out from the output buffer
516 
517  // capture the reset event
518  _state_reset_status.quat_counter++;
519 
520  return true;
521 
522  } else {
523  // align mag states only
524 
525  // calculate initial earth magnetic field states
527 
528  // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
529  clearMagCov();
530 
532  for (uint8_t index = 16; index <= 21; index ++) {
533  P[index][index] = sq(_params.mag_noise);
534  }
535 
536  // save covariance data for re-use when auto-switching between heading and 3-axis fusion
537  saveMagCovData();
538  }
539 
540  // record the start time for the magnetic field alignment
542 
543  return true;
544  }
545 
546  } else {
547  // attempt a normal alignment using the magnetometer
549 
550  }
551 }
552 
553 // Reset heading and magnetic field states
554 bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
555 {
556  // prevent a reset being performed more than once on the same frame
558  return true;
559  }
560 
562  stopMagFusion();
563  return false;
564  }
565 
566  // save a copy of the quaternion state for later use in calculating the amount of reset change
567  Quatf quat_before_reset = _state.quat_nominal;
568  Quatf quat_after_reset = _state.quat_nominal;
569 
570  // update transformation matrix from body to world frame using the current estimate
572 
573  // calculate the initial quaternion
574  // determine if a 321 or 312 Euler sequence is best
575  if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
576  // use a 321 sequence
577 
578  // rotate the magnetometer measurement into earth frame
579  Eulerf euler321(_state.quat_nominal);
580 
581  // Set the yaw angle to zero and calculate the rotation matrix from body to earth frame
582  euler321(2) = 0.0f;
583  Dcmf R_to_earth(euler321);
584 
585  // calculate the observed yaw angle
587  // convert the observed quaternion to a rotation matrix
588  Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
589  // calculate the yaw angle for a 312 sequence
590  euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
591 
592  } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
593  // rotate the magnetometer measurements into earth frame using a zero yaw angle
594  Vector3f mag_earth_pred = R_to_earth * mag_init;
595  // the angle of the projection onto the horizontal gives the yaw angle
596  euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
597 
599  // we are operating without knowing the earth frame yaw angle
600  return true;
601 
602  } else {
603  // there is no yaw observation
604  return false;
605  }
606 
607  // calculate initial quaternion states for the ekf
608  // we don't change the output attitude to avoid jumps
609  quat_after_reset = Quatf(euler321);
610 
611  } else {
612  // use a 312 sequence
613 
614  // Calculate the 312 sequence euler angles that rotate from earth to body frame
615  // See http://www.atacolorado.com/eulersequences.doc
616  Vector3f euler312;
617  euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
618  euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
619  euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
620 
621  // Set the first rotation (yaw) to zero and calculate the rotation matrix from body to earth frame
622  euler312(0) = 0.0f;
623 
624  // Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
625  float c2 = cosf(euler312(2));
626  float s2 = sinf(euler312(2));
627  float s1 = sinf(euler312(1));
628  float c1 = cosf(euler312(1));
629  float s0 = sinf(euler312(0));
630  float c0 = cosf(euler312(0));
631 
632  Dcmf R_to_earth;
633  R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
634  R_to_earth(1, 1) = c0 * c1;
635  R_to_earth(2, 2) = c2 * c1;
636  R_to_earth(0, 1) = -c1 * s0;
637  R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
638  R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
639  R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
640  R_to_earth(2, 0) = -s2 * c1;
641  R_to_earth(2, 1) = s1;
642 
643  // calculate the observed yaw angle
645  // convert the observed quaternion to a rotation matrix
646  Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
647  // calculate the yaw angle for a 312 sequence
648  euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
649 
650  } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
651  // rotate the magnetometer measurements into earth frame using a zero yaw angle
652  Vector3f mag_earth_pred = R_to_earth * mag_init;
653  // the angle of the projection onto the horizontal gives the yaw angle
654  euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
655 
657  // we are operating without knowing the earth frame yaw angle
658  return true;
659 
660  } else {
661  // there is no yaw observation
662  return false;
663  }
664 
665  // re-calculate the rotation matrix using the updated yaw angle
666  s0 = sinf(euler312(0));
667  c0 = cosf(euler312(0));
668  R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
669  R_to_earth(1, 1) = c0 * c1;
670  R_to_earth(2, 2) = c2 * c1;
671  R_to_earth(0, 1) = -c1 * s0;
672  R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
673  R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
674  R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
675  R_to_earth(2, 0) = -s2 * c1;
676  R_to_earth(2, 1) = s1;
677 
678  // calculate initial quaternion states for the ekf
679  // we don't change the output attitude to avoid jumps
680  quat_after_reset = Quatf(R_to_earth);
681  }
682 
683  // set the earth magnetic field states using the updated rotation
684  Dcmf R_to_earth_after(quat_after_reset);
685  _state.mag_I = R_to_earth_after * mag_init;
686 
687  // reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
688  clearMagCov();
689 
691  for (uint8_t index = 16; index <= 21; index ++) {
692  P[index][index] = sq(_params.mag_noise);
693  }
694 
695  // save covariance data for re-use when auto-switching between heading and 3-axis fusion
696  saveMagCovData();
697  }
698 
699  // record the time for the magnetic field alignment event
701 
702  // calculate the amount that the quaternion has changed by
703  Quatf q_error = quat_after_reset * quat_before_reset.inversed();
704  q_error.normalize();
705 
706  // update quaternion states
707  _state.quat_nominal = quat_after_reset;
709 
710  // record the state change
711  _state_reset_status.quat_change = q_error;
712 
713  // update transformation matrix from body to world frame using the current estimate
715 
716  // reset the rotation from the EV to EKF frame of reference if it is being used
719  }
720 
721  if (increase_yaw_var) {
722  // update the yaw angle variance using the variance of the measurement
724  // using error estimate from external vision data
726  } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
727  // using magnetic heading tuning parameter
729  }
730  }
731 
732  if (update_buffer) {
733  // add the reset amount to the output observer buffered data
734  for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
735  _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
736  }
737 
738  // apply the change in attitude quaternion to our newest quaternion estimate
739  // which was already taken out from the output buffer
741  }
742 
743  // capture the reset event
744  _state_reset_status.quat_counter++;
745 
746  return true;
747 }
748 
749 // Return the magnetic declination in radians to be used by the alignment and fusion processing
751 {
752  // set source of magnetic declination for internal use
754  // Use value consistent with earth field state
755  return atan2f(_state.mag_I(1), _state.mag_I(0));
756 
758  // use parameter value until GPS is available, then use value returned by geo library
760  return _mag_declination_gps;
761 
762  } else {
764  }
765 
766  } else {
767  // always use the parameter value
769  }
770 }
771 
772 // This function forces the covariance matrix to be symmetric
773 void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
774 {
775  for (unsigned row = first; row <= last; row++) {
776  for (unsigned column = 0; column < row; column++) {
777  float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2;
778  cov_mat[row][column] = tmp;
779  cov_mat[column][row] = tmp;
780  }
781  }
782 }
783 
785 {
786  for (int i = 0; i < 4; i++) {
787  _state.quat_nominal(i) = math::constrain(_state.quat_nominal(i), -1.0f, 1.0f);
788  }
789 
790  for (int i = 0; i < 3; i++) {
791  _state.vel(i) = math::constrain(_state.vel(i), -1000.0f, 1000.0f);
792  }
793 
794  for (int i = 0; i < 3; i++) {
795  _state.pos(i) = math::constrain(_state.pos(i), -1.e6f, 1.e6f);
796  }
797 
798  for (int i = 0; i < 3; i++) {
800  }
801 
802  for (int i = 0; i < 3; i++) {
804  }
805 
806  for (int i = 0; i < 3; i++) {
807  _state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f);
808  }
809 
810  for (int i = 0; i < 3; i++) {
811  _state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f);
812  }
813 
814  for (int i = 0; i < 2; i++) {
815  _state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f);
816  }
817 }
818 
819 // calculate the earth rotation vector
820 void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
821 {
822  omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
823  omega(1) = 0.0f;
824  omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
825 }
826 
827 // gets the innovations of velocity and position measurements
828 // 0-2 vel, 3-5 pos
829 void Ekf::get_vel_pos_innov(float vel_pos_innov[6])
830 {
831  memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6);
832 }
833 
834 // gets the innovations for of the NE auxiliary velocity measurement
835 void Ekf::get_aux_vel_innov(float aux_vel_innov[2])
836 {
837  memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2);
838 }
839 
840 // writes the innovations of the earth magnetic field measurements
841 void Ekf::get_mag_innov(float mag_innov[3])
842 {
843  memcpy(mag_innov, _mag_innov, 3 * sizeof(float));
844 }
845 
846 // gets the innovations of the airspeed measurement
847 void Ekf::get_airspeed_innov(float *airspeed_innov)
848 {
849  memcpy(airspeed_innov, &_airspeed_innov, sizeof(float));
850 }
851 
852 // gets the innovations of the synthetic sideslip measurements
853 void Ekf::get_beta_innov(float *beta_innov)
854 {
855  memcpy(beta_innov, &_beta_innov, sizeof(float));
856 }
857 
858 // gets the innovations of the heading measurement
859 void Ekf::get_heading_innov(float *heading_innov)
860 {
861  memcpy(heading_innov, &_heading_innov, sizeof(float));
862 }
863 
864 // gets the innovation variances of velocity and position measurements
865 // 0-2 vel, 3-5 pos
866 void Ekf::get_vel_pos_innov_var(float vel_pos_innov_var[6])
867 {
868  memcpy(vel_pos_innov_var, _vel_pos_innov_var, sizeof(float) * 6);
869 }
870 
871 // gets the innovation variances of the earth magnetic field measurements
872 void Ekf::get_mag_innov_var(float mag_innov_var[3])
873 {
874  memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3);
875 }
876 
877 // gets the innovation variance of the airspeed measurement
878 void Ekf::get_airspeed_innov_var(float *airspeed_innov_var)
879 {
880  memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float));
881 }
882 
883 // gets the innovation variance of the synthetic sideslip measurement
884 void Ekf::get_beta_innov_var(float *beta_innov_var)
885 {
886  memcpy(beta_innov_var, &_beta_innov_var, sizeof(float));
887 }
888 
889 // gets the innovation variance of the heading measurement
890 void Ekf::get_heading_innov_var(float *heading_innov_var)
891 {
892  memcpy(heading_innov_var, &_heading_innov_var, sizeof(float));
893 }
894 
895 // get GPS check status
896 void Ekf::get_gps_check_status(uint16_t *val)
897 {
899 }
900 
901 // get the state vector at the delayed time horizon
903 {
904  for (int i = 0; i < 4; i++) {
905  state[i] = _state.quat_nominal(i);
906  }
907 
908  for (int i = 0; i < 3; i++) {
909  state[i + 4] = _state.vel(i);
910  }
911 
912  for (int i = 0; i < 3; i++) {
913  state[i + 7] = _state.pos(i);
914  }
915 
916  for (int i = 0; i < 3; i++) {
917  state[i + 10] = _state.gyro_bias(i);
918  }
919 
920  for (int i = 0; i < 3; i++) {
921  state[i + 13] = _state.accel_bias(i);
922  }
923 
924  for (int i = 0; i < 3; i++) {
925  state[i + 16] = _state.mag_I(i);
926  }
927 
928  for (int i = 0; i < 3; i++) {
929  state[i + 19] = _state.mag_B(i);
930  }
931 
932  for (int i = 0; i < 2; i++) {
933  state[i + 22] = _state.wind_vel(i);
934  }
935 }
936 
937 // get the accelerometer bias
938 void Ekf::get_accel_bias(float bias[3])
939 {
940  float temp[3];
941  temp[0] = _state.accel_bias(0) / _dt_ekf_avg;
942  temp[1] = _state.accel_bias(1) / _dt_ekf_avg;
943  temp[2] = _state.accel_bias(2) / _dt_ekf_avg;
944  memcpy(bias, temp, 3 * sizeof(float));
945 }
946 
947 // get the gyroscope bias in rad/s
948 void Ekf::get_gyro_bias(float bias[3])
949 {
950  float temp[3];
951  temp[0] = _state.gyro_bias(0) / _dt_ekf_avg;
952  temp[1] = _state.gyro_bias(1) / _dt_ekf_avg;
953  temp[2] = _state.gyro_bias(2) / _dt_ekf_avg;
954  memcpy(bias, temp, 3 * sizeof(float));
955 }
956 
957 // get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
958 // return true if the origin is valid
959 bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
960 {
961  memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
962  memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
963  memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
965 }
966 
967 // return an array containing the output predictor angular, velocity and position tracking
968 // error magnitudes (rad), (m/s), (m)
969 void Ekf::get_output_tracking_error(float error[3])
970 {
971  memcpy(error, _output_tracking_error, 3 * sizeof(float));
972 }
973 
974 /*
975 Returns following IMU vibration metrics in the following array locations
976 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
977 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
978 2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
979 */
980 void Ekf::get_imu_vibe_metrics(float vibe[3])
981 {
982  memcpy(vibe, _vibe_metrics, 3 * sizeof(float));
983 }
984 
985 /*
986  First argument returns GPS drift metrics in the following array locations
987  0 : Horizontal position drift rate (m/s)
988  1 : Vertical position drift rate (m/s)
989  2 : Filtered horizontal velocity (m/s)
990  Second argument returns true when IMU movement is blocking the drift calculation
991  Function returns true if the metrics have been updated and not returned previously by this function
992 */
993 bool Ekf::get_gps_drift_metrics(float drift[3], bool *blocked)
994 {
995  memcpy(drift, _gps_drift_metrics, 3 * sizeof(float));
996  *blocked = !_vehicle_at_rest;
997  if (_gps_drift_updated) {
998  _gps_drift_updated = false;
999  return true;
1000  }
1001  return false;
1002 }
1003 
1004 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
1005 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
1006 {
1007  // report absolute accuracy taking into account the uncertainty in location of the origin
1008  // If not aiding, return 0 for horizontal position estimate as no estimate is available
1009  // TODO - allow for baro drift in vertical position error
1010  float hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
1011 
1012  // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
1013  // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
1014  // and using state variances for accuracy reporting is overly optimistic in these situations
1016  hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
1017  }
1018 
1019  *ekf_eph = hpos_err;
1020  *ekf_epv = sqrtf(P[9][9] + sq(_gps_origin_epv));
1021 }
1022 
1023 // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
1024 void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
1025 {
1026  // TODO - allow for baro drift in vertical position error
1027  float hpos_err = sqrtf(P[7][7] + P[8][8]);
1028 
1029  // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
1030  // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
1031  // and using state variances for accuracy reporting is overly optimistic in these situations
1033  hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
1034  }
1035 
1036  *ekf_eph = hpos_err;
1037  *ekf_epv = sqrtf(P[9][9]);
1038 }
1039 
1040 // get the 1-sigma horizontal and vertical velocity uncertainty
1041 void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
1042 {
1043  float hvel_err = sqrtf(P[4][4] + P[5][5]);
1044 
1045  // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
1046  // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
1047  // and using state variances for accuracy reporting is overly optimistic in these situations
1048  if (_is_dead_reckoning) {
1049  float vel_err_conservative = 0.0f;
1050 
1052  float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
1053  vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
1054  }
1055 
1057  vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
1058  }
1059 
1061  // What is the right thing to do here
1062 // vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
1063  }
1064 
1065  hvel_err = math::max(hvel_err, vel_err_conservative);
1066  }
1067 
1068  *ekf_evh = hvel_err;
1069  *ekf_evv = sqrtf(P[6][6]);
1070 }
1071 
1072 /*
1073 Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
1074 vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
1075 vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
1076 hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
1077 hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
1078 */
1079 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max)
1080 {
1081  // Calculate range finder limits
1082  float rangefinder_hagl_min = _rng_valid_min_val;
1083  // Allow use of 75% of rangefinder maximum range to allow for angular motion
1084  float rangefinder_hagl_max = 0.75f * _rng_valid_max_val;
1085 
1086  // Calculate optical flow limits
1087  // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
1088  float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f);
1089  float flow_hagl_min = _flow_min_distance;
1090  float flow_hagl_max = _flow_max_distance;
1091 
1092  // TODO : calculate visual odometry limits
1093 
1094  bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid;
1095 
1097 
1098  // Do not require limiting by default
1099  *vxy_max = NAN;
1100  *vz_max = NAN;
1101  *hagl_min = NAN;
1102  *hagl_max = NAN;
1103 
1104  // Keep within range sensor limit when using rangefinder as primary height source
1105  if (relying_on_rangefinder) {
1106  *vxy_max = NAN;
1107  *vz_max = NAN;
1108  *hagl_min = rangefinder_hagl_min;
1109  *hagl_max = rangefinder_hagl_max;
1110  }
1111 
1112  // Keep within flow AND range sensor limits when exclusively using optical flow
1113  if (relying_on_optical_flow) {
1114  *vxy_max = flow_vxy_max;
1115  *vz_max = NAN;
1116  *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
1117  *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
1118  }
1119 
1120 }
1121 
1123 {
1124  if (_imu_sample_delayed.time_us - _last_imu_bias_cov_reset_us < (uint64_t)10e6) {
1125  return false;
1126 
1127  }
1128 
1129  // Zero the delta angle and delta velocity bias states
1130  _state.gyro_bias.zero();
1131  _state.accel_bias.zero();
1132 
1133  // Zero the corresponding covariances
1134  zeroCols(P, 10, 15);
1135  zeroRows(P, 10, 15);
1136 
1137  // Set the corresponding variances to the values use for initial alignment
1138  float dt = FILTER_UPDATE_PERIOD_S;
1139  P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
1140  P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
1142 
1143  // Set previous frame values
1144  _prev_dvel_bias_var(0) = P[13][13];
1145  _prev_dvel_bias_var(1) = P[14][14];
1146  _prev_dvel_bias_var(2) = P[15][15];
1147 
1148  return true;
1149 
1150 }
1151 
1152 // get EKF innovation consistency check status information comprising of:
1153 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
1154 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
1155 // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
1156 // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
1157 void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta)
1158 {
1159  // return the integer bitmask containing the consistency check pass/fail status
1160  *status = _innov_check_fail_status.value;
1161  // return the largest magnetometer innovation test ratio
1163  // return the largest NED velocity innovation test ratio
1165  // return the largest NE position innovation test ratio
1166  *pos = sqrtf(math::max(_vel_pos_test_ratio[3], _vel_pos_test_ratio[4]));
1167  // return the vertical position innovation test ratio
1168  *hgt = sqrtf(_vel_pos_test_ratio[5]);
1169  // return the airspeed fusion innovation test ratio
1170  *tas = sqrtf(_tas_test_ratio);
1171  // return the terrain height innovation test ratio
1172  *hagl = sqrtf(_terr_test_ratio);
1173  // return the synthetic sideslip innovation test ratio
1174  *beta = sqrtf(_beta_test_ratio);
1175 }
1176 
1177 // return a bitmask integer that describes which state estimates are valid
1179 {
1180  ekf_solution_status soln_status;
1181 
1187  soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
1188  soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
1189  soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
1190  soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
1191  soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
1192  bool gps_vel_innov_bad = (_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f);
1193  bool gps_pos_innov_bad = (_vel_pos_test_ratio[3] > 1.0f) || (_vel_pos_test_ratio[4] > 1.0f);
1194  bool mag_innov_good = (_mag_test_ratio[0] < 1.0f) && (_mag_test_ratio[1] < 1.0f) && (_mag_test_ratio[2] < 1.0f) && (_yaw_test_ratio < 1.0f);
1195  soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
1197  *status = soln_status.value;
1198 }
1199 
1200 // fuse measurement
1201 void Ekf::fuse(float *K, float innovation)
1202 {
1203  for (unsigned i = 0; i < 4; i++) {
1204  _state.quat_nominal(i) = _state.quat_nominal(i) - K[i] * innovation;
1205  }
1206 
1207  _state.quat_nominal.normalize();
1208 
1209  for (unsigned i = 0; i < 3; i++) {
1210  _state.vel(i) = _state.vel(i) - K[i + 4] * innovation;
1211  }
1212 
1213  for (unsigned i = 0; i < 3; i++) {
1214  _state.pos(i) = _state.pos(i) - K[i + 7] * innovation;
1215  }
1216 
1217  for (unsigned i = 0; i < 3; i++) {
1218  _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 10] * innovation;
1219  }
1220 
1221  for (unsigned i = 0; i < 3; i++) {
1222  _state.accel_bias(i) = _state.accel_bias(i) - K[i + 13] * innovation;
1223  }
1224 
1225  for (unsigned i = 0; i < 3; i++) {
1226  _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation;
1227  }
1228 
1229  for (unsigned i = 0; i < 3; i++) {
1230  _state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation;
1231  }
1232 
1233  for (unsigned i = 0; i < 2; i++) {
1234  _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation;
1235  }
1236 }
1237 
1238 // zero specified range of rows in the state covariance matrix
1239 void Ekf::zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
1240 {
1241  uint8_t row;
1242 
1243  for (row = first; row <= last; row++) {
1244  memset(&cov_mat[row][0], 0, sizeof(cov_mat[0][0]) * 24);
1245  }
1246 }
1247 
1248 // zero specified range of columns in the state covariance matrix
1249 void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
1250 {
1251  uint8_t row;
1252 
1253  for (row = 0; row <= 23; row++) {
1254  memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first));
1255  }
1256 }
1257 
1258 void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
1259 {
1260  // save diagonal elements
1261  uint8_t row;
1262  float variances[_k_num_states];
1263 
1264  for (row = first; row <= last; row++) {
1265  variances[row] = cov_mat[row][row];
1266  }
1267 
1268  // zero rows and columns
1269  zeroRows(cov_mat, first, last);
1270  zeroCols(cov_mat, first, last);
1271 
1272  // restore diagonals
1273  for (row = first; row <= last; row++) {
1274  cov_mat[row][row] = variances[row];
1275  }
1276 }
1277 
1279 {
1280  // save 4x4 elements
1281  uint32_t row;
1282  uint32_t col;
1283  float variances[4][4];
1284  for (row = 0; row < 4; row++) {
1285  for (col = 0; col < 4; col++) {
1286  variances[row][col] = P[row][col];
1287  }
1288  }
1289 
1290  // zero rows and columns
1291  zeroRows(P, 0, 3);
1292  zeroCols(P, 0, 3);
1293 
1294  // restore 4x4 elements
1295  for (row = 0; row < 4; row++) {
1296  for (col = 0; col < 4; col++) {
1297  P[row][col] = variances[row][col];
1298  }
1299  }
1300 }
1301 
1302 void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)
1303 {
1304  // zero rows and columns
1305  zeroRows(cov_mat, first, last);
1306  zeroCols(cov_mat, first, last);
1307 
1308  // set diagonals
1309  uint8_t row;
1310 
1311  for (row = first; row <= last; row++) {
1312  cov_mat[row][row] = variance;
1313  }
1314 
1315 }
1316 
1318 {
1319  // return true if the origin is set we are not doing unconstrained free inertial navigation
1320  // and have not started using synthetic position observations to constrain drift
1322 }
1323 
1324 // return true if we are totally reliant on inertial dead-reckoning for position
1326 {
1333 
1334  _is_wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
1335  _is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
1336 
1337  // record the time we start inertial dead reckoning
1338  if (!_is_dead_reckoning) {
1340  }
1341 
1342  // report if we have been deadreckoning for too long
1344 }
1345 
1346 // perform a vector cross product
1348 {
1349  Vector3f vecOut;
1350  vecOut(0) = vecIn1(1) * vecIn2(2) - vecIn1(2) * vecIn2(1);
1351  vecOut(1) = vecIn1(2) * vecIn2(0) - vecIn1(0) * vecIn2(2);
1352  vecOut(2) = vecIn1(0) * vecIn2(1) - vecIn1(1) * vecIn2(0);
1353  return vecOut;
1354 }
1355 
1356 // calculate the inverse rotation matrix from a quaternion rotation
1357 // this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
1359 {
1360  float q00 = quat(0) * quat(0);
1361  float q11 = quat(1) * quat(1);
1362  float q22 = quat(2) * quat(2);
1363  float q33 = quat(3) * quat(3);
1364  float q01 = quat(0) * quat(1);
1365  float q02 = quat(0) * quat(2);
1366  float q03 = quat(0) * quat(3);
1367  float q12 = quat(1) * quat(2);
1368  float q13 = quat(1) * quat(3);
1369  float q23 = quat(2) * quat(3);
1370 
1371  Matrix3f dcm;
1372  dcm(0, 0) = q00 + q11 - q22 - q33;
1373  dcm(1, 1) = q00 - q11 + q22 - q33;
1374  dcm(2, 2) = q00 - q11 - q22 + q33;
1375  dcm(1, 0) = 2.0f * (q12 - q03);
1376  dcm(2, 0) = 2.0f * (q13 + q02);
1377  dcm(0, 1) = 2.0f * (q12 + q03);
1378  dcm(2, 1) = 2.0f * (q23 - q01);
1379  dcm(0, 2) = 2.0f * (q13 - q02);
1380  dcm(1, 2) = 2.0f * (q23 + q01);
1381 
1382  return dcm;
1383 }
1384 
1385 // calculate the variances for the rotation vector equivalent
1387 {
1388  Vector3f rot_var_vec;
1389  float q0, q1, q2, q3;
1390 
1391  if (_state.quat_nominal(0) >= 0.0f) {
1392  q0 = _state.quat_nominal(0);
1393  q1 = _state.quat_nominal(1);
1394  q2 = _state.quat_nominal(2);
1395  q3 = _state.quat_nominal(3);
1396 
1397  } else {
1398  q0 = -_state.quat_nominal(0);
1399  q1 = -_state.quat_nominal(1);
1400  q2 = -_state.quat_nominal(2);
1401  q3 = -_state.quat_nominal(3);
1402  }
1403  float t2 = q0*q0;
1404  float t3 = acosf(q0);
1405  float t4 = -t2+1.0f;
1406  float t5 = t2-1.0f;
1407  if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
1408  float t6 = 1.0f/t5;
1409  float t7 = q1*t6*2.0f;
1410  float t8 = 1.0f/powf(t4,1.5f);
1411  float t9 = q0*q1*t3*t8*2.0f;
1412  float t10 = t7+t9;
1413  float t11 = 1.0f/sqrtf(t4);
1414  float t12 = q2*t6*2.0f;
1415  float t13 = q0*q2*t3*t8*2.0f;
1416  float t14 = t12+t13;
1417  float t15 = q3*t6*2.0f;
1418  float t16 = q0*q3*t3*t8*2.0f;
1419  float t17 = t15+t16;
1420  rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
1421  rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
1422  rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
1423  } else {
1424  rot_var_vec(0) = 4.0f * P[1][1];
1425  rot_var_vec(1) = 4.0f * P[2][2];
1426  rot_var_vec(2) = 4.0f * P[3][3];
1427  }
1428 
1429  return rot_var_vec;
1430 }
1431 
1432 // initialise the quaternion covariances using rotation vector variances
1434 {
1435  // calculate an equivalent rotation vector from the quaternion
1436  float q0,q1,q2,q3;
1437  if (_state.quat_nominal(0) >= 0.0f) {
1438  q0 = _state.quat_nominal(0);
1439  q1 = _state.quat_nominal(1);
1440  q2 = _state.quat_nominal(2);
1441  q3 = _state.quat_nominal(3);
1442 
1443  } else {
1444  q0 = -_state.quat_nominal(0);
1445  q1 = -_state.quat_nominal(1);
1446  q2 = -_state.quat_nominal(2);
1447  q3 = -_state.quat_nominal(3);
1448  }
1449  float delta = 2.0f*acosf(q0);
1450  float scaler = (delta/sinf(delta*0.5f));
1451  float rotX = scaler*q1;
1452  float rotY = scaler*q2;
1453  float rotZ = scaler*q3;
1454 
1455  // autocode generated using matlab symbolic toolbox
1456  float t2 = rotX*rotX;
1457  float t4 = rotY*rotY;
1458  float t5 = rotZ*rotZ;
1459  float t6 = t2+t4+t5;
1460  if (t6 > 1e-9f) {
1461  float t7 = sqrtf(t6);
1462  float t8 = t7*0.5f;
1463  float t3 = sinf(t8);
1464  float t9 = t3*t3;
1465  float t10 = 1.0f/t6;
1466  float t11 = 1.0f/sqrtf(t6);
1467  float t12 = cosf(t8);
1468  float t13 = 1.0f/powf(t6,1.5f);
1469  float t14 = t3*t11;
1470  float t15 = rotX*rotY*t3*t13;
1471  float t16 = rotX*rotZ*t3*t13;
1472  float t17 = rotY*rotZ*t3*t13;
1473  float t18 = t2*t10*t12*0.5f;
1474  float t27 = t2*t3*t13;
1475  float t19 = t14+t18-t27;
1476  float t23 = rotX*rotY*t10*t12*0.5f;
1477  float t28 = t15-t23;
1478  float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
1479  float t25 = rotX*rotZ*t10*t12*0.5f;
1480  float t31 = t16-t25;
1481  float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f;
1482  float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f;
1483  float t24 = t15-t23;
1484  float t26 = t16-t25;
1485  float t29 = t4*t10*t12*0.5f;
1486  float t34 = t3*t4*t13;
1487  float t30 = t14+t29-t34;
1488  float t32 = t5*t10*t12*0.5f;
1489  float t40 = t3*t5*t13;
1490  float t33 = t14+t32-t40;
1491  float t36 = rotY*rotZ*t10*t12*0.5f;
1492  float t39 = t17-t36;
1493  float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
1494  float t37 = t15-t23;
1495  float t38 = t17-t36;
1496  float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25);
1497  float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39;
1498  float t43 = t16-t25;
1499  float t44 = t17-t36;
1500 
1501  // zero all the quaternion covariances
1502  zeroRows(P, 0, 3);
1503  zeroCols(P, 0, 3);
1504 
1505  // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
1506  P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
1507  P[0][1] = t22;
1508  P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;
1509  P[0][3] = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f;
1510  P[1][0] = t22;
1511  P[1][1] = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26);
1512  P[1][2] = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
1513  P[1][3] = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
1514  P[2][0] = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f;
1515  P[2][1] = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30;
1516  P[2][2] = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38);
1517  P[2][3] = t42;
1518  P[3][0] = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f;
1519  P[3][1] = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33;
1520  P[3][2] = t42;
1521  P[3][3] = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44);
1522 
1523  } else {
1524  // the equations are badly conditioned so use a small angle approximation
1525  P[0][0] = 0.0f;
1526  P[0][1] = 0.0f;
1527  P[0][2] = 0.0f;
1528  P[0][3] = 0.0f;
1529  P[1][0] = 0.0f;
1530  P[1][1] = 0.25f * rot_vec_var(0);
1531  P[1][2] = 0.0f;
1532  P[1][3] = 0.0f;
1533  P[2][0] = 0.0f;
1534  P[2][1] = 0.0f;
1535  P[2][2] = 0.25f * rot_vec_var(1);
1536  P[2][3] = 0.0f;
1537  P[3][0] = 0.0f;
1538  P[3][1] = 0.0f;
1539  P[3][2] = 0.0f;
1540  P[3][3] = 0.25f * rot_vec_var(2);
1541 
1542  }
1543 }
1544 
1546 {
1548 
1549  _control_status.flags.gps_hgt = false;
1550  _control_status.flags.rng_hgt = false;
1551  _control_status.flags.ev_hgt = false;
1552 }
1553 
1555 {
1556  _control_status.flags.rng_hgt = true;
1557 
1558  _control_status.flags.baro_hgt = false;
1559  _control_status.flags.gps_hgt = false;
1560  _control_status.flags.ev_hgt = false;
1561 }
1562 
1564 {
1565  _control_status.flags.gps_hgt = true;
1566 
1567  _control_status.flags.baro_hgt = false;
1568  _control_status.flags.rng_hgt = false;
1569  _control_status.flags.ev_hgt = false;
1570 }
1571 
1573 {
1574  _control_status.flags.ev_hgt = true;
1575 
1576  _control_status.flags.baro_hgt = false;
1577  _control_status.flags.gps_hgt = false;
1578  _control_status.flags.rng_hgt = false;
1579 }
1580 
1582 {
1583  stopMag3DFusion();
1584  stopMagHdgFusion();
1585  clearMagCov();
1586 }
1587 
1589 {
1590  // save covariance data for re-use if currently doing 3-axis fusion
1592  saveMagCovData();
1593  _control_status.flags.mag_3D = false;
1594  }
1595 }
1596 
1598 {
1599  _control_status.flags.mag_hdg = false;
1600 }
1601 
1603 {
1604  stopMag3DFusion();
1605  _control_status.flags.mag_hdg = true;
1606 }
1607 
1609 {
1610  if (!_control_status.flags.mag_3D) {
1611  stopMagHdgFusion();
1612  zeroMagCov();
1613  loadMagCovData();
1614  _control_status.flags.mag_3D = true;
1615  }
1616 }
1617 
1618 // update the estimated misalignment between the EV navigation frame and the EKF navigation frame
1619 // and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
1621 {
1622  // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
1623  Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
1624  q_error.normalize();
1625 
1626  // convert to a delta angle and apply a spike and low pass filter
1627  Vector3f rot_vec = q_error.to_axis_angle();
1628 
1629  float rot_vec_norm = rot_vec.norm();
1630 
1631  if (rot_vec_norm > 1e-6f) {
1632 
1633  // apply an input limiter to protect from spikes
1634  Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
1635  float input_delta_len = _input_delta_vec.norm();
1636 
1637  if (input_delta_len > 0.1f) {
1638  rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
1639  }
1640 
1641  // Apply a first order IIR low pass filter
1642  const float omega_lpf_us = 0.2e-6f; // cutoff frequency in rad/uSec
1643  float alpha = math::constrain(omega_lpf_us * (float)(_time_last_imu - _ev_rot_last_time_us), 0.0f, 1.0f);
1645  _ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
1646 
1647  }
1648 
1649  // convert filtered vector to a quaternion and then to a rotation matrix
1650  q_error.from_axis_angle(_ev_rot_vec_filt);
1651  _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
1652 
1653 }
1654 
1655 // reset the estimated misalignment between the EV navigation frame and the EKF navigation frame
1656 // and update the rotation matrix which rotates EV measurements into the EKF's navigation frame
1658 {
1659  // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon.
1660  Quatf q_error = _state.quat_nominal * _ev_sample_delayed.quat.inversed();
1661  q_error.normalize();
1662 
1663  // convert to a delta angle and reset
1664  Vector3f rot_vec = q_error.to_axis_angle();
1665 
1666  float rot_vec_norm = rot_vec.norm();
1667 
1668  if (rot_vec_norm > 1e-9f) {
1669  _ev_rot_vec_filt = rot_vec;
1670 
1671  } else {
1672  _ev_rot_vec_filt.zero();
1673  }
1674 
1675  // reset the rotation matrix
1676  _ev_rot_mat = Dcmf(q_error); // rotation from EV reference to EKF reference
1677 }
1678 
1679 // return the quaternions for the rotation from External Vision system reference frame to the EKF reference frame
1681 {
1682  Quatf quat_ev2ekf;
1683  quat_ev2ekf.from_axis_angle(_ev_rot_vec_filt);
1684 
1685  for (unsigned i = 0; i < 4; i++) {
1686  quat[i] = quat_ev2ekf(i);
1687  }
1688 }
1689 
1690 // Increase the yaw error variance of the quaternions
1691 // Argument is additional yaw variance in rad**2
1692 void Ekf::increaseQuatYawErrVariance(float yaw_variance)
1693 {
1694  // See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file
1695  // The auto-code was cleaned up and had terms multiplied by zero removed to give the following:
1696 
1697  // Intermediate variables
1698  float SG[3];
1702 
1703  float SQ[4];
1704  SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
1705  SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
1706  SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
1707  SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
1708 
1709  // Limit yaw variance increase to prevent a badly conditioned covariance matrix
1710  yaw_variance = fminf(yaw_variance, 1.0e-2f);
1711 
1712  // Add covariances for additonal yaw uncertainty to existing covariances.
1713  // This assumes that the additional yaw error is uncorrrelated to existing errors
1714  P[0][0] += yaw_variance*sq(SQ[2]);
1715  P[0][1] += yaw_variance*SQ[1]*SQ[2];
1716  P[1][1] += yaw_variance*sq(SQ[1]);
1717  P[0][2] += yaw_variance*SQ[0]*SQ[2];
1718  P[1][2] += yaw_variance*SQ[0]*SQ[1];
1719  P[2][2] += yaw_variance*sq(SQ[0]);
1720  P[0][3] -= yaw_variance*SQ[2]*SQ[3];
1721  P[1][3] -= yaw_variance*SQ[1]*SQ[3];
1722  P[2][3] -= yaw_variance*SQ[0]*SQ[3];
1723  P[3][3] += yaw_variance*sq(SQ[3]);
1724  P[1][0] += yaw_variance*SQ[1]*SQ[2];
1725  P[2][0] += yaw_variance*SQ[0]*SQ[2];
1726  P[2][1] += yaw_variance*SQ[0]*SQ[1];
1727  P[3][0] -= yaw_variance*SQ[2]*SQ[3];
1728  P[3][1] -= yaw_variance*SQ[1]*SQ[3];
1729  P[3][2] -= yaw_variance*SQ[0]*SQ[3];
1730 }
1731 
1732 // save covariance data for re-use when auto-switching between heading and 3-axis fusion
1734 {
1735  // save variances for the D earth axis and XYZ body axis field
1736  for (uint8_t index = 0; index <= 3; index ++) {
1737  _saved_mag_bf_variance[index] = P[index + 18][index + 18];
1738  }
1739 
1740  // save the NE axis covariance sub-matrix
1741  for (uint8_t row = 0; row <= 1; row ++) {
1742  for (uint8_t col = 0; col <= 1; col ++) {
1743  _saved_mag_ef_covmat[row][col] = P[row + 16][col + 16];
1744  }
1745  }
1746 }
1747 
1749 {
1750  // re-instate variances for the D earth axis and XYZ body axis field
1751  for (uint8_t index = 0; index <= 3; index ++) {
1752  P[index + 18][index + 18] = _saved_mag_bf_variance[index];
1753  }
1754  // re-instate the NE axis covariance sub-matrix
1755  for (uint8_t row = 0; row <= 1; row ++) {
1756  for (uint8_t col = 0; col <= 1; col ++) {
1757  P[row + 16][col + 16] = _saved_mag_ef_covmat[row][col];
1758  }
1759  }
1760 }
1761 
1762 float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const
1763 {
1764  float y = input - accumulator;
1765  float t = sum_previous + y;
1766  accumulator = (t - sum_previous) - y;
1767  return t;
1768 }
Matrix3f quat_to_invrotmat(const Quatf &quat)
t38
Definition: K_LOSX.c:39
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
void loadMagCovData()
struct estimator::filter_control_status_u::@60 flags
void setControlBaroHeight()
uint16_t velocity_horiz
1 - True if the horizontal velocity estimate is good
Definition: common.h:474
float _beta_innov
synthetic sideslip measurement innovation (rad)
Definition: ekf.h:382
t34
Definition: calcH_YAWGPS.c:33
float switch_on_gyro_bias
1-sigma gyro bias uncertainty at switch on (rad/sec)
Definition: common.h:252
void clearMagCov()
Definition: covariance.cpp:917
float hgt
gps height measurement (m)
Definition: common.h:116
t8
Definition: calcH_YAW312.c:9
Adapter / shim layer for system calls needed by ECL.
stateSample _state
state struct of the ekf running at the delayed time horizon
Definition: ekf.h:288
t12
Definition: calcH_YAW312.c:11
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
t24
Definition: calcH_YAWGPS.c:23
float _flow_innov[2]
flow measurement innovation (rad/sec)
Definition: ekf.h:392
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
Definition: common.h:442
float mag_heading_noise
measurement noise used for simple heading fusion (rad)
Definition: common.h:269
RingBuffer< outputSample > _output_buffer
t9
Definition: calcH_YAW312.c:1
void initialiseQuatCovariances(Vector3f &rot_vec_var)
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
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)
Definition: ekf_helper.cpp:554
#define MAG_FUSE_TYPE_3D
Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised...
Definition: common.h:202
t43
Definition: K_LOSX.c:46
RingBuffer< extVisionSample > _ext_vision_buffer
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
t4
Definition: calcH_YAW312.c:5
void get_state_delayed(float *state) override
Definition: ekf_helper.cpp:902
uint16_t pos_horiz_abs
4 - True if the horizontal position (absolute) estimate is good
Definition: common.h:477
void get_heading_innov(float *heading_innov) override
Definition: ekf_helper.cpp:859
Dcm< float > Dcmf
Definition: Dcm.hpp:185
const data_type & get_newest()
Definition: RingBuffer.h:121
Vector3f accel_bias
delta velocity bias estimate in m/s
Definition: common.h:372
t28
Definition: calcH_YAWGPS.c:27
float _gps_alt_ref
WGS-84 height (m)
Definition: ekf.h:425
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
uint32_t fuse_aspd
19 - true when airspeed measurements are being fused
Definition: common.h:460
Vector2f pos
NE earth frame gps horizontal position measurement (m)
Definition: common.h:115
void get_gyro_bias(float bias[3]) override
Definition: ekf_helper.cpp:948
bool resetVelocity()
Definition: ekf_helper.cpp:50
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:158
float _heading_innov_var
heading measurement innovation variance (rad**2)
Definition: ekf.h:389
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
Definition: common.h:445
SquareMatrix< float, 3 > Matrix3f
uint16_t pred_pos_horiz_rel
8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estim...
Definition: common.h:481
t36
Definition: K_LOSX.c:37
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
Definition: common.h:459
void get_output_tracking_error(float error[3]) override
Definition: ekf_helper.cpp:969
float SQ[4]
Definition: YawCovariance.c:16
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
Definition: ekf.h:435
uint64_t _last_gps_origin_time_us
true when all active GPS checks have passed
Definition: ekf.h:424
t6
Definition: calcH_YAW312.c:7
void get_mag_innov_var(float mag_innov_var[3]) override
Definition: ekf_helper.cpp:872
t5
Definition: calcH_YAW312.c:6
t14
Definition: calcH_YAW312.c:13
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
Definition: common.h:451
float SG[3]
Definition: YawCovariance.c:11
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:122
uint16_t pos_vert_abs
5 - True if the vertical position (absolute) estimate is good
Definition: common.h:478
Vector3f mag_I
NED earth magnetic field in gauss.
Definition: common.h:373
float _rng_valid_min_val
minimum distance that the rangefinder can measure (m)
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
Definition: common.h:450
RingBuffer< outputVert > _output_vert_buffer
float kahanSummation(float sum_previous, float input, float &accumulator) const
void zeroMagCov()
Definition: covariance.cpp:923
t44
Definition: K_LOSX.c:47
t19
Definition: calcH_YAWGPS.c:17
void get_imu_vibe_metrics(float vibe[3]) override
Definition: ekf_helper.cpp:980
rangeSample _range_sample_delayed
#define MAG_FUSE_TYPE_INDOOR
The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aid...
Definition: common.h:204
float switch_on_accel_bias
1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
Definition: common.h:253
uint16_t velocity_vert
2 - True if the vertical velocity estimate is good
Definition: common.h:475
float _mag_innov[3]
earth magnetic field innovations (Gauss)
Definition: ekf.h:376
t29
Definition: calcH_YAWGPS.c:28
float _flow_max_rate
maximum angular flow rate that the optical flow sensor can measure (rad/s)
t21
Definition: calcH_YAWGPS.c:20
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
Definition: common.h:222
float dt
amount of integration time (sec)
Definition: common.h:151
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
void zeroCols(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:156
float _flow_min_distance
minimum distance that the optical flow sensor can operate at (m)
t27
Definition: calcH_YAWGPS.c:26
float vel_d
D velocity calculated using alternative algorithm (m/sec)
Definition: common.h:100
const T & getState() const
Definition: AlphaFilter.hpp:67
void calcExtVisRotMat()
float rng
range (distance to ground) measurement (m)
Definition: common.h:136
#define GPS_MAX_INTERVAL
Maximum allowable time interval between GPS measurements (uSec)
Definition: common.h:208
float _terrain_vpos
estimated vertical position of the terrain underneath the vehicle in local NED frame (m) ...
Definition: ekf.h:457
t42
Definition: K_LOSX.c:45
t39
Definition: K_LOSX.c:40
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
t33
Definition: calcH_YAWGPS.c:32
#define MASK_USE_GEO_DECL
set to true to use the declination from the geo library when the GPS position becomes available...
Definition: common.h:184
float sacc
1-std speed error (m/sec)
Definition: common.h:121
#define BARO_MAX_INTERVAL
Maximum allowable time interval between pressure altitude measurements (uSec)
Definition: common.h:209
void saveMagCovData()
RingBuffer< gpsSample > _gps_buffer
float getMagDeclination()
Definition: ekf_helper.cpp:750
t22
Definition: calcH_YAWGPS.c:21
void resetExtVisRotMat()
gps_check_fail_status_u _gps_check_fail_status
Definition: ekf.h:447
t31
Definition: calcH_YAWGPS.c:30
void fuse(float *K, float innovation)
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
t10
Definition: calcH_YAW312.c:2
void get_airspeed_innov_var(float *airspeed_innov_var) override
Definition: ekf_helper.cpp:878
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
Definition: ekf.h:348
void calcEarthRateNED(Vector3f &omega, float lat_rad) const
Definition: ekf_helper.cpp:820
t15
Definition: calcH_YAWGPS.c:14
void get_accel_bias(float bias[3]) override
Definition: ekf_helper.cpp:938
t41
Definition: K_LOSX.c:42
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
Definition: common.h:465
float _flow_max_distance
maximum distance that the optical flow sensor can operate at (m)
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:163
void update_deadreckoning_status()
float baro_noise
observation noise for barometric height fusion (m)
Definition: common.h:261
int32_t mag_declination_source
bitmask used to control the handling of declination data
Definition: common.h:274
bool realignYawGPS()
Definition: ekf_helper.cpp:405
void get_heading_innov_var(float *heading_innov_var) override
Definition: ekf_helper.cpp:890
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
Definition: ekf.h:330
float acc_bias_lim
maximum accel bias magnitude (m/sec**2)
Definition: common.h:336
void zeroOffDiag(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
t16
Definition: calcH_YAWGPS.c:15
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:159
float mag_noise
measurement noise used for 3-axis magnetoemeter fusion (Gauss)
Definition: common.h:270
Vector3f calcRotVecVariances()
float _beta_innov_var
synthetic sideslip measurement innovation variance (rad**2)
Definition: ekf.h:383
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
Definition: common.h:117
t20
Definition: calcH_YAWGPS.c:18
void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override
Definition: ekf_helper.cpp:866
int32_t valid_timeout_max
amount of time spent inertial dead reckoning before the estimator reports the state estimates as inva...
Definition: common.h:344
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
t37
Definition: K_LOSX.c:38
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
t26
Definition: calcH_YAWGPS.c:25
uint16_t const_pos_mode
7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or ...
Definition: common.h:480
Vector3f pos
NED position estimate in earth frame (m/sec)
Definition: common.h:95
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
int32_t range_aid
allow switching primary height source to range finder if certain conditions are met ...
Definition: common.h:297
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void stopMag3DFusion()
bool get_gps_drift_metrics(float drift[3], bool *blocked) override
Definition: ekf_helper.cpp:993
uint16_t attitude
0 - True if the attitude estimate is good
Definition: common.h:473
Vector2< float > Vector2f
Definition: Vector2.hpp:69
float rng_gnd_clearance
minimum valid value for range when on ground (m)
Definition: common.h:291
t13
Definition: calcH_YAW312.c:12
filter_control_status_u _control_status
void get_airspeed_innov(float *airspeed_innov) override
Definition: ekf_helper.cpp:847
t35
Definition: calcH_YAWGPS.c:34
Euler< float > Eulerf
Definition: Euler.hpp:156
RingBuffer< baroSample > _baro_buffer
Vector3f vel
NED velocity in earth frame in m/s.
Definition: common.h:369
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)
Definition: ekf.h:374
float range_noise
observation noise for range finder measurements (m)
Definition: common.h:289
uint16_t pos_horiz_rel
3 - True if the horizontal position (relative) estimate is good
Definition: common.h:476
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
Definition: common.h:455
void stopMagHdgFusion()
int32_t mag_fusion_type
integer used to specify the type of magnetometer fusion used
Definition: common.h:275
static constexpr float sq(float var)
Definition: ekf.h:695
float _mag_innov_var[3]
earth magnetic field innovation variance (Gauss**2)
Definition: ekf.h:377
t30
Definition: calcH_YAWGPS.c:29
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
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
Definition: common.h:454
void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) override
void get_ekf_soln_status(uint16_t *status) override
fault_status_u _fault_status
float _rng_valid_max_val
maximum distance that the rangefinder can measure (m)
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
uint16_t pos_vert_agl
6 - True if the vertical position (above ground) estimate is good
Definition: common.h:479
float hgt
pressure altitude above sea level (m)
Definition: common.h:131
float _airspeed_innov_var
airspeed measurement innovation variance ((m/sec)**2)
Definition: ekf.h:380
void increaseQuatYawErrVariance(float yaw_variance)
bool _hpos_prev_available
true when previous values of the estimate and measurement are available for use
Definition: ekf.h:310
static constexpr float CONSTANTS_EARTH_SPIN_RATE
Definition: geo.h:64
t32
Definition: calcH_YAWGPS.c:31
Dcmf _ev_rot_mat
transformation matrix that rotates observations from the EV to the EKF navigation frame...
Definition: ekf.h:312
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
Definition: common.h:464
#define ECL_WARN_TIMESTAMPED
Definition: ecl.h:94
uint64_t _time_ins_deadreckon_start
amount of time we have been doing inertial only deadreckoning (uSec)
Definition: ekf.h:327
void setControlRangeHeight()
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
void setControlEVHeight()
void resetHeight()
Definition: ekf_helper.cpp:214
t11
Definition: calcH_YAW312.c:10
void alignOutputFilter()
Definition: ekf_helper.cpp:378
void get_mag_innov(float mag_innov[3]) override
Definition: ekf_helper.cpp:841
t18
Definition: calcH_YAWGPS.c:19
void get_aux_vel_innov(float aux_vel_innov[2]) override
Definition: ekf_helper.cpp:835
Vector3f pos
NED position in earth frame in m.
Definition: common.h:370
bool _mag_use_inhibit
true when magnetometer use is being inhibited
Definition: ekf.h:359
innovation_fault_status_u _innov_check_fail_status
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
unsigned no_aid_timeout_max
maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift befor...
Definition: common.h:342
extVisionSample _ev_sample_delayed
uint32_t opt_flow
3 - true if optical flow measurements are being fused
Definition: common.h:444
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
uint64_t _time_last_arsp_fuse
time the last fusion of airspeed measurements were performed (uSec)
Definition: ekf.h:335
Vector3f mag_B
magnetometer bias estimate in body frame in gauss
Definition: common.h:374
#define MAG_FUSE_TYPE_NONE
Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the ...
Definition: common.h:205
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
uint64_t _flt_mag_align_start_time
time that inflight magnetic field alignment started (uSec)
Definition: ekf.h:441
#define MASK_ROTATE_EV
set to true to if the EV observations are in a non NED reference frame and need to be rotated before ...
Definition: common.h:195
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:132
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Definition: common.h:446
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Vector3f vel
NED velocity estimate in earth frame (m/sec)
Definition: common.h:94
float dt
Definition: px4io.c:73
uint32_t in_air
7 - true when the vehicle is airborne
Definition: common.h:448
float mag_declination_deg
magnetic declination (degrees)
Definition: common.h:271
struct Ekf::@62 _state_reset_status
reset event monitoring structure containing velocity, position, height and yaw reset information ...
void get_vel_pos_innov(float vel_pos_innov[6]) override
Definition: ekf_helper.cpp:829
t2
Definition: calcH_YAW312.c:3
float vel_d_integ
Integral of vel_d (m)
Definition: common.h:101
Vector2f _last_known_posNE
last known local NE position vector (m)
Definition: ekf.h:338
uint16_t pred_pos_horiz_abs
9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estim...
Definition: common.h:482
uint32_t wind
8 - true when wind velocity is being estimated
Definition: common.h:449
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
Definition: ekf.h:331
void startMag3DFusion()
void get_gps_check_status(uint16_t *val) override
Definition: ekf_helper.cpp:896
struct estimator::ekf_solution_status::@61 flags
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Definition: common.h:453
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
Definition: common.h:456
float _airspeed_innov
airspeed measurement innovation (m/sec)
Definition: ekf.h:379
float hacc
1-std horizontal position error (m)
Definition: common.h:119
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:157
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
Definition: common.h:452
float angErr
1-Sigma angular error (rad)
Definition: common.h:162
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
uint64_t _time_last_vel_fuse
time the last fusion of velocity measurements was performed (uSec)
Definition: ekf.h:332
float _baro_hgt_offset
baro height reading at the local NED origin (m)
Definition: ekf.h:436
uint32_t gps
2 - true if GPS measurements are being fused
Definition: common.h:443
float pos_noaid_noise
observation noise for non-aiding position fusion (m)
Definition: common.h:260
t17
Definition: calcH_YAWGPS.c:16
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Definition: common.h:441
static constexpr float FILTER_UPDATE_PERIOD_S
bool _bad_vert_accel_detected
true when bad vertical accelerometer data has been detected
Definition: ekf.h:479
void uncorrelateQuatStates()
void stopMagFusion()
Vector2f wind_vel
wind velocity in m/s
Definition: common.h:375
Vector3f _ev_rot_vec_filt
filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (ra...
Definition: ekf.h:311
t23
Definition: calcH_YAWGPS.c:22
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:161
t7
Definition: calcH_YAW312.c:8
Vector3f mag
NED magnetometer body frame measurements (Gauss)
Definition: common.h:126
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 gyro_bias
delta angle bias estimate in rad
Definition: common.h:371
Quatf quat_nominal
nominal quaternion describing vehicle attitude
Definition: common.h:93
float _dt_ekf_avg
average update rate of the ekf
Definition: ekf.h:285
bool resetPosition()
Definition: ekf_helper.cpp:139
Class for core functions for ekf attitude and position estimator.
bool isTerrainEstimateValid() const override
void startMagHdgFusion()
void get_beta_innov_var(float *beta_innov_var) override
Definition: ekf_helper.cpp:884
uint16_t accel_error
11 - True if the EKF has detected bad accelerometer data
Definition: common.h:484
bool _using_synthetic_position
true if we are using a synthetic position to constrain drift
Definition: ekf.h:328
outputSample _output_sample_delayed
t40
Definition: K_LOSX.c:41
bool global_position_is_valid() override
float calcOptFlowMeasVar()
uint8_t get_length() const
Definition: RingBuffer.h:117
t3
Definition: calcH_YAW312.c:4
uint16_t gps_glitch
10 - True if the EKF has detected a GPS glitch
Definition: common.h:483
t25
Definition: calcH_YAWGPS.c:24