46 #include <mathlib/mathlib.h> 50 bool fuse_map[6] = {};
51 bool innov_check_pass_map[6] = {};
53 float gate_size[6] = {};
61 fuse_map[0] = fuse_map[1] =
true;
89 fuse_map[3] = fuse_map[4] =
true;
110 float deadzone_start = 0.0f;
114 if (innovation[5] < -deadzone_start) {
115 if (innovation[5] <= -deadzone_end) {
116 innovation[5] += deadzone_end;
119 innovation[5] = -deadzone_start;
163 for (
unsigned obs_index = 0; obs_index < 6; obs_index++) {
164 if (fuse_map[obs_index]) {
166 unsigned state_index = obs_index + 4;
182 innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass;
184 innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass;
192 }
else if (!vel_check_pass) {
209 }
else if (!pos_check_pass) {
220 }
else if (!innov_check_pass_map[5]) {
226 for (
unsigned obs_index = 0; obs_index < 6; obs_index++) {
228 if (!fuse_map[obs_index] || !innov_check_pass_map[obs_index]) {
232 unsigned state_index = obs_index + 4;
243 for (
unsigned column = 0; column <
_k_num_states; column++) {
244 KHP[row][column] = Kfusion[row] *
P[state_index][column];
253 if (
P[i][i] < KHP[i][i]) {
262 if (obs_index == 0) {
265 }
else if (obs_index == 1) {
268 }
else if (obs_index == 2) {
271 }
else if (obs_index == 3) {
274 }
else if (obs_index == 4) {
277 }
else if (obs_index == 5) {
283 if (obs_index == 0) {
286 }
else if (obs_index == 1) {
289 }
else if (obs_index == 2) {
292 }
else if (obs_index == 3) {
295 }
else if (obs_index == 4) {
298 }
else if (obs_index == 5) {
308 for (
unsigned column = 0; column <
_k_num_states; column++) {
309 P[row][column] =
P[row][column] - KHP[row][column];
317 fuse(Kfusion, innovation[obs_index]);
float ev_pos_innov_gate
vision position fusion innovation consistency gate size (STD)
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
struct estimator::filter_control_status_u::@60 flags
float _posInnovGateNE
Number of standard deviations used for the NE position fusion innovation consistency check...
bool _fuse_height
true when baro height data should be fused
float hgt
gps height measurement (m)
bool reject_vel_NED
0 - true if velocity observations have been rejected
Adapter / shim layer for system calls needed by ECL.
stateSample _state
state struct of the ekf running at the delayed time horizon
float _vel_pos_innov_var[6]
NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) ...
bool reject_pos_NE
1 - true if horizontal position observations have been rejected
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
bool bad_pos_D
14 - true if fusion of the Down position has encountered a numerical error
bool reject_pos_D
2 - true if true if vertical position observations have been rejected
float range_cos_max_tilt
cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data ...
float _gps_alt_ref
WGS-84 height (m)
float gnd_effect_deadzone
Size of deadzone applied to negative baro innovations when ground effect compensation is active (m) ...
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
float gps_pos_noise
minimum allowed observation noise for gps position fusion (m)
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
bool bad_vel_N
9 - true if fusion of the North velocity has encountered a numerical error
bool _fuse_hpos_as_odom
true when the NE position data is being fused using an odometry assumption
bool bad_pos_N
12 - true if fusion of the North position has encountered a numerical error
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
bool _fuse_vert_vel
true when gps vertical velocity measurement should be fused
struct estimator::fault_status_u::@57 flags
rangeSample _range_sample_delayed
float vacc
1-std vertical position error (m)
float hgtErr
1-Sigma height accuracy (m)
float P[_k_num_states][_k_num_states]
state covariance matrix
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.
float rng
range (distance to ground) measurement (m)
uint64_t _time_last_hgt_fuse
time the last fusion of height measurements was performed (uSec)
float sacc
1-std speed error (m/sec)
bool _fuse_hor_vel
true when gps horizontal velocity measurement should be fused
void fuse(float *K, float innovation)
float _hvelInnovGate
Number of standard deviations used for the horizontal velocity fusion innovation consistency check...
float baro_noise
observation noise for barometric height fusion (m)
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
baroSample _baro_sample_delayed
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
float rng_gnd_clearance
minimum valid value for range when on ground (m)
filter_control_status_u _control_status
float _aux_vel_innov[2]
NE auxiliary velocity innovations: (m/sec)
float range_noise
observation noise for range finder measurements (m)
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
static constexpr float sq(float var)
float baro_innov_gate
barometric and GPS height innovation consistency gate size (STD)
fault_status_u _fault_status
float hgt
pressure altitude above sea level (m)
gpsSample _gps_sample_delayed
float _posObsNoiseNE
1-STD observation noise used for the fusion of NE position data (m)
void fixCovarianceErrors()
Vector3f pos
NED position in earth frame in m.
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
float _vvelInnovGate
Number of standard deviations used for the vertical velocity fusion innovation consistency check...
constexpr _Tp max(_Tp a, _Tp b)
bool bad_vel_E
10 - true if fusion of the East velocity has encountered a numerical error
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
float _vel_pos_test_ratio[6]
float range_noise_scaler
scaling from range measurement to noise (m/m)
Vector3f _velObsVarNED
1-STD observation noise variance used for the fusion of NED velocity data (m/sec)**2 ...
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
float range_innov_gate
range finder fusion innovation consistency gate size (STD)
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
uint64_t _time_last_vel_fuse
time the last fusion of velocity measurements was performed (uSec)
float _baro_hgt_offset
baro height reading at the local NED origin (m)
float pos_noaid_noise
observation noise for non-aiding position fusion (m)
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
struct estimator::innovation_fault_status_u::@58 flags
static constexpr uint8_t _k_num_states
number of EKF states
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
bool bad_pos_E
13 - true if fusion of the East position has encountered a numerical error
bool _fuse_hor_vel_aux
true when auxiliary horizontal velocity measurement should be fused