PX4 Firmware
PX4 Autopilot Software http://px4.io
gps_yaw_fusion.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 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 gps_yaw_fusion.cpp
36  * Definition of functions required to use yaw obtained from GPS dual antenna measurements.
37  *
38  * @author Paul Riseborough <p_riseborough@live.com.au>
39  *
40  */
41 
42 #include "ekf.h"
43 
44 #include <ecl.h>
45 #include <mathlib/mathlib.h>
46 #include <cstdlib>
47 
49 {
50  // assign intermediate state variables
51  float q0 = _state.quat_nominal(0);
52  float q1 = _state.quat_nominal(1);
53  float q2 = _state.quat_nominal(2);
54  float q3 = _state.quat_nominal(3);
55 
56  float R_YAW = 1.0f;
57  float predicted_hdg;
58  float H_YAW[4];
59  float measured_hdg;
60 
61  // check if data has been set to NAN indicating no measurement
63  // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
64  measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
65 
66  // define the predicted antenna array vector and rotate into earth frame
67  Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
68  Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
69 
70  // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
71  if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
72  return;
73  }
74 
75  // calculate predicted antenna yaw angle
76  predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
77 
78  // calculate observation jacobian
79  float t2 = sinf(_gps_yaw_offset);
80  float t3 = cosf(_gps_yaw_offset);
81  float t4 = q0*q3*2.0f;
82  float t5 = q0*q0;
83  float t6 = q1*q1;
84  float t7 = q2*q2;
85  float t8 = q3*q3;
86  float t9 = q1*q2*2.0f;
87  float t10 = t5+t6-t7-t8;
88  float t11 = t3*t10;
89  float t12 = t4+t9;
90  float t13 = t3*t12;
91  float t14 = t5-t6+t7-t8;
92  float t15 = t2*t14;
93  float t16 = t13+t15;
94  float t17 = t4-t9;
95  float t19 = t2*t17;
96  float t20 = t11-t19;
97  float t18 = (t20*t20);
98  if (t18 < 1e-6f) {
99  return;
100  }
101  t18 = 1.0f / t18;
102  float t21 = t16*t16;
103  float t22 = sq(t11-t19);
104  if (t22 < 1e-6f) {
105  return;
106  }
107  t22 = 1.0f/t22;
108  float t23 = q1*t3*2.0f;
109  float t24 = q2*t2*2.0f;
110  float t25 = t23+t24;
111  float t26 = 1.0f/t20;
112  float t27 = q1*t2*2.0f;
113  float t28 = t21*t22;
114  float t29 = t28+1.0f;
115  if (fabsf(t29) < 1e-6f) {
116  return;
117  }
118  float t30 = 1.0f/t29;
119  float t31 = q0*t3*2.0f;
120  float t32 = t31-q3*t2*2.0f;
121  float t33 = q3*t3*2.0f;
122  float t34 = q0*t2*2.0f;
123  float t35 = t33+t34;
124 
125  H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
126  H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
127  H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
128  H_YAW[3] = t30*(t26*t32+t16*t22*t35);
129 
130  // using magnetic heading tuning parameter
131  R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
132 
133  } else {
134  // there is nothing to fuse
135  return;
136  }
137 
138  // wrap the heading to the interval between +-pi
139  measured_hdg = wrap_pi(measured_hdg);
140 
141  // calculate the innovation and define the innovation gate
142  float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
143  _heading_innov = predicted_hdg - measured_hdg;
144 
145  // wrap the innovation to the interval between +-pi
147 
148  // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
149  // calculate the innovation variance
150  float PH[4];
151  _heading_innov_var = R_YAW;
152 
153  for (unsigned row = 0; row <= 3; row++) {
154  PH[row] = 0.0f;
155 
156  for (uint8_t col = 0; col <= 3; col++) {
157  PH[row] += P[row][col] * H_YAW[col];
158  }
159 
160  _heading_innov_var += H_YAW[row] * PH[row];
161  }
162 
163  float heading_innov_var_inv;
164 
165  // check if the innovation variance calculation is badly conditioned
166  if (_heading_innov_var >= R_YAW) {
167  // the innovation variance contribution from the state covariances is not negative, no fault
168  _fault_status.flags.bad_hdg = false;
169  heading_innov_var_inv = 1.0f / _heading_innov_var;
170 
171  } else {
172  // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
173  _fault_status.flags.bad_hdg = true;
174 
175  // we reinitialise the covariance matrix and abort this fusion step
177  ECL_ERR_TIMESTAMPED("EKF GPS yaw fusion numerical error - covariance reset");
178  return;
179  }
180 
181  // calculate the Kalman gains
182  // only calculate gains for states we are using
183  float Kfusion[_k_num_states] = {};
184 
185  for (uint8_t row = 0; row <= 15; row++) {
186  Kfusion[row] = 0.0f;
187 
188  for (uint8_t col = 0; col <= 3; col++) {
189  Kfusion[row] += P[row][col] * H_YAW[col];
190  }
191 
192  Kfusion[row] *= heading_innov_var_inv;
193  }
194 
195  if (_control_status.flags.wind) {
196  for (uint8_t row = 22; row <= 23; row++) {
197  Kfusion[row] = 0.0f;
198 
199  for (uint8_t col = 0; col <= 3; col++) {
200  Kfusion[row] += P[row][col] * H_YAW[col];
201  }
202 
203  Kfusion[row] *= heading_innov_var_inv;
204  }
205  }
206 
207  // innovation test ratio
209 
210  // we are no longer using 3-axis fusion so set the reported test levels to zero
211  memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
212 
213  // set the magnetometer unhealthy if the test fails
214  if (_yaw_test_ratio > 1.0f) {
216 
217  // if we are in air we don't want to fuse the measurement
218  // we allow to use it when on the ground because the large innovation could be caused
219  // by interference or a large initial gyro bias
221  return;
222 
223  } else {
224  // constrain the innovation to the maximum set by the gate
225  float gate_limit = sqrtf((sq(innov_gate) * _heading_innov_var));
226  _heading_innov = math::constrain(_heading_innov, -gate_limit, gate_limit);
227  }
228 
229  } else {
231  }
232 
233  // apply covariance correction via P_new = (I -K*H)*P
234  // first calculate expression for KHP
235  // then calculate P - KHP
236  float KHP[_k_num_states][_k_num_states];
237  float KH[4];
238 
239  for (unsigned row = 0; row < _k_num_states; row++) {
240 
241  KH[0] = Kfusion[row] * H_YAW[0];
242  KH[1] = Kfusion[row] * H_YAW[1];
243  KH[2] = Kfusion[row] * H_YAW[2];
244  KH[3] = Kfusion[row] * H_YAW[3];
245 
246  for (unsigned column = 0; column < _k_num_states; column++) {
247  float tmp = KH[0] * P[0][column];
248  tmp += KH[1] * P[1][column];
249  tmp += KH[2] * P[2][column];
250  tmp += KH[3] * P[3][column];
251  KHP[row][column] = tmp;
252  }
253  }
254 
255  // if the covariance correction will result in a negative variance, then
256  // the covariance matrix is unhealthy and must be corrected
257  bool healthy = true;
258  _fault_status.flags.bad_hdg = false;
259 
260  for (int i = 0; i < _k_num_states; i++) {
261  if (P[i][i] < KHP[i][i]) {
262  // zero rows and columns
263  zeroRows(P, i, i);
264  zeroCols(P, i, i);
265 
266  //flag as unhealthy
267  healthy = false;
268 
269  // update individual measurement health status
270  _fault_status.flags.bad_hdg = true;
271 
272  }
273  }
274 
275  // only apply covariance and state corrections if healthy
276  if (healthy) {
277  // apply the covariance corrections
278  for (unsigned row = 0; row < _k_num_states; row++) {
279  for (unsigned column = 0; column < _k_num_states; column++) {
280  P[row][column] = P[row][column] - KHP[row][column];
281  }
282  }
283 
284  // correct the covariance matrix for gross errors
286 
287  // apply the state corrections
288  fuse(Kfusion, _heading_innov);
289 
290  }
291 }
292 
294 {
295  // check if data has been set to NAN indicating no measurement
297 
298  // define the predicted antenna array vector and rotate into earth frame
299  Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
300  Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
301 
302  // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
303  if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
304  return false;
305  }
306 
307  float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0));
308 
309  // get measurement and correct for antenna array yaw offset
310  float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
311 
312  // calculate the amount the yaw needs to be rotated by
313  float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
314 
315  // save a copy of the quaternion state for later use in calculating the amount of reset change
316  Quatf quat_before_reset = _state.quat_nominal;
317  Quatf quat_after_reset = _state.quat_nominal;
318 
319  // obtain the yaw angle using the best conditioned from either a Tait-Bryan 321 or 312 sequence
320  // to avoid gimbal lock
321  if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
322  // get the roll, pitch, yaw estimates from the quaternion states using a 321 Tait-Bryan rotation sequence
323  Quatf q_init(_state.quat_nominal);
324  Eulerf euler_init(q_init);
325 
326  // correct the yaw angle
327  euler_init(2) += yaw_delta;
328  euler_init(2) = wrap_pi(euler_init(2));
329 
330  // update the quaternions
331  quat_after_reset = Quatf(euler_init);
332 
333  } else {
334  // Calculate the 312 Tait-Bryan sequence euler angles that rotate from earth to body frame
335  // PX4 math library does not support this so are using equations from
336  // http://www.atacolorado.com/eulersequences.doc
337  Vector3f euler312;
338  euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
339  euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
340  euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
341 
342  // correct the yaw angle
343  euler312(0) += yaw_delta;
344  euler312(0) = wrap_pi(euler312(0));
345 
346  // Calculate the body to earth frame rotation matrix from the corrected euler angles
347  float c2 = cosf(euler312(2));
348  float s2 = sinf(euler312(2));
349  float s1 = sinf(euler312(1));
350  float c1 = cosf(euler312(1));
351  float s0 = sinf(euler312(0));
352  float c0 = cosf(euler312(0));
353 
354  Dcmf R_to_earth;
355  R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
356  R_to_earth(1, 1) = c0 * c1;
357  R_to_earth(2, 2) = c2 * c1;
358  R_to_earth(0, 1) = -c1 * s0;
359  R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
360  R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
361  R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
362  R_to_earth(2, 0) = -s2 * c1;
363  R_to_earth(2, 1) = s1;
364 
365  // update the quaternions
366  quat_after_reset = Quatf(R_to_earth);
367  }
368 
369  // calculate the amount that the quaternion has changed by
370  Quatf q_error = _state.quat_nominal * quat_before_reset.inversed();
371  q_error.normalize();
372 
373  // convert the quaternion delta to a delta angle
374  Vector3f delta_ang_error;
375  float scalar;
376 
377  if (q_error(0) >= 0.0f) {
378  scalar = -2.0f;
379 
380  } else {
381  scalar = 2.0f;
382  }
383 
384  delta_ang_error(0) = scalar * q_error(1);
385  delta_ang_error(1) = scalar * q_error(2);
386  delta_ang_error(2) = scalar * q_error(3);
387 
388  // update the quaternion state estimates and corresponding covariances only if the change in angle has been large or the yaw is not yet aligned
389  if (delta_ang_error.norm() > math::radians(15.0f) || !_control_status.flags.yaw_align) {
390  // update quaternion states
391  _state.quat_nominal = quat_after_reset;
393 
394  // record the state change
395  _state_reset_status.quat_change = q_error;
396 
397  // update transformation matrix from body to world frame using the current estimate
399 
400  // reset the rotation from the EV to EKF frame of reference if it is being used
403  }
404 
405  // update the yaw angle variance using the variance of the measurement
407 
408  // add the reset amount to the output observer buffered data
409  for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
410  _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
411  }
412 
413  // apply the change in attitude quaternion to our newest quaternion estimate
414  // which was already taken out from the output buffer
416 
417  // capture the reset event
418  _state_reset_status.quat_counter++;
419 
420  }
421 
422  return true;
423  }
424 
425  return false;
426 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
struct estimator::filter_control_status_u::@60 flags
t34
Definition: calcH_YAWGPS.c:33
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
#define ECL_ERR_TIMESTAMPED
Definition: ecl.h:95
t24
Definition: calcH_YAWGPS.c:23
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
bool resetGpsAntYaw()
t4
Definition: calcH_YAW312.c:5
Dcm< float > Dcmf
Definition: Dcm.hpp:185
t28
Definition: calcH_YAWGPS.c:27
float _heading_innov_var
heading measurement innovation variance (rad**2)
Definition: ekf.h:389
t6
Definition: calcH_YAW312.c:7
t5
Definition: calcH_YAW312.c:6
t14
Definition: calcH_YAW312.c:13
void initialiseCovariance()
Definition: covariance.cpp:49
t19
Definition: calcH_YAWGPS.c:17
struct estimator::fault_status_u::@57 flags
void fuseGpsAntYaw()
t29
Definition: calcH_YAWGPS.c:28
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 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)
t27
Definition: calcH_YAWGPS.c:26
float _heading_innov
heading measurement innovation (rad)
Definition: ekf.h:388
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:118
t33
Definition: calcH_YAWGPS.c:32
void resetExtVisRotMat()
t22
Definition: calcH_YAWGPS.c:21
t31
Definition: calcH_YAWGPS.c:30
void fuse(float *K, float innovation)
t10
Definition: calcH_YAW312.c:2
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
Definition: ekf.h:348
t15
Definition: calcH_YAWGPS.c:14
t16
Definition: calcH_YAWGPS.c:15
t20
Definition: calcH_YAWGPS.c:18
t26
Definition: calcH_YAWGPS.c:25
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
bool bad_hdg
3 - true if the fusion of the heading angle has encountered a numerical error
Definition: common.h:383
t13
Definition: calcH_YAW312.c:12
filter_control_status_u _control_status
t35
Definition: calcH_YAWGPS.c:34
Euler< float > Eulerf
Definition: Euler.hpp:156
static constexpr float sq(float var)
Definition: ekf.h:695
t30
Definition: calcH_YAWGPS.c:29
fault_status_u _fault_status
void increaseQuatYawErrVariance(float yaw_variance)
t32
Definition: calcH_YAWGPS.c:31
void fixCovarianceErrors()
Definition: covariance.cpp:728
bool reject_yaw
6 - true if the yaw observation has been rejected
Definition: common.h:410
#define MASK_USE_EVPOS
set to true to use external vision position data
Definition: common.h:192
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
t11
Definition: calcH_YAW312.c:10
t18
Definition: calcH_YAWGPS.c:19
innovation_fault_status_u _innov_check_fail_status
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Kfusion[0]
Definition: K_VELX.c:93
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
#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
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
uint32_t in_air
7 - true when the vehicle is airborne
Definition: common.h:448
struct Ekf::@62 _state_reset_status
reset event monitoring structure containing velocity, position, height and yaw reset information ...
t2
Definition: calcH_YAW312.c:3
uint32_t wind
8 - true when wind velocity is being estimated
Definition: common.h:449
t17
Definition: calcH_YAWGPS.c:16
void uncorrelateQuatStates()
struct estimator::innovation_fault_status_u::@58 flags
t23
Definition: calcH_YAWGPS.c:22
t7
Definition: calcH_YAW312.c:8
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
float heading_innov_gate
heading fusion innovation consistency gate size (STD)
Definition: common.h:272
Quatf quat_nominal
nominal quaternion describing vehicle attitude
Definition: common.h:93
Class for core functions for ekf attitude and position estimator.
uint8_t get_length() const
Definition: RingBuffer.h:117
t3
Definition: calcH_YAW312.c:4
#define ISFINITE(x)
Definition: ecl.h:100
t25
Definition: calcH_YAWGPS.c:24