45 #include <mathlib/mathlib.h> 76 predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
81 float t4 = q0*q3*2.0f;
86 float t9 = q1*q2*2.0f;
87 float t10 = t5+t6-t7-
t8;
91 float t14 = t5-t6+t7-
t8;
103 float t22 =
sq(t11-t19);
108 float t23 = q1*t3*2.0f;
109 float t24 = q2*t2*2.0f;
112 float t27 = q1*t2*2.0f;
114 float t29 = t28+1.0f;
115 if (fabsf(t29) < 1e-6
f) {
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;
125 H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*
t32)/(t18*t21+1.0
f);
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);
139 measured_hdg =
wrap_pi(measured_hdg);
153 for (
unsigned row = 0; row <= 3; row++) {
156 for (uint8_t col = 0; col <= 3; col++) {
157 PH[row] +=
P[row][col] * H_YAW[col];
163 float heading_innov_var_inv;
185 for (uint8_t row = 0; row <= 15; row++) {
188 for (uint8_t col = 0; col <= 3; col++) {
189 Kfusion[row] +=
P[row][col] * H_YAW[col];
192 Kfusion[row] *= heading_innov_var_inv;
196 for (uint8_t row = 22; row <= 23; row++) {
199 for (uint8_t col = 0; col <= 3; col++) {
200 Kfusion[row] +=
P[row][col] * H_YAW[col];
203 Kfusion[row] *= heading_innov_var_inv;
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];
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;
261 if (
P[i][i] < KHP[i][i]) {
279 for (
unsigned column = 0; column <
_k_num_states; column++) {
280 P[row][column] =
P[row][column] - KHP[row][column];
307 float predicted_yaw = atan2f(ant_vec_ef(1),ant_vec_ef(0));
313 float yaw_delta =
wrap_pi(measured_yaw - predicted_yaw);
324 Eulerf euler_init(q_init);
327 euler_init(2) += yaw_delta;
328 euler_init(2) =
wrap_pi(euler_init(2));
331 quat_after_reset =
Quatf(euler_init);
343 euler312(0) += yaw_delta;
344 euler312(0) =
wrap_pi(euler312(0));
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));
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;
366 quat_after_reset =
Quatf(R_to_earth);
377 if (q_error(0) >= 0.0
f) {
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);
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
struct estimator::filter_control_status_u::@60 flags
Adapter / shim layer for system calls needed by ECL.
stateSample _state
state struct of the ekf running at the delayed time horizon
#define ECL_ERR_TIMESTAMPED
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
float _heading_innov_var
heading measurement innovation variance (rad**2)
void initialiseCovariance()
struct estimator::fault_status_u::@57 flags
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
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)
float _heading_innov
heading measurement innovation (rad)
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
void fuse(float *K, float innovation)
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool bad_hdg
3 - true if the fusion of the heading angle has encountered a numerical error
filter_control_status_u _control_status
static constexpr float sq(float var)
fault_status_u _fault_status
void increaseQuatYawErrVariance(float yaw_variance)
gpsSample _gps_sample_delayed
void fixCovarianceErrors()
bool reject_yaw
6 - true if the yaw observation has been rejected
#define MASK_USE_EVPOS
set to true to use external vision position data
Type wrap_pi(Type x)
Wrap value in range [-π, π)
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
innovation_fault_status_u _innov_check_fail_status
Vector3< float > Vector3f
constexpr _Tp max(_Tp a, _Tp b)
#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 ...
Quaternion< float > Quatf
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
struct Ekf::@62 _state_reset_status
reset event monitoring structure containing velocity, position, height and yaw reset information ...
uint32_t wind
8 - true when wind velocity is being estimated
void uncorrelateQuatStates()
struct estimator::innovation_fault_status_u::@58 flags
static constexpr uint8_t _k_num_states
number of EKF states
float heading_innov_gate
heading fusion innovation consistency gate size (STD)
Quatf quat_nominal
nominal quaternion describing vehicle attitude
Class for core functions for ekf attitude and position estimator.
uint8_t get_length() const