PX4 Firmware
PX4 Autopilot Software http://px4.io
vel_pos_fusion.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 vel_pos_fusion.cpp
36  * Function for fusing gps and baro measurements/
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
40  * @author Paul Riseborough <p_riseborough@live.com.au>
41  *
42  */
43 
44 #include "ekf.h"
45 #include <ecl.h>
46 #include <mathlib/mathlib.h>
47 
49 {
50  bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available
51  bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
52  float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
53  float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
54  float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
55  float innovation[6]; // local copy of innovations for [VN,VE,VD,PN,PE,PD]
56  memcpy(innovation, _vel_pos_innov, sizeof(_vel_pos_innov));
57 
58  // calculate innovations, innovations gate sizes and observation variances
60  // enable fusion for NE velocity axes
61  fuse_map[0] = fuse_map[1] = true;
62 
63  // handle special case where we are getting velocity observations from an auxiliary source
64  if (!_fuse_hor_vel) {
65  innovation[0] = _aux_vel_innov[0];
66  innovation[1] = _aux_vel_innov[1];
67  }
68 
69  // Set observation noise variance and innovation consistency check gate size for the NE position observations
70  R[0] = _velObsVarNED(0);
71  R[1] = _velObsVarNED(1);
72  gate_size[1] = gate_size[0] = _hvelInnovGate;
73 
74  }
75 
76  if (_fuse_vert_vel) {
77  fuse_map[2] = true;
78  // observation variance - use receiver reported accuracy with parameter setting the minimum value
79  R[2] = _velObsVarNED(2);
80  // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
81  R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc);
82  R[2] = R[2] * R[2];
83  // innovation gate size
84  gate_size[2] = _vvelInnovGate;
85  }
86 
87  if (_fuse_pos) {
88  // enable fusion for the NE position axes
89  fuse_map[3] = fuse_map[4] = true;
90 
91  // Set observation noise variance and innovation consistency check gate size for the NE position observations
92  R[4] = R[3] = sq(_posObsNoiseNE);
93  gate_size[4] = gate_size[3] = _posInnovGateNE;
94 
95  }
96 
97  if (_fuse_height) {
99  fuse_map[5] = true;
100  // vertical position innovation - baro measurement has opposite sign to earth z axis
102  // observation variance - user parameter defined
103  R[5] = fmaxf(_params.baro_noise, 0.01f);
104  R[5] = R[5] * R[5];
105  // innovation gate size
106  gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
107 
108  // Compensate for positive static pressure transients (negative vertical position innovations)
109  // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
110  float deadzone_start = 0.0f;
111  float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
112 
114  if (innovation[5] < -deadzone_start) {
115  if (innovation[5] <= -deadzone_end) {
116  innovation[5] += deadzone_end;
117 
118  } else {
119  innovation[5] = -deadzone_start;
120  }
121  }
122  }
123 
124  } else if (_control_status.flags.gps_hgt) {
125  fuse_map[5] = true;
126  // vertical position innovation - gps measurement has opposite sign to earth z axis
128  // observation variance - receiver defined and parameter limited
129  // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
130  float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
131  float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
132  R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit);
133  R[5] = R[5] * R[5];
134  // innovation gate size
135  gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
136 
138  fuse_map[5] = true;
139  // use range finder with tilt correction
142  // observation variance - user parameter defined
144  // innovation gate size
145  gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
146 
147  } else if (_control_status.flags.ev_hgt) {
148  fuse_map[5] = true;
149  // calculate the innovation assuming the external vision observation is in local NED frame
150  innovation[5] = _state.pos(2) - _ev_sample_delayed.pos(2);
151  // observation variance - defined externally
152  R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
153  R[5] = R[5] * R[5];
154  // innovation gate size
155  gate_size[5] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
156  }
157 
158  // update innovation class variable for logging purposes
159  _vel_pos_innov[5] = innovation[5];
160  }
161 
162  // calculate innovation test ratios
163  for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
164  if (fuse_map[obs_index]) {
165  // compute the innovation variance SK = HPH + R
166  unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
167  _vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
168  // Compute the ratio of innovation to gate size
169  _vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) *
170  _vel_pos_innov_var[obs_index]);
171  }else{
172  _vel_pos_test_ratio[obs_index] = 0;
173  }
174  }
175 
176  // check position, velocity and height innovations
177  // treat 3D velocity, 2D position and height as separate sensors
178  // always pass position checks if using synthetic position measurements or yet to complete tilt alignment
179  // always pass height checks if yet to complete tilt alignment
180  bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f)
181  && (_vel_pos_test_ratio[2] <= 1.0f);
182  innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
183  bool pos_check_pass = ((_vel_pos_test_ratio[3] <= 1.0f) && (_vel_pos_test_ratio[4] <= 1.0f)) || !_control_status.flags.tilt_align;
184  innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
185  innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
186 
187  // record the successful velocity fusion event
188  if ((_fuse_hor_vel || _fuse_hor_vel_aux || _fuse_vert_vel) && vel_check_pass) {
191 
192  } else if (!vel_check_pass) {
194  }
195 
197 
198  // record the successful position fusion event
199  if (pos_check_pass && _fuse_pos) {
200  if (!_fuse_hpos_as_odom) {
202 
203  } else {
205  }
206 
208 
209  } else if (!pos_check_pass) {
211  }
212 
213  _fuse_pos = false;
214 
215  // record the successful height fusion event
216  if (innov_check_pass_map[5] && _fuse_height) {
219 
220  } else if (!innov_check_pass_map[5]) {
222  }
223 
224  _fuse_height = false;
225 
226  for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
227  // skip fusion if not requested or checks have failed
228  if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
229  continue;
230  }
231 
232  unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
233 
234  // calculate kalman gain K = PHS, where S = 1/innovation variance
235  for (int row = 0; row < _k_num_states; row++) {
236  Kfusion[row] = P[row][state_index] / _vel_pos_innov_var[obs_index];
237  }
238 
239  // update covariance matrix via Pnew = (I - KH)P
240  float KHP[_k_num_states][_k_num_states];
241 
242  for (unsigned row = 0; row < _k_num_states; row++) {
243  for (unsigned column = 0; column < _k_num_states; column++) {
244  KHP[row][column] = Kfusion[row] * P[state_index][column];
245  }
246  }
247 
248  // if the covariance correction will result in a negative variance, then
249  // the covariance matrix is unhealthy and must be corrected
250  bool healthy = true;
251 
252  for (int i = 0; i < _k_num_states; i++) {
253  if (P[i][i] < KHP[i][i]) {
254  // zero rows and columns
255  zeroRows(P, i, i);
256  zeroCols(P, i, i);
257 
258  //flag as unhealthy
259  healthy = false;
260 
261  // update individual measurement health status
262  if (obs_index == 0) {
264 
265  } else if (obs_index == 1) {
267 
268  } else if (obs_index == 2) {
270 
271  } else if (obs_index == 3) {
273 
274  } else if (obs_index == 4) {
276 
277  } else if (obs_index == 5) {
279  }
280 
281  } else {
282  // update individual measurement health status
283  if (obs_index == 0) {
284  _fault_status.flags.bad_vel_N = false;
285 
286  } else if (obs_index == 1) {
287  _fault_status.flags.bad_vel_E = false;
288 
289  } else if (obs_index == 2) {
290  _fault_status.flags.bad_vel_D = false;
291 
292  } else if (obs_index == 3) {
293  _fault_status.flags.bad_pos_N = false;
294 
295  } else if (obs_index == 4) {
296  _fault_status.flags.bad_pos_E = false;
297 
298  } else if (obs_index == 5) {
299  _fault_status.flags.bad_pos_D = false;
300  }
301  }
302  }
303 
304  // only apply covariance and state corrections if healthy
305  if (healthy) {
306  // apply the covariance corrections
307  for (unsigned row = 0; row < _k_num_states; row++) {
308  for (unsigned column = 0; column < _k_num_states; column++) {
309  P[row][column] = P[row][column] - KHP[row][column];
310  }
311  }
312 
313  // correct the covariance matrix for gross errors
315 
316  // apply the state corrections
317  fuse(Kfusion, innovation[obs_index]);
318  }
319  }
320 }
float ev_pos_innov_gate
vision position fusion innovation consistency gate size (STD)
Definition: common.h:305
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
struct estimator::filter_control_status_u::@60 flags
float _posInnovGateNE
Number of standard deviations used for the NE position fusion innovation consistency check...
Definition: ekf.h:300
bool _fuse_height
true when baro height data should be fused
Definition: ekf.h:293
float hgt
gps height measurement (m)
Definition: common.h:116
bool reject_vel_NED
0 - true if velocity observations have been rejected
Definition: common.h:404
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
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
bool reject_pos_NE
1 - true if horizontal position observations have been rejected
Definition: common.h:405
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
Definition: ekf.h:372
bool bad_pos_D
14 - true if fusion of the Down position has encountered a numerical error
Definition: common.h:394
bool reject_pos_D
2 - true if true if vertical position observations have been rejected
Definition: common.h:406
float range_cos_max_tilt
cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data ...
Definition: common.h:299
float _gps_alt_ref
WGS-84 height (m)
Definition: ekf.h:425
float gnd_effect_deadzone
Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) ...
Definition: common.h:265
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
Definition: ekf.h:435
float gps_pos_noise
minimum allowed observation noise for gps position fusion (m)
Definition: common.h:259
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
Definition: common.h:451
bool bad_vel_N
9 - true if fusion of the North velocity has encountered a numerical error
Definition: common.h:389
bool _fuse_hpos_as_odom
true when the NE position data is being fused using an odometry assumption
Definition: ekf.h:307
bool bad_pos_N
12 - true if fusion of the North position has encountered a numerical error
Definition: common.h:392
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
Definition: common.h:450
bool _fuse_vert_vel
true when gps vertical velocity measurement should be fused
Definition: ekf.h:296
struct estimator::fault_status_u::@57 flags
rangeSample _range_sample_delayed
float vacc
1-std vertical position error (m)
Definition: common.h:120
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:160
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 rng
range (distance to ground) measurement (m)
Definition: common.h:136
uint64_t _time_last_hgt_fuse
time the last fusion of height measurements was performed (uSec)
Definition: ekf.h:333
float sacc
1-std speed error (m/sec)
Definition: common.h:121
bool _fuse_hor_vel
true when gps horizontal velocity measurement should be fused
Definition: ekf.h:295
void fuse(float *K, float innovation)
float _hvelInnovGate
Number of standard deviations used for the horizontal velocity fusion innovation consistency check...
Definition: ekf.h:303
float baro_noise
observation noise for barometric height fusion (m)
Definition: common.h:261
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
Definition: ekf.h:330
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
Definition: common.h:461
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float rng_gnd_clearance
minimum valid value for range when on ground (m)
Definition: common.h:291
filter_control_status_u _control_status
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
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
Definition: common.h:455
static constexpr float sq(float var)
Definition: ekf.h:695
float baro_innov_gate
barometric and GPS height innovation consistency gate size (STD)
Definition: common.h:262
fault_status_u _fault_status
float hgt
pressure altitude above sea level (m)
Definition: common.h:131
float _posObsNoiseNE
1-STD observation noise used for the fusion of NE position data (m)
Definition: ekf.h:299
void fixCovarianceErrors()
Definition: covariance.cpp:728
Vector3f pos
NED position in earth frame in m.
Definition: common.h:370
void fuseVelPosHeight()
innovation_fault_status_u _innov_check_fail_status
extVisionSample _ev_sample_delayed
float _R_rng_to_earth_2_2
2,2 element of the rotation matrix from sensor frame to earth frame
Definition: ekf.h:465
float _vvelInnovGate
Number of standard deviations used for the vertical velocity fusion innovation consistency check...
Definition: ekf.h:304
Kfusion[0]
Definition: K_VELX.c:93
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
bool bad_vel_E
10 - true if fusion of the East velocity has encountered a numerical error
Definition: common.h:390
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
bool _fuse_pos
true when gps position data should be fused
Definition: ekf.h:294
float range_noise_scaler
scaling from range measurement to noise (m/m)
Definition: common.h:293
Vector3f _velObsVarNED
1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 ...
Definition: ekf.h:302
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
Definition: ekf.h:331
float range_innov_gate
range finder fusion innovation consistency gate size (STD)
Definition: common.h:290
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
Definition: common.h:452
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
float pos_noaid_noise
observation noise for non-aiding position fusion (m)
Definition: common.h:260
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
Definition: common.h:441
struct estimator::innovation_fault_status_u::@58 flags
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
Class for core functions for ekf attitude and position estimator.
bool bad_vel_D
11 - true if fusion of the Down velocity has encountered a numerical error
Definition: common.h:391
bool bad_pos_E
13 - true if fusion of the East position has encountered a numerical error
Definition: common.h:393
bool _fuse_hor_vel_aux
true when auxiliary horizontal velocity measurement should be fused
Definition: ekf.h:297