PX4 Firmware
PX4 Autopilot Software http://px4.io
WindEstimator.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. 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 PX4 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 WindEstimator.cpp
36  * A wind and airspeed scale estimator.
37  */
38 
39 #include "WindEstimator.hpp"
40 
41 bool
42 WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas)
43 {
44  // do no initialise if ground velocity is low
45  // this should prevent the filter from initialising on the ground
46  if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) {
47  return false;
48  }
49 
50  const float v_n = velI(0);
51  const float v_e = velI(1);
52 
53  // estimate heading from ground velocity
54  const float heading_est = atan2f(v_e, v_n);
55 
56  // initilaise wind states assuming zero side slip and horizontal flight
57  _state(w_n) = velI(w_n) - tas_meas * cosf(heading_est);
58  _state(w_e) = velI(w_e) - tas_meas * sinf(heading_est);
59  _state(tas) = 1.0f;
60 
61  // compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
62  float L0 = v_e * v_e;
63  float L1 = v_n * v_n;
64  float L2 = L0 + L1;
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;
69 
71  L.setZero();
72  L(0, 0) = L4;
73  L(0, 1) = L0 * L3 + L6;
74  L(1, 0) = L1 * L3 + L6;
75  L(1, 1) = L4;
76  L(2, 2) = 1.0f;
77 
78  // get an estimate of the state covariance matrix given the estimated variance of ground velocity
79  // and measured airspeed
80  _P.setZero();
81  _P(w_n, w_n) = velIvar(0);
82  _P(w_e, w_e) = velIvar(1);
83  _P(tas, tas) = 0.0001f;
84 
85  _P = L * _P * L.transpose();
86 
87  // reset the timestamp for measurement rejection
90 
91  _wind_estimator_reset = true;
92 
93  return true;
94 }
95 
96 void
97 WindEstimator::update(uint64_t time_now)
98 {
99  if (!_initialised) {
100  return;
101  }
102 
103  // set reset state to false (is set to true when initialise fuction is called later)
104  _wind_estimator_reset = false;
105 
106  // run covariance prediction at 1Hz
107  if (time_now - _time_last_update < 1000 * 1000 || _time_last_update == 0) {
108  if (_time_last_update == 0) {
109  _time_last_update = time_now;
110  }
111 
112  return;
113  }
114 
115  float dt = (float)(time_now - _time_last_update) * 1e-6f;
116  _time_last_update = time_now;
117 
118  float q_w = _wind_p_var;
119  float q_k_tas = _tas_scale_p_var;
120 
121  float SPP0 = dt * dt;
122  float SPP1 = SPP0 * q_w;
123  float SPP2 = SPP1 + _P(0, 1);
124 
125  matrix::Matrix3f P_next;
126 
127  P_next(0, 0) = SPP1 + _P(0, 0);
128  P_next(0, 1) = SPP2;
129  P_next(0, 2) = _P(0, 2);
130  P_next(1, 0) = SPP2;
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);
136  _P = P_next;
137 }
138 
139 void
140 WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
141  const matrix::Vector2f &velIvar)
142 {
143  matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };
144 
145  if (!_initialised) {
146  // try to initialise
147  _initialised = initialise(velI, velIvar_constrained, true_airspeed);
148  return;
149  }
150 
151  // don't fuse faster than 10Hz
152  if (time_now - _time_last_airspeed_fuse < 100 * 1000) {
153  return;
154  }
155 
156  _time_last_airspeed_fuse = time_now;
157 
158  // assign helper variables
159  const float v_n = velI(0);
160  const float v_e = velI(1);
161  const float v_d = velI(2);
162 
163  const float k_tas = _state(tas);
164 
165  // compute kalman gain K
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;
168 
170  H_tas(0, 0) = HH1 * (-v_n + w_n);
171  H_tas(0, 1) = HH1 * (-v_e + w_e);
172  H_tas(0, 2) = HH0;
173 
175 
176  const matrix::Matrix<float, 1, 1> S = H_tas * _P * H_tas.transpose() + _tas_var;
177 
178  K /= S(0,0);
179  // compute innovation
180  const float airspeed_pred = _state(tas) * sqrtf((v_n - _state(w_n)) * (v_n - _state(w_n)) + (v_e - _state(w_e)) *
181  (v_e - _state(w_e)) + v_d * v_d);
182 
183  _tas_innov = true_airspeed - airspeed_pred;
184 
185  // innovation variance
186  _tas_innov_var = S(0,0);
187 
188  bool reinit_filter = false;
189  bool meas_is_rejected = false;
190 
191  meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas, reinit_filter);
192 
193  reinit_filter |= _tas_innov_var < 0.0f;
194 
195  if (meas_is_rejected || reinit_filter) {
196  if (reinit_filter) {
197  _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed);
198  }
199 
200  // we either did a filter reset or the current measurement was rejected so do not fuse
201  return;
202  }
203 
204  // apply correction to state
205  _state(w_n) += _tas_innov * K(0, 0);
206  _state(w_e) += _tas_innov * K(1, 0);
207  _state(tas) += _tas_innov * K(2, 0);
208 
209  // update covariance matrix
210  _P = _P - K * H_tas * _P;
211 
213 }
214 
215 void
216 WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
217 {
218  if (!_initialised) {
219  _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
220  return;
221  }
222 
223  // don't fuse faster than 10Hz
224  if (time_now - _time_last_beta_fuse < 100 * 1000) {
225  return;
226  }
227 
228  _time_last_beta_fuse = time_now;
229 
230  const float v_n = velI(0);
231  const float v_e = velI(1);
232  const float v_d = velI(2);
233 
234  // compute sideslip observation vector
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))) /
252  (HB14 * HB14);
253 
255  H_beta(0, 0) = HB13 * HB16 + HB15 * (HB1 - HB3);
256  H_beta(0, 1) = HB15 * (HB12 - HB7 + HB8) + HB16 * HB5;
257  H_beta(0, 2) = 0;
258 
259  // compute kalman gain
261 
262  const matrix::Matrix<float, 1, 1> S = H_beta * _P * H_beta.transpose() + _beta_var;
263 
264  K /= S(0,0);
265 
266  // compute predicted side slip angle
267  matrix::Vector3f rel_wind(velI(0) - _state(w_n), velI(1) - _state(w_e), velI(2));
268  matrix::Dcmf R_body_to_earth(q_att);
269  rel_wind = R_body_to_earth.transpose() * rel_wind;
270 
271  if (fabsf(rel_wind(0)) < 0.1f) {
272  return;
273  }
274 
275  // use small angle approximation, sin(x) = x for small x
276  const float beta_pred = rel_wind(1) / rel_wind(0);
277 
278  _beta_innov = 0.0f - beta_pred;
279  _beta_innov_var = S(0,0);
280 
281  bool reinit_filter = false;
282  bool meas_is_rejected = false;
283 
285  reinit_filter);
286 
287  reinit_filter |= _beta_innov_var < 0.0f;
288 
289  if (meas_is_rejected || reinit_filter) {
290  if (reinit_filter) {
291  _initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length());
292  }
293 
294  // we either did a filter reset or the current measurement was rejected so do not fuse
295  return;
296  }
297 
298  // apply correction to state
299  _state(w_n) += _beta_innov * K(0, 0);
300  _state(w_e) += _beta_innov * K(1, 0);
301  _state(tas) += _beta_innov * K(2, 0);
302 
303  // update covariance matrix
304  _P = _P - K * H_beta * _P;
305 
307 }
308 
309 void
311 {
312  for (unsigned i = 0; i < 3; i++) {
313  if (_P(i, i) < 0.0f) {
314  // ill-conditioned covariance matrix, reset filter
315  _initialised = false;
316  return;
317  }
318 
319  // limit covariance diagonals if they grow too large
320  if (i < 2) {
321  _P(i, i) = _P(i, i) > 25.0f ? 25.0f : _P(i, i);
322 
323  } else if (i == 2) {
324  _P(i, i) = _P(i, i) > 0.1f ? 0.1f : _P(i, i);
325  }
326  }
327 
328  if (!ISFINITE(_state(w_n)) || !ISFINITE(_state(w_e)) || !ISFINITE(_state(tas))) {
329  _initialised = false;
330  return;
331  }
332 
333  // check if we should inhibit learning of airspeed scale factor and rather use a pre-set value.
334  // airspeed scale factor errors arise from sensor installation which does not change and only needs
335  // to be computed once for a perticular installation.
336  if (_enforced_airspeed_scale < 0) {
337  _state(tas) = math::max(0.0f, _state(tas));
338  } else {
340  }
341 
342  // attain symmetry
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;
348  }
349  }
350 }
351 
352 bool
353 WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
354  uint64_t &time_meas_rejected, bool &reinit_filter)
355 {
356  if (innov * innov > gate_size * gate_size * innov_var) {
357  time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
358 
359  } else {
360  time_meas_rejected = 0;
361  }
362 
363  reinit_filter = time_now - time_meas_rejected > 5 * 1000 * 1000 && time_meas_rejected != 0;
364 
365  return time_meas_rejected != 0;
366 }
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
Quaternion class.
Definition: Dcm.hpp:24
Matrix< Type, N, M > transpose() const
Definition: Matrix.hpp:353
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 > &)
Definition: integration.cpp:8
Type length() const
Definition: Vector.hpp:83
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)
Definition: Limits.hpp:60
float dt
Definition: px4io.c:73
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)
void setZero()
Definition: Matrix.hpp:416
float _beta_innov_var
sideslip innovation variance
matrix::Matrix3f _P
state covariance matrix
matrix::Vector3f _state
state vector
void run_sanity_checks()
uint64_t _time_last_beta_fuse
timestamp of last sideslip fusion
#define ISFINITE(x)
Definition: ecl.h:100
bool _wind_estimator_reset
wind estimator was reset in this cycle