45 #include <mathlib/mathlib.h> 76 vel_optflow_body(2) = 0.0f;
221 bool vert_pos_reset =
false;
223 bool vert_vel_reset =
false;
240 vert_pos_reset =
true;
260 vert_pos_reset =
true;
278 vert_pos_reset =
true;
296 vert_pos_reset =
true;
317 P[6][6] =
sq(1.5
f * gps_newest.
sacc);
329 vert_vel_reset =
true;
332 if (vert_pos_reset) {
337 if (vert_vel_reset) {
344 if (vert_pos_reset) {
348 if (vert_vel_reset) {
354 if (vert_pos_reset) {
359 if (vert_vel_reset) {
366 if (vert_pos_reset) {
371 if (vert_vel_reset) {
421 float courseYawError = gpsCOG - ekfGOG;
424 bool badYawErr = fabsf(courseYawError) > 0.5f;
425 bool badMagYaw = (badYawErr && badVelInnov);
454 euler321(2) += courseYawError;
485 float sineYawError =
math::constrain(sqrtf(SpdErrorVariance) / gpsSpeed, 0.0
f, 1.0
f);
494 for (uint8_t index = 16; index <= 21; index ++) {
532 for (uint8_t index = 16; index <= 21; index ++) {
583 Dcmf R_to_earth(euler321);
590 euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
594 Vector3f mag_earth_pred = R_to_earth * mag_init;
596 euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) +
getMagDeclination();
609 quat_after_reset =
Quatf(euler321);
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));
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;
648 euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
652 Vector3f mag_earth_pred = R_to_earth * mag_init;
654 euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) +
getMagDeclination();
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;
680 quat_after_reset =
Quatf(R_to_earth);
684 Dcmf R_to_earth_after(quat_after_reset);
691 for (uint8_t index = 16; index <= 21; index ++) {
703 Quatf q_error = quat_after_reset * quat_before_reset.inversed();
721 if (increase_yaw_var) {
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;
786 for (
int i = 0; i < 4; i++) {
790 for (
int i = 0; i < 3; i++) {
794 for (
int i = 0; i < 3; i++) {
798 for (
int i = 0; i < 3; i++) {
802 for (
int i = 0; i < 3; i++) {
806 for (
int i = 0; i < 3; i++) {
810 for (
int i = 0; i < 3; i++) {
814 for (
int i = 0; i < 2; i++) {
843 memcpy(mag_innov,
_mag_innov, 3 *
sizeof(
float));
904 for (
int i = 0; i < 4; i++) {
908 for (
int i = 0; i < 3; i++) {
912 for (
int i = 0; i < 3; i++) {
916 for (
int i = 0; i < 3; i++) {
920 for (
int i = 0; i < 3; i++) {
924 for (
int i = 0; i < 3; i++) {
928 for (
int i = 0; i < 3; i++) {
932 for (
int i = 0; i < 2; i++) {
944 memcpy(bias, temp, 3 *
sizeof(
float));
954 memcpy(bias, temp, 3 *
sizeof(
float));
1019 *ekf_eph = hpos_err;
1027 float hpos_err = sqrtf(
P[7][7] +
P[8][8]);
1036 *ekf_eph = hpos_err;
1037 *ekf_epv = sqrtf(
P[9][9]);
1043 float hvel_err = sqrtf(
P[4][4] +
P[5][5]);
1049 float vel_err_conservative = 0.0f;
1065 hvel_err =
math::max(hvel_err, vel_err_conservative);
1068 *ekf_evh = hvel_err;
1069 *ekf_evv = sqrtf(
P[6][6]);
1105 if (relying_on_rangefinder) {
1108 *hagl_min = rangefinder_hagl_min;
1109 *hagl_max = rangefinder_hagl_max;
1113 if (relying_on_optical_flow) {
1114 *vxy_max = flow_vxy_max;
1116 *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min);
1117 *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max);
1195 soln_status.
flags.
gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good;
1197 *status = soln_status.
value;
1203 for (
unsigned i = 0; i < 4; i++) {
1209 for (
unsigned i = 0; i < 3; i++) {
1213 for (
unsigned i = 0; i < 3; i++) {
1217 for (
unsigned i = 0; i < 3; i++) {
1221 for (
unsigned i = 0; i < 3; i++) {
1225 for (
unsigned i = 0; i < 3; i++) {
1229 for (
unsigned i = 0; i < 3; i++) {
1233 for (
unsigned i = 0; i < 2; i++) {
1243 for (row = first; row <= last; row++) {
1244 memset(&cov_mat[row][0], 0,
sizeof(cov_mat[0][0]) * 24);
1253 for (row = 0; row <= 23; row++) {
1254 memset(&cov_mat[row][first], 0,
sizeof(cov_mat[0][0]) * (1 + last - first));
1264 for (row = first; row <= last; row++) {
1265 variances[row] = cov_mat[row][row];
1273 for (row = first; row <= last; row++) {
1274 cov_mat[row][row] = variances[row];
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];
1295 for (row = 0; row < 4; row++) {
1296 for (col = 0; col < 4; col++) {
1297 P[row][col] = variances[row][col];
1311 for (row = first; row <= last; row++) {
1312 cov_mat[row][row] = variance;
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);
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);
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);
1389 float q0, q1, q2, q3;
1404 float t3 = acosf(q0);
1405 float t4 = -t2+1.0f;
1407 if ((t4 > 1e-9
f) && (t5 < -1e-9
f)) {
1409 float t7 = q1*t6*2.0f;
1410 float t8 = 1.0f/powf(t4,1.5
f);
1411 float t9 = q0*q1*t3*t8*2.0f;
1413 float t11 = 1.0f/sqrtf(t4);
1414 float t12 = q2*t6*2.0f;
1415 float t13 = q0*q2*t3*t8*2.0f;
1417 float t15 = q3*t6*2.0f;
1418 float t16 = q0*q3*t3*t8*2.0f;
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.0
f)*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;
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];
1449 float delta = 2.0f*acosf(q0);
1450 float scaler = (delta/sinf(delta*0.5
f));
1451 float rotX = scaler*q1;
1452 float rotY = scaler*q2;
1453 float rotZ = scaler*q3;
1456 float t2 = rotX*rotX;
1457 float t4 = rotY*rotY;
1458 float t5 = rotZ*rotZ;
1459 float t6 = t2+t4+
t5;
1461 float t7 = sqrtf(t6);
1463 float t3 = sinf(t8);
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);
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;
1476 float t23 = rotX*rotY*t10*t12*0.5f;
1478 float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f;
1479 float t25 = rotX*rotZ*t10*t12*0.5f;
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;
1485 float t29 = t4*t10*t12*0.5f;
1488 float t32 = t5*t10*t12*0.5f;
1491 float t36 = rotY*rotZ*t10*t12*0.5f;
1493 float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f;
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;
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;
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;
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);
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;
1521 P[3][3] = rot_vec_var(2)*(t33*
t33)+rot_vec_var(0)*(t43*
t43)+rot_vec_var(1)*(t44*
t44);
1530 P[1][1] = 0.25f * rot_vec_var(0);
1535 P[2][2] = 0.25f * rot_vec_var(1);
1540 P[3][3] = 0.25f * rot_vec_var(2);
1624 q_error.normalize();
1627 Vector3f rot_vec = q_error.to_axis_angle();
1629 float rot_vec_norm = rot_vec.norm();
1631 if (rot_vec_norm > 1e-6
f) {
1635 float input_delta_len = _input_delta_vec.norm();
1637 if (input_delta_len > 0.1
f) {
1638 rot_vec = _ev_rot_vec_filt + _input_delta_vec * (0.1f / input_delta_len);
1642 const float omega_lpf_us = 0.2e-6
f;
1645 _ev_rot_vec_filt = _ev_rot_vec_filt * (1.0f - alpha) + rot_vec * alpha;
1661 q_error.normalize();
1664 Vector3f rot_vec = q_error.to_axis_angle();
1666 float rot_vec_norm = rot_vec.norm();
1668 if (rot_vec_norm > 1e-9
f) {
1685 for (
unsigned i = 0; i < 4; i++) {
1686 quat[i] = quat_ev2ekf(i);
1710 yaw_variance = fminf(yaw_variance, 1.0e-2
f);
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];
1736 for (uint8_t index = 0; index <= 3; index ++) {
1741 for (uint8_t row = 0; row <= 1; row ++) {
1742 for (uint8_t col = 0; col <= 1; col ++) {
1751 for (uint8_t index = 0; index <= 3; index ++) {
1755 for (uint8_t row = 0; row <= 1; row ++) {
1756 for (uint8_t col = 0; col <= 1; col ++) {
1764 float y = input - accumulator;
1765 float t = sum_previous + y;
1766 accumulator = (t - sum_previous) - y;
Matrix3f quat_to_invrotmat(const Quatf &quat)
float _gps_drift_metrics[3]
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
struct estimator::filter_control_status_u::@60 flags
void setControlBaroHeight()
uint16_t velocity_horiz
1 - True if the horizontal velocity estimate is good
float _beta_innov
synthetic sideslip measurement innovation (rad)
float switch_on_gyro_bias
1-sigma gyro bias uncertainty at switch on (rad/sec)
float hgt
gps height measurement (m)
Adapter / shim layer for system calls needed by ECL.
stateSample _state
state struct of the ekf running at the delayed time horizon
static struct vehicle_status_s status
float _vel_pos_innov_var[6]
NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2) ...
void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) override
float _flow_innov[2]
flow measurement innovation (rad/sec)
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
float mag_heading_noise
measurement noise used for simple heading fusion (rad)
RingBuffer< outputSample > _output_buffer
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) ...
void setControlGPSHeight()
uint64_t _ev_rot_last_time_us
previous time that the calculation of the EV to EKF rotation matrix was updated (uSec) ...
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var=true, bool update_buffer=true)
#define MAG_FUSE_TYPE_3D
Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised...
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) ...
bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) override
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
void get_state_delayed(float *state) override
uint16_t pos_horiz_abs
4 - True if the horizontal position (absolute) estimate is good
void get_heading_innov(float *heading_innov) override
const data_type & get_newest()
Vector3f accel_bias
delta velocity bias estimate in m/s
float _gps_alt_ref
WGS-84 height (m)
uint64_t _last_imu_bias_cov_reset_us
time the last reset of IMU delta angle and velocity state covariances was performed (uSec) ...
uint32_t fuse_aspd
19 - true when airspeed measurements are being fused
Vector2f pos
NE earth frame gps horizontal position measurement (m)
void get_gyro_bias(float bias[3]) override
Quatf quat
quaternion defining rotation from body to earth frame
float _heading_innov_var
heading measurement innovation variance (rad**2)
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
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...
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
void get_output_tracking_error(float error[3]) override
float _hgt_sensor_offset
set as necessary if desired to maintain the same height after a height reset (m)
uint64_t _last_gps_origin_time_us
true when all active GPS checks have passed
void get_mag_innov_var(float mag_innov_var[3]) override
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
bool _is_wind_dead_reckoning
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
uint64_t time_us
timestamp of the measurement (uSec)
uint16_t pos_vert_abs
5 - True if the vertical position (absolute) estimate is good
Vector3f mag_I
NED earth magnetic field in gauss.
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
RingBuffer< outputVert > _output_vert_buffer
float kahanSummation(float sum_previous, float input, float &accumulator) const
void get_imu_vibe_metrics(float vibe[3]) override
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...
float switch_on_accel_bias
1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
uint16_t velocity_vert
2 - True if the vertical velocity estimate is good
float _mag_innov[3]
earth magnetic field innovations (Gauss)
float _flow_max_rate
maximum angular flow rate that the optical flow sensor can measure (rad/s)
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
float dt
amount of integration time (sec)
Vector2f posNE_change
North, East position change due to last reset (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 _flow_min_distance
minimum distance that the optical flow sensor can operate at (m)
float vel_d
D velocity calculated using alternative algorithm (m/sec)
const T & getState() const
float rng
range (distance to ground) measurement (m)
#define GPS_MAX_INTERVAL
Maximum allowable time interval between GPS measurements (uSec)
float _terrain_vpos
estimated vertical position of the terrain underneath the vehicle in local NED frame (m) ...
bool _NED_origin_initialised
float _heading_innov
heading measurement innovation (rad)
uint64_t _time_last_of_fuse
time the last fusion of optical flow measurements were performed (uSec)
#define MASK_USE_GEO_DECL
set to true to use the declination from the geo library when the GPS position becomes available...
float sacc
1-std speed error (m/sec)
#define BARO_MAX_INTERVAL
Maximum allowable time interval between pressure altitude measurements (uSec)
RingBuffer< gpsSample > _gps_buffer
float getMagDeclination()
gps_check_fail_status_u _gps_check_fail_status
void fuse(float *K, float innovation)
AlphaFilterVector3f _mag_lpf
filtered magnetometer measurement (Gauss)
Vector3f _prev_dvel_bias_var
saved delta velocity XYZ bias variances (m/sec)**2
void get_airspeed_innov_var(float *airspeed_innov_var) override
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
void calcEarthRateNED(Vector3f &omega, float lat_rad) const
void get_accel_bias(float bias[3]) override
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
float _flow_max_distance
maximum distance that the optical flow sensor can operate at (m)
uint64_t time_us
timestamp of the measurement (uSec)
void update_deadreckoning_status()
float baro_noise
observation noise for barometric height fusion (m)
outputVert _output_vert_delayed
int32_t mag_declination_source
bitmask used to control the handling of declination data
void get_heading_innov_var(float *heading_innov_var) override
uint64_t _time_last_pos_fuse
time the last fusion of horizontal position measurements was performed (uSec)
float acc_bias_lim
maximum accel bias magnitude (m/sec**2)
void zeroOffDiag(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
float posErr
1-Sigma horizontal position accuracy (m)
float mag_noise
measurement noise used for 3-axis magnetoemeter fusion (Gauss)
Vector3f calcRotVecVariances()
float _beta_innov_var
synthetic sideslip measurement innovation variance (rad**2)
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
void get_vel_pos_innov_var(float vel_pos_innov_var[6]) override
int32_t valid_timeout_max
amount of time spent inertial dead reckoning before the estimator reports the state estimates as inva...
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)...
uint64_t time_us
timestamp of the measurement (uSec)
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 ...
Vector3f pos
NED position estimate in earth frame (m/sec)
float _output_tracking_error[3]
contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
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 ...
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool get_gps_drift_metrics(float drift[3], bool *blocked) override
uint16_t attitude
0 - True if the attitude estimate is good
Vector2< float > Vector2f
float rng_gnd_clearance
minimum valid value for range when on ground (m)
filter_control_status_u _control_status
void get_airspeed_innov(float *airspeed_innov) override
RingBuffer< baroSample > _baro_buffer
float _mag_declination_gps
Vector3f vel
NED velocity in earth frame in m/s.
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)
float range_noise
observation noise for range finder measurements (m)
uint16_t pos_horiz_rel
3 - True if the horizontal position (relative) estimate is good
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
int32_t mag_fusion_type
integer used to specify the type of magnetometer fusion used
static constexpr float sq(float var)
float _mag_innov_var[3]
earth magnetic field innovation variance (Gauss**2)
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
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 ...
uint16_t pos_vert_agl
6 - True if the vertical position (above ground) estimate is good
float hgt
pressure altitude above sea level (m)
float _airspeed_innov_var
airspeed measurement innovation variance ((m/sec)**2)
void increaseQuatYawErrVariance(float yaw_variance)
gpsSample _gps_sample_delayed
bool _hpos_prev_available
true when previous values of the estimate and measurement are available for use
static constexpr float CONSTANTS_EARTH_SPIN_RATE
Dcmf _ev_rot_mat
transformation matrix that rotates observations from the EV to the EKF navigation frame...
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
imuSample _imu_sample_delayed
#define ECL_WARN_TIMESTAMPED
uint64_t _time_ins_deadreckon_start
amount of time we have been doing inertial only deadreckoning (uSec)
void setControlRangeHeight()
bool _deadreckon_time_exceeded
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
void setControlEVHeight()
void get_mag_innov(float mag_innov[3]) override
void get_aux_vel_innov(float aux_vel_innov[2]) override
Vector3f pos
NED position in earth frame in m.
bool _mag_use_inhibit
true when magnetometer use is being inhibited
innovation_fault_status_u _innov_check_fail_status
Vector3< float > Vector3f
void makeSymmetrical(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
unsigned no_aid_timeout_max
maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift befor...
extVisionSample _ev_sample_delayed
uint32_t opt_flow
3 - true if optical flow measurements are being fused
float _R_rng_to_earth_2_2
2,2 element of the rotation matrix from sensor frame to earth frame
uint64_t _time_last_arsp_fuse
time the last fusion of airspeed measurements were performed (uSec)
Vector3f mag_B
magnetometer bias estimate in body frame in gauss
#define MAG_FUSE_TYPE_NONE
Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the ...
constexpr _Tp max(_Tp a, _Tp b)
uint64_t _flt_mag_align_start_time
time that inflight magnetic field alignment started (uSec)
#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 ...
uint64_t time_us
timestamp of the measurement (uSec)
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Quaternion< float > Quatf
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)
uint32_t in_air
7 - true when the vehicle is airborne
float mag_declination_deg
magnetic declination (degrees)
struct Ekf::@62 _state_reset_status
reset event monitoring structure containing velocity, position, height and yaw reset information ...
flowSample _flow_sample_delayed
void get_vel_pos_innov(float vel_pos_innov[6]) override
float vel_d_integ
Integral of vel_d (m)
float _vel_pos_test_ratio[6]
Vector2f _last_known_posNE
last known local NE position vector (m)
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...
uint32_t wind
8 - true when wind velocity is being estimated
uint64_t _time_last_delpos_fuse
time the last fusion of incremental horizontal position measurements was performed (uSec) ...
void get_gps_check_status(uint16_t *val) override
struct estimator::ekf_solution_status::@61 flags
uint32_t ev_pos
12 - true when local position data from external vision is being fused
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
float _airspeed_innov
airspeed measurement innovation (m/sec)
float hacc
1-std horizontal position error (m)
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
float angErr
1-Sigma angular error (rad)
outputVert _output_vert_new
uint8_t _num_bad_flight_yaw_events
number of times a bad heading has been detected in flight and required a yaw reset ...
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)
uint32_t gps
2 - true if GPS measurements are being fused
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
static constexpr float FILTER_UPDATE_PERIOD_S
bool _bad_vert_accel_detected
true when bad vertical accelerometer data has been detected
void uncorrelateQuatStates()
Vector2f wind_vel
wind velocity in m/s
Vector3f _ev_rot_vec_filt
filtered rotation vector defining the rotation EV to EKF reference, initiliazied to zero rotation (ra...
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
float velErr
1-Sigma velocity accuracy (m/sec)
Vector3f mag
NED magnetometer body frame measurements (Gauss)
void get_beta_innov(float *beta_innov) override
static constexpr uint8_t _k_num_states
number of EKF states
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)
Vector3f gyro_bias
delta angle bias estimate in rad
Quatf quat_nominal
nominal quaternion describing vehicle attitude
float _dt_ekf_avg
average update rate of the ekf
Class for core functions for ekf attitude and position estimator.
bool isTerrainEstimateValid() const override
void get_beta_innov_var(float *beta_innov_var) override
uint16_t accel_error
11 - True if the EKF has detected bad accelerometer data
bool _using_synthetic_position
true if we are using a synthetic position to constrain drift
outputSample _output_sample_delayed
bool global_position_is_valid() override
float calcOptFlowMeasVar()
uint8_t get_length() const
uint16_t gps_glitch
10 - True if the EKF has detected a GPS glitch
magSample _mag_sample_delayed