46 if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0
f) {
50 const float v_n = velI(0);
51 const float v_e = velI(1);
54 const float heading_est = atan2f(v_e, v_n);
65 float L3 = tas_meas / (L2 * sqrtf(L2));
66 float L4 = L3 * v_e * v_n + 1.0f;
67 float L5 = 1.0f / sqrtf(L2);
68 float L6 = -L5 * tas_meas;
73 L(0, 1) = L0 * L3 + L6;
74 L(1, 0) = L1 * L3 + L6;
121 float SPP0 = dt *
dt;
122 float SPP1 = SPP0 * q_w;
123 float SPP2 = SPP1 +
_P(0, 1);
127 P_next(0, 0) = SPP1 +
_P(0, 0);
129 P_next(0, 2) =
_P(0, 2);
131 P_next(1, 1) = SPP1 +
_P(1, 1);
132 P_next(1, 2) =
_P(1, 2);
133 P_next(2, 0) =
_P(0, 2);
134 P_next(2, 1) =
_P(1, 2);
135 P_next(2, 2) = SPP0 * q_k_tas +
_P(2, 2);
159 const float v_n = velI(0);
160 const float v_e = velI(1);
161 const float v_d = velI(2);
166 const float HH0 = sqrtf(v_d * v_d + (v_e -
w_e) * (v_e -
w_e) + (v_n -
w_n) * (v_n -
w_n));
167 const float HH1 = k_tas / HH0;
170 H_tas(0, 0) = HH1 * (-v_n +
w_n);
171 H_tas(0, 1) = HH1 * (-v_e +
w_e);
188 bool reinit_filter =
false;
189 bool meas_is_rejected =
false;
195 if (meas_is_rejected || reinit_filter) {
230 const float v_n = velI(0);
231 const float v_e = velI(1);
232 const float v_d = velI(2);
235 float HB0 = 2.0f * q_att(0);
236 float HB1 = HB0 * q_att(3);
237 float HB2 = 2.0f * q_att(1);
238 float HB3 = HB2 * q_att(2);
239 float HB4 = v_e -
w_e;
240 float HB5 = HB1 + HB3;
241 float HB6 = v_n -
w_n;
242 float HB7 = q_att(0) * q_att(0);
243 float HB8 = q_att(3) * q_att(3);
244 float HB9 = HB7 - HB8;
245 float HB10 = q_att(1) * q_att(1);
246 float HB11 = q_att(2) * q_att(2);
247 float HB12 = HB10 - HB11;
248 float HB13 = HB12 + HB9;
249 float HB14 = HB13 * HB6 + HB4 * HB5 + v_d * (-HB0 * q_att(2) + HB2 * q_att(3));
250 float HB15 = 1.0f / HB14;
251 float HB16 = (HB4 * (-HB10 + HB11 + HB9) + HB6 * (-HB1 + HB3) + v_d * (HB0 * q_att(1) + 2.0f * q_att(2) * q_att(3))) /
255 H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3);
256 H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
269 rel_wind = R_body_to_earth.
transpose() * rel_wind;
271 if (fabsf(rel_wind(0)) < 0.1
f) {
276 const float beta_pred = rel_wind(1) / rel_wind(0);
281 bool reinit_filter =
false;
282 bool meas_is_rejected =
false;
289 if (meas_is_rejected || reinit_filter) {
304 _P =
_P - K * H_beta *
_P;
312 for (
unsigned i = 0; i < 3; i++) {
313 if (
_P(i, i) < 0.0
f) {
321 _P(i, i) =
_P(i, i) > 25.0f ? 25.0f :
_P(i, i);
324 _P(i, i) =
_P(i, i) > 0.1f ? 0.1f :
_P(i, i);
343 for (
unsigned row = 0; row < 3; row++) {
344 for (
unsigned column = 0; column < row; column++) {
345 float tmp = (
_P(row, column) +
_P(column, row)) / 2;
346 _P(row, column) = tmp;
347 _P(column, row) = tmp;
354 uint64_t &time_meas_rejected,
bool &reinit_filter)
356 if (innov * innov > gate_size * gate_size * innov_var) {
357 time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
360 time_meas_rejected = 0;
363 reinit_filter = time_now - time_meas_rejected > 5 * 1000 * 1000 && time_meas_rejected != 0;
365 return time_meas_rejected != 0;
float _tas_innov
true airspeed innovation
uint64_t _time_last_update
timestamp of last covariance prediction
void update(uint64_t time_now)
bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size, uint64_t &time_meas_rejected, bool &reinit_filter)
uint64_t _time_last_airspeed_fuse
timestamp of last airspeed fusion
Matrix< Type, N, M > transpose() const
void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
A wind and airspeed scale estimator.
uint8_t _beta_gate
sideslip fusion gate size
float _enforced_airspeed_scale
by default we want to estimate the true airspeed scale factor (see enforce_airspeed_scale(...) )
float _tas_innov_var
true airspeed innovation variance
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool _initialised
True: filter has been initialised.
float _tas_scale_p_var
true airspeed scale process noise variance
uint64_t _time_rejected_tas
timestamp of when true airspeed measurements have consistently started to be rejected ...
float _beta_innov
sideslip innovation
float _tas_var
true airspeed measurement noise variance
float _wind_p_var
wind process noise variance
void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI, const matrix::Vector2f &velIvar)
float _beta_var
sideslip measurement noise variance
constexpr _Tp max(_Tp a, _Tp b)
uint64_t _time_rejected_beta
timestamp of when sideslip measurements have consistently started to be rejected
uint8_t _tas_gate
airspeed fusion gate size
bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas)
float _beta_innov_var
sideslip innovation variance
matrix::Matrix3f _P
state covariance matrix
matrix::Vector3f _state
state vector
uint64_t _time_last_beta_fuse
timestamp of last sideslip fusion
bool _wind_estimator_reset
wind estimator was reset in this cycle