APM:Libraries
AP_NavEKF3_AirDataFusion.cpp
Go to the documentation of this file.
1 #include <AP_HAL/AP_HAL.h>
2 
3 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
4 
5 #include "AP_NavEKF3.h"
6 #include "AP_NavEKF3_core.h"
7 #include <AP_AHRS/AP_AHRS.h>
8 #include <AP_Vehicle/AP_Vehicle.h>
9 
10 extern const AP_HAL::HAL& hal;
11 
12 /********************************************************
13 * RESET FUNCTIONS *
14 ********************************************************/
15 
16 /********************************************************
17 * FUSE MEASURED_DATA *
18 ********************************************************/
19 
20 /*
21  * Fuse true airspeed measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
22  * The script file used to generate these and other equations in this filter can be found here:
23  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
24 */
25 void NavEKF3_core::FuseAirspeed()
26 {
27  // start performance timer
28  hal.util->perf_begin(_perf_FuseAirspeed);
29 
30  // declarations
31  float vn;
32  float ve;
33  float vd;
34  float vwn;
35  float vwe;
36  float EAS2TAS = _ahrs->get_EAS2TAS();
37  const float R_TAS = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(EAS2TAS, 0.9f, 10.0f));
38  float SH_TAS[3];
39  float SK_TAS[2];
40  Vector24 H_TAS = {};
41  float VtasPred;
42 
43  // health is set bad until test passed
44  tasHealth = false;
45 
46  // copy required states to local variable names
47  vn = stateStruct.velocity.x;
48  ve = stateStruct.velocity.y;
49  vd = stateStruct.velocity.z;
50  vwn = stateStruct.wind_vel.x;
51  vwe = stateStruct.wind_vel.y;
52 
53  // calculate the predicted airspeed
54  VtasPred = norm((ve - vwe) , (vn - vwn) , vd);
55  // perform fusion of True Airspeed measurement
56  if (VtasPred > 1.0f)
57  {
58  // calculate observation jacobians
59  SH_TAS[0] = 1.0f/VtasPred;
60  SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
61  SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
62  H_TAS[4] = SH_TAS[2];
63  H_TAS[5] = SH_TAS[1];
64  H_TAS[6] = vd*SH_TAS[0];
65  H_TAS[22] = -SH_TAS[2];
66  H_TAS[23] = -SH_TAS[1];
67  // calculate Kalman gains
68  float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[22][4]*SH_TAS[2] - P[23][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[22][5]*SH_TAS[2] - P[23][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][22]*SH_TAS[2] + P[5][22]*SH_TAS[1] - P[22][22]*SH_TAS[2] - P[23][22]*SH_TAS[1] + P[6][22]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][23]*SH_TAS[2] + P[5][23]*SH_TAS[1] - P[22][23]*SH_TAS[2] - P[23][23]*SH_TAS[1] + P[6][23]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[22][6]*SH_TAS[2] - P[23][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
69  if (temp >= R_TAS) {
70  SK_TAS[0] = 1.0f / temp;
71  faultStatus.bad_airspeed = false;
72  } else {
73  // the calculation is badly conditioned, so we cannot perform fusion on this step
74  // we reset the covariance matrix and try again next measurement
75  CovarianceInit();
76  faultStatus.bad_airspeed = true;
77  return;
78  }
79  SK_TAS[1] = SH_TAS[1];
80  Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
81  Kfusion[1] = SK_TAS[0]*(P[1][4]*SH_TAS[2] - P[1][22]*SH_TAS[2] + P[1][5]*SK_TAS[1] - P[1][23]*SK_TAS[1] + P[1][6]*vd*SH_TAS[0]);
82  Kfusion[2] = SK_TAS[0]*(P[2][4]*SH_TAS[2] - P[2][22]*SH_TAS[2] + P[2][5]*SK_TAS[1] - P[2][23]*SK_TAS[1] + P[2][6]*vd*SH_TAS[0]);
83  Kfusion[3] = SK_TAS[0]*(P[3][4]*SH_TAS[2] - P[3][22]*SH_TAS[2] + P[3][5]*SK_TAS[1] - P[3][23]*SK_TAS[1] + P[3][6]*vd*SH_TAS[0]);
84  Kfusion[4] = SK_TAS[0]*(P[4][4]*SH_TAS[2] - P[4][22]*SH_TAS[2] + P[4][5]*SK_TAS[1] - P[4][23]*SK_TAS[1] + P[4][6]*vd*SH_TAS[0]);
85  Kfusion[5] = SK_TAS[0]*(P[5][4]*SH_TAS[2] - P[5][22]*SH_TAS[2] + P[5][5]*SK_TAS[1] - P[5][23]*SK_TAS[1] + P[5][6]*vd*SH_TAS[0]);
86  Kfusion[6] = SK_TAS[0]*(P[6][4]*SH_TAS[2] - P[6][22]*SH_TAS[2] + P[6][5]*SK_TAS[1] - P[6][23]*SK_TAS[1] + P[6][6]*vd*SH_TAS[0]);
87  Kfusion[7] = SK_TAS[0]*(P[7][4]*SH_TAS[2] - P[7][22]*SH_TAS[2] + P[7][5]*SK_TAS[1] - P[7][23]*SK_TAS[1] + P[7][6]*vd*SH_TAS[0]);
88  Kfusion[8] = SK_TAS[0]*(P[8][4]*SH_TAS[2] - P[8][22]*SH_TAS[2] + P[8][5]*SK_TAS[1] - P[8][23]*SK_TAS[1] + P[8][6]*vd*SH_TAS[0]);
89  Kfusion[9] = SK_TAS[0]*(P[9][4]*SH_TAS[2] - P[9][22]*SH_TAS[2] + P[9][5]*SK_TAS[1] - P[9][23]*SK_TAS[1] + P[9][6]*vd*SH_TAS[0]);
90 
91  if (!inhibitDelAngBiasStates) {
92  Kfusion[10] = SK_TAS[0]*(P[10][4]*SH_TAS[2] - P[10][22]*SH_TAS[2] + P[10][5]*SK_TAS[1] - P[10][23]*SK_TAS[1] + P[10][6]*vd*SH_TAS[0]);
93  Kfusion[11] = SK_TAS[0]*(P[11][4]*SH_TAS[2] - P[11][22]*SH_TAS[2] + P[11][5]*SK_TAS[1] - P[11][23]*SK_TAS[1] + P[11][6]*vd*SH_TAS[0]);
94  Kfusion[12] = SK_TAS[0]*(P[12][4]*SH_TAS[2] - P[12][22]*SH_TAS[2] + P[12][5]*SK_TAS[1] - P[12][23]*SK_TAS[1] + P[12][6]*vd*SH_TAS[0]);
95  } else {
96  // zero indexes 10 to 12 = 3*4 bytes
97  memset(&Kfusion[10], 0, 12);
98  }
99 
100  if (!inhibitDelVelBiasStates) {
101  Kfusion[13] = SK_TAS[0]*(P[13][4]*SH_TAS[2] - P[13][22]*SH_TAS[2] + P[13][5]*SK_TAS[1] - P[13][23]*SK_TAS[1] + P[13][6]*vd*SH_TAS[0]);
102  Kfusion[14] = SK_TAS[0]*(P[14][4]*SH_TAS[2] - P[14][22]*SH_TAS[2] + P[14][5]*SK_TAS[1] - P[14][23]*SK_TAS[1] + P[14][6]*vd*SH_TAS[0]);
103  Kfusion[15] = SK_TAS[0]*(P[15][4]*SH_TAS[2] - P[15][22]*SH_TAS[2] + P[15][5]*SK_TAS[1] - P[15][23]*SK_TAS[1] + P[15][6]*vd*SH_TAS[0]);
104  } else {
105  // zero indexes 13 to 15 = 3*4 bytes
106  memset(&Kfusion[13], 0, 12);
107  }
108 
109  // zero Kalman gains to inhibit magnetic field state estimation
110  if (!inhibitMagStates) {
111  Kfusion[16] = SK_TAS[0]*(P[16][4]*SH_TAS[2] - P[16][22]*SH_TAS[2] + P[16][5]*SK_TAS[1] - P[16][23]*SK_TAS[1] + P[16][6]*vd*SH_TAS[0]);
112  Kfusion[17] = SK_TAS[0]*(P[17][4]*SH_TAS[2] - P[17][22]*SH_TAS[2] + P[17][5]*SK_TAS[1] - P[17][23]*SK_TAS[1] + P[17][6]*vd*SH_TAS[0]);
113  Kfusion[18] = SK_TAS[0]*(P[18][4]*SH_TAS[2] - P[18][22]*SH_TAS[2] + P[18][5]*SK_TAS[1] - P[18][23]*SK_TAS[1] + P[18][6]*vd*SH_TAS[0]);
114  Kfusion[19] = SK_TAS[0]*(P[19][4]*SH_TAS[2] - P[19][22]*SH_TAS[2] + P[19][5]*SK_TAS[1] - P[19][23]*SK_TAS[1] + P[19][6]*vd*SH_TAS[0]);
115  Kfusion[20] = SK_TAS[0]*(P[20][4]*SH_TAS[2] - P[20][22]*SH_TAS[2] + P[20][5]*SK_TAS[1] - P[20][23]*SK_TAS[1] + P[20][6]*vd*SH_TAS[0]);
116  Kfusion[21] = SK_TAS[0]*(P[21][4]*SH_TAS[2] - P[21][22]*SH_TAS[2] + P[21][5]*SK_TAS[1] - P[21][23]*SK_TAS[1] + P[21][6]*vd*SH_TAS[0]);
117  } else {
118  // zero indexes 16 to 21 = 6*4 bytes
119  memset(&Kfusion[16], 0, 24);
120  }
121 
122  if (!inhibitWindStates) {
123  Kfusion[22] = SK_TAS[0]*(P[22][4]*SH_TAS[2] - P[22][22]*SH_TAS[2] + P[22][5]*SK_TAS[1] - P[22][23]*SK_TAS[1] + P[22][6]*vd*SH_TAS[0]);
124  Kfusion[23] = SK_TAS[0]*(P[23][4]*SH_TAS[2] - P[23][22]*SH_TAS[2] + P[23][5]*SK_TAS[1] - P[23][23]*SK_TAS[1] + P[23][6]*vd*SH_TAS[0]);
125  } else {
126  // zero indexes 22 to 23 = 2*4 bytes
127  memset(&Kfusion[22], 0, 8);
128  }
129 
130  // calculate measurement innovation variance
131  varInnovVtas = 1.0f/SK_TAS[0];
132 
133  // calculate measurement innovation
134  innovVtas = VtasPred - tasDataDelayed.tas;
135 
136  // calculate the innovation consistency test ratio
137  tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas);
138 
139  // fail if the ratio is > 1, but don't fail if bad IMU data
140  tasHealth = ((tasTestRatio < 1.0f) || badIMUdata);
141  tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms;
142 
143  // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth
144  if (tasHealth || (tasTimeout && posTimeout)) {
145 
146  // restart the counter
147  lastTasPassTime_ms = imuSampleTime_ms;
148 
149  // correct the state vector
150  for (uint8_t j= 0; j<=stateIndexLim; j++) {
151  statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas;
152  }
153  stateStruct.quat.normalize();
154 
155  // correct the covariance P = (I - K*H)*P
156  // take advantage of the empty columns in KH to reduce the
157  // number of operations
158  for (unsigned i = 0; i<=stateIndexLim; i++) {
159  for (unsigned j = 0; j<=3; j++) {
160  KH[i][j] = 0.0f;
161  }
162  for (unsigned j = 4; j<=6; j++) {
163  KH[i][j] = Kfusion[i] * H_TAS[j];
164  }
165  for (unsigned j = 7; j<=21; j++) {
166  KH[i][j] = 0.0f;
167  }
168  for (unsigned j = 22; j<=23; j++) {
169  KH[i][j] = Kfusion[i] * H_TAS[j];
170  }
171  }
172  for (unsigned j = 0; j<=stateIndexLim; j++) {
173  for (unsigned i = 0; i<=stateIndexLim; i++) {
174  ftype res = 0;
175  res += KH[i][4] * P[4][j];
176  res += KH[i][5] * P[5][j];
177  res += KH[i][6] * P[6][j];
178  res += KH[i][22] * P[22][j];
179  res += KH[i][23] * P[23][j];
180  KHP[i][j] = res;
181  }
182  }
183  for (unsigned i = 0; i<=stateIndexLim; i++) {
184  for (unsigned j = 0; j<=stateIndexLim; j++) {
185  P[i][j] = P[i][j] - KHP[i][j];
186  }
187  }
188  }
189  }
190 
191  // force the covariance matrix to me symmetrical and limit the variances to prevent ill-condiioning.
192  ForceSymmetry();
193  ConstrainVariances();
194 
195  // stop performance timer
196  hal.util->perf_end(_perf_FuseAirspeed);
197 }
198 
199 // select fusion of true airspeed measurements
200 void NavEKF3_core::SelectTasFusion()
201 {
202  // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
203  // If so, don't fuse measurements on this time step to reduce frame over-runs
204  // Only allow one time slip to prevent high rate magnetometer data locking out fusion of other measurements
205  if (magFusePerformed && dtIMUavg < 0.005f && !airSpdFusionDelayed) {
206  airSpdFusionDelayed = true;
207  return;
208  } else {
209  airSpdFusionDelayed = false;
210  }
211 
212  // get true airspeed measurement
213  readAirSpdData();
214 
215  // If we haven't received airspeed data for a while, then declare the airspeed data as being timed out
216  if (imuSampleTime_ms - tasDataNew.time_ms > frontend->tasRetryTime_ms) {
217  tasTimeout = true;
218  }
219 
220  // if the filter is initialised, wind states are not inhibited and we have data to fuse, then perform TAS fusion
221  if (tasDataToFuse && statesInitialised && !inhibitWindStates) {
222  FuseAirspeed();
223  prevTasStep_ms = imuSampleTime_ms;
224  }
225 }
226 
227 
228 // select fusion of synthetic sideslip measurements
229 // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
230 // it requires a stable wind for best results and should not be used for aerobatic flight with manoeuvres that induce large sidslip angles (eg knife-edge, spins, etc)
231 void NavEKF3_core::SelectBetaFusion()
232 {
233  // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
234  // If so, don't fuse measurements on this time step to reduce frame over-runs
235  // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
236  if (magFusePerformed && dtIMUavg < 0.005f && !sideSlipFusionDelayed) {
237  sideSlipFusionDelayed = true;
238  return;
239  } else {
240  sideSlipFusionDelayed = false;
241  }
242 
243  // set true when the fusion time interval has triggered
244  bool f_timeTrigger = ((imuSampleTime_ms - prevBetaStep_ms) >= frontend->betaAvg_ms);
245  // set true when use of synthetic sideslip fusion is necessary because we have limited sensor data or are dead reckoning position
246  bool f_required = !(use_compass() && useAirspeed() && ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->posRetryTimeNoVel_ms));
247  // set true when sideslip fusion is feasible (requires zero sideslip assumption to be valid and use of wind states)
248  bool f_feasible = (assume_zero_sideslip() && !inhibitWindStates);
249  // use synthetic sideslip fusion if feasible, required and enough time has lapsed since the last fusion
250  if (f_feasible && f_required && f_timeTrigger) {
251  FuseSideslip();
252  prevBetaStep_ms = imuSampleTime_ms;
253  }
254 }
255 
256 /*
257  * Fuse sythetic sideslip measurement of zero using explicit algebraic equations generated with Matlab symbolic toolbox.
258  * The script file used to generate these and other equations in this filter can be found here:
259  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
260 */
261 void NavEKF3_core::FuseSideslip()
262 {
263  // start performance timer
264  hal.util->perf_begin(_perf_FuseSideslip);
265 
266  // declarations
267  float q0;
268  float q1;
269  float q2;
270  float q3;
271  float vn;
272  float ve;
273  float vd;
274  float vwn;
275  float vwe;
276  const float R_BETA = 0.03f; // assume a sideslip angle RMS of ~10 deg
277  Vector13 SH_BETA;
278  Vector8 SK_BETA;
279  Vector3f vel_rel_wind;
280  Vector24 H_BETA;
281  float innovBeta;
282 
283  // copy required states to local variable names
284  q0 = stateStruct.quat[0];
285  q1 = stateStruct.quat[1];
286  q2 = stateStruct.quat[2];
287  q3 = stateStruct.quat[3];
288  vn = stateStruct.velocity.x;
289  ve = stateStruct.velocity.y;
290  vd = stateStruct.velocity.z;
291  vwn = stateStruct.wind_vel.x;
292  vwe = stateStruct.wind_vel.y;
293 
294  // calculate predicted wind relative velocity in NED
295  vel_rel_wind.x = vn - vwn;
296  vel_rel_wind.y = ve - vwe;
297  vel_rel_wind.z = vd;
298 
299  // rotate into body axes
300  vel_rel_wind = prevTnb * vel_rel_wind;
301 
302  // perform fusion of assumed sideslip = 0
303  if (vel_rel_wind.x > 5.0f)
304  {
305  // Calculate observation jacobians
306  SH_BETA[0] = (vn - vwn)*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + (ve - vwe)*(2*q0*q3 + 2*q1*q2);
307  if (fabsf(SH_BETA[0]) <= 1e-9f) {
308  faultStatus.bad_sideslip = true;
309  return;
310  } else {
311  faultStatus.bad_sideslip = false;
312  }
313  SH_BETA[1] = (ve - vwe)*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - (vn - vwn)*(2*q0*q3 - 2*q1*q2);
314  SH_BETA[2] = vn - vwn;
315  SH_BETA[3] = ve - vwe;
316  SH_BETA[4] = 1/sq(SH_BETA[0]);
317  SH_BETA[5] = 1/SH_BETA[0];
318  SH_BETA[6] = SH_BETA[5]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
319  SH_BETA[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
320  SH_BETA[8] = 2*q0*SH_BETA[3] - 2*q3*SH_BETA[2] + 2*q1*vd;
321  SH_BETA[9] = 2*q0*SH_BETA[2] + 2*q3*SH_BETA[3] - 2*q2*vd;
322  SH_BETA[10] = 2*q2*SH_BETA[2] - 2*q1*SH_BETA[3] + 2*q0*vd;
323  SH_BETA[11] = 2*q1*SH_BETA[2] + 2*q2*SH_BETA[3] + 2*q3*vd;
324  SH_BETA[12] = 2*q0*q3;
325 
326  H_BETA[0] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
327  H_BETA[1] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
328  H_BETA[2] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
329  H_BETA[3] = - SH_BETA[5]*SH_BETA[9] - SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
330  H_BETA[4] = - SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) - SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
331  H_BETA[5] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
332  H_BETA[6] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
333  for (uint8_t i=7; i<=21; i++) {
334  H_BETA[i] = 0.0f;
335  }
336  H_BETA[22] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
337  H_BETA[23] = SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2) - SH_BETA[6];
338 
339  // Calculate Kalman gains
340  float temp = (R_BETA - (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][4]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][4]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][4]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][4]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][4]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][4]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][4]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7])*(P[22][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][22]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][22]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][22]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][22]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][22]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][22]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][22]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][5]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][5]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][5]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][5]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][5]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][5]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][5]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2))*(P[22][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][23]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][23]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][23]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][23]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][23]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][23]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][23]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9])*(P[22][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][0]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][0]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][0]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][0]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][0]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][0]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][0]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11])*(P[22][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][1]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][1]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][1]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][1]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][1]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][1]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][1]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10])*(P[22][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][2]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][2]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][2]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][2]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][2]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][2]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][2]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) - (SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8])*(P[22][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][3]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][3]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][3]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][3]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][3]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][3]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][3]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))) + (SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))*(P[22][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) - P[4][6]*(SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7]) + P[5][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) - P[23][6]*(SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2)) + P[0][6]*(SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9]) + P[1][6]*(SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11]) + P[2][6]*(SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10]) - P[3][6]*(SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8]) + P[6][6]*(SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3))));
341  if (temp >= R_BETA) {
342  SK_BETA[0] = 1.0f / temp;
343  faultStatus.bad_sideslip = false;
344  } else {
345  // the calculation is badly conditioned, so we cannot perform fusion on this step
346  // we reset the covariance matrix and try again next measurement
347  CovarianceInit();
348  faultStatus.bad_sideslip = true;
349  return;
350  }
351  SK_BETA[1] = SH_BETA[5]*(SH_BETA[12] - 2*q1*q2) + SH_BETA[1]*SH_BETA[4]*SH_BETA[7];
352  SK_BETA[2] = SH_BETA[6] - SH_BETA[1]*SH_BETA[4]*(SH_BETA[12] + 2*q1*q2);
353  SK_BETA[3] = SH_BETA[5]*(2*q0*q1 + 2*q2*q3) + SH_BETA[1]*SH_BETA[4]*(2*q0*q2 - 2*q1*q3);
354  SK_BETA[4] = SH_BETA[5]*SH_BETA[10] - SH_BETA[1]*SH_BETA[4]*SH_BETA[11];
355  SK_BETA[5] = SH_BETA[5]*SH_BETA[8] - SH_BETA[1]*SH_BETA[4]*SH_BETA[9];
356  SK_BETA[6] = SH_BETA[5]*SH_BETA[11] + SH_BETA[1]*SH_BETA[4]*SH_BETA[10];
357  SK_BETA[7] = SH_BETA[5]*SH_BETA[9] + SH_BETA[1]*SH_BETA[4]*SH_BETA[8];
358 
359  Kfusion[0] = SK_BETA[0]*(P[0][0]*SK_BETA[5] + P[0][1]*SK_BETA[4] - P[0][4]*SK_BETA[1] + P[0][5]*SK_BETA[2] + P[0][2]*SK_BETA[6] + P[0][6]*SK_BETA[3] - P[0][3]*SK_BETA[7] + P[0][22]*SK_BETA[1] - P[0][23]*SK_BETA[2]);
360  Kfusion[1] = SK_BETA[0]*(P[1][0]*SK_BETA[5] + P[1][1]*SK_BETA[4] - P[1][4]*SK_BETA[1] + P[1][5]*SK_BETA[2] + P[1][2]*SK_BETA[6] + P[1][6]*SK_BETA[3] - P[1][3]*SK_BETA[7] + P[1][22]*SK_BETA[1] - P[1][23]*SK_BETA[2]);
361  Kfusion[2] = SK_BETA[0]*(P[2][0]*SK_BETA[5] + P[2][1]*SK_BETA[4] - P[2][4]*SK_BETA[1] + P[2][5]*SK_BETA[2] + P[2][2]*SK_BETA[6] + P[2][6]*SK_BETA[3] - P[2][3]*SK_BETA[7] + P[2][22]*SK_BETA[1] - P[2][23]*SK_BETA[2]);
362  Kfusion[3] = SK_BETA[0]*(P[3][0]*SK_BETA[5] + P[3][1]*SK_BETA[4] - P[3][4]*SK_BETA[1] + P[3][5]*SK_BETA[2] + P[3][2]*SK_BETA[6] + P[3][6]*SK_BETA[3] - P[3][3]*SK_BETA[7] + P[3][22]*SK_BETA[1] - P[3][23]*SK_BETA[2]);
363  Kfusion[4] = SK_BETA[0]*(P[4][0]*SK_BETA[5] + P[4][1]*SK_BETA[4] - P[4][4]*SK_BETA[1] + P[4][5]*SK_BETA[2] + P[4][2]*SK_BETA[6] + P[4][6]*SK_BETA[3] - P[4][3]*SK_BETA[7] + P[4][22]*SK_BETA[1] - P[4][23]*SK_BETA[2]);
364  Kfusion[5] = SK_BETA[0]*(P[5][0]*SK_BETA[5] + P[5][1]*SK_BETA[4] - P[5][4]*SK_BETA[1] + P[5][5]*SK_BETA[2] + P[5][2]*SK_BETA[6] + P[5][6]*SK_BETA[3] - P[5][3]*SK_BETA[7] + P[5][22]*SK_BETA[1] - P[5][23]*SK_BETA[2]);
365  Kfusion[6] = SK_BETA[0]*(P[6][0]*SK_BETA[5] + P[6][1]*SK_BETA[4] - P[6][4]*SK_BETA[1] + P[6][5]*SK_BETA[2] + P[6][2]*SK_BETA[6] + P[6][6]*SK_BETA[3] - P[6][3]*SK_BETA[7] + P[6][22]*SK_BETA[1] - P[6][23]*SK_BETA[2]);
366  Kfusion[7] = SK_BETA[0]*(P[7][0]*SK_BETA[5] + P[7][1]*SK_BETA[4] - P[7][4]*SK_BETA[1] + P[7][5]*SK_BETA[2] + P[7][2]*SK_BETA[6] + P[7][6]*SK_BETA[3] - P[7][3]*SK_BETA[7] + P[7][22]*SK_BETA[1] - P[7][23]*SK_BETA[2]);
367  Kfusion[8] = SK_BETA[0]*(P[8][0]*SK_BETA[5] + P[8][1]*SK_BETA[4] - P[8][4]*SK_BETA[1] + P[8][5]*SK_BETA[2] + P[8][2]*SK_BETA[6] + P[8][6]*SK_BETA[3] - P[8][3]*SK_BETA[7] + P[8][22]*SK_BETA[1] - P[8][23]*SK_BETA[2]);
368  Kfusion[9] = SK_BETA[0]*(P[9][0]*SK_BETA[5] + P[9][1]*SK_BETA[4] - P[9][4]*SK_BETA[1] + P[9][5]*SK_BETA[2] + P[9][2]*SK_BETA[6] + P[9][6]*SK_BETA[3] - P[9][3]*SK_BETA[7] + P[9][22]*SK_BETA[1] - P[9][23]*SK_BETA[2]);
369 
370  if (!inhibitDelAngBiasStates) {
371  Kfusion[10] = SK_BETA[0]*(P[10][0]*SK_BETA[5] + P[10][1]*SK_BETA[4] - P[10][4]*SK_BETA[1] + P[10][5]*SK_BETA[2] + P[10][2]*SK_BETA[6] + P[10][6]*SK_BETA[3] - P[10][3]*SK_BETA[7] + P[10][22]*SK_BETA[1] - P[10][23]*SK_BETA[2]);
372  Kfusion[11] = SK_BETA[0]*(P[11][0]*SK_BETA[5] + P[11][1]*SK_BETA[4] - P[11][4]*SK_BETA[1] + P[11][5]*SK_BETA[2] + P[11][2]*SK_BETA[6] + P[11][6]*SK_BETA[3] - P[11][3]*SK_BETA[7] + P[11][22]*SK_BETA[1] - P[11][23]*SK_BETA[2]);
373  Kfusion[12] = SK_BETA[0]*(P[12][0]*SK_BETA[5] + P[12][1]*SK_BETA[4] - P[12][4]*SK_BETA[1] + P[12][5]*SK_BETA[2] + P[12][2]*SK_BETA[6] + P[12][6]*SK_BETA[3] - P[12][3]*SK_BETA[7] + P[12][22]*SK_BETA[1] - P[12][23]*SK_BETA[2]);
374  } else {
375  // zero indexes 10 to 12 = 3*4 bytes
376  memset(&Kfusion[10], 0, 12);
377  }
378 
379  if (!inhibitDelVelBiasStates) {
380  Kfusion[13] = SK_BETA[0]*(P[13][0]*SK_BETA[5] + P[13][1]*SK_BETA[4] - P[13][4]*SK_BETA[1] + P[13][5]*SK_BETA[2] + P[13][2]*SK_BETA[6] + P[13][6]*SK_BETA[3] - P[13][3]*SK_BETA[7] + P[13][22]*SK_BETA[1] - P[13][23]*SK_BETA[2]);
381  Kfusion[14] = SK_BETA[0]*(P[14][0]*SK_BETA[5] + P[14][1]*SK_BETA[4] - P[14][4]*SK_BETA[1] + P[14][5]*SK_BETA[2] + P[14][2]*SK_BETA[6] + P[14][6]*SK_BETA[3] - P[14][3]*SK_BETA[7] + P[14][22]*SK_BETA[1] - P[14][23]*SK_BETA[2]);
382  Kfusion[15] = SK_BETA[0]*(P[15][0]*SK_BETA[5] + P[15][1]*SK_BETA[4] - P[15][4]*SK_BETA[1] + P[15][5]*SK_BETA[2] + P[15][2]*SK_BETA[6] + P[15][6]*SK_BETA[3] - P[15][3]*SK_BETA[7] + P[15][22]*SK_BETA[1] - P[15][23]*SK_BETA[2]);
383  } else {
384  // zero indexes 13 to 15 = 3*4 bytes
385  memset(&Kfusion[13], 0, 12);
386  }
387 
388  // zero Kalman gains to inhibit magnetic field state estimation
389  if (!inhibitMagStates) {
390  Kfusion[16] = SK_BETA[0]*(P[16][0]*SK_BETA[5] + P[16][1]*SK_BETA[4] - P[16][4]*SK_BETA[1] + P[16][5]*SK_BETA[2] + P[16][2]*SK_BETA[6] + P[16][6]*SK_BETA[3] - P[16][3]*SK_BETA[7] + P[16][22]*SK_BETA[1] - P[16][23]*SK_BETA[2]);
391  Kfusion[17] = SK_BETA[0]*(P[17][0]*SK_BETA[5] + P[17][1]*SK_BETA[4] - P[17][4]*SK_BETA[1] + P[17][5]*SK_BETA[2] + P[17][2]*SK_BETA[6] + P[17][6]*SK_BETA[3] - P[17][3]*SK_BETA[7] + P[17][22]*SK_BETA[1] - P[17][23]*SK_BETA[2]);
392  Kfusion[18] = SK_BETA[0]*(P[18][0]*SK_BETA[5] + P[18][1]*SK_BETA[4] - P[18][4]*SK_BETA[1] + P[18][5]*SK_BETA[2] + P[18][2]*SK_BETA[6] + P[18][6]*SK_BETA[3] - P[18][3]*SK_BETA[7] + P[18][22]*SK_BETA[1] - P[18][23]*SK_BETA[2]);
393  Kfusion[19] = SK_BETA[0]*(P[19][0]*SK_BETA[5] + P[19][1]*SK_BETA[4] - P[19][4]*SK_BETA[1] + P[19][5]*SK_BETA[2] + P[19][2]*SK_BETA[6] + P[19][6]*SK_BETA[3] - P[19][3]*SK_BETA[7] + P[19][22]*SK_BETA[1] - P[19][23]*SK_BETA[2]);
394  Kfusion[20] = SK_BETA[0]*(P[20][0]*SK_BETA[5] + P[20][1]*SK_BETA[4] - P[20][4]*SK_BETA[1] + P[20][5]*SK_BETA[2] + P[20][2]*SK_BETA[6] + P[20][6]*SK_BETA[3] - P[20][3]*SK_BETA[7] + P[20][22]*SK_BETA[1] - P[20][23]*SK_BETA[2]);
395  Kfusion[21] = SK_BETA[0]*(P[21][0]*SK_BETA[5] + P[21][1]*SK_BETA[4] - P[21][4]*SK_BETA[1] + P[21][5]*SK_BETA[2] + P[21][2]*SK_BETA[6] + P[21][6]*SK_BETA[3] - P[21][3]*SK_BETA[7] + P[21][22]*SK_BETA[1] - P[21][23]*SK_BETA[2]);
396  } else {
397  // zero indexes 16 to 21 = 6*4 bytes
398  memset(&Kfusion[16], 0, 24);
399  }
400 
401  if (!inhibitWindStates) {
402  Kfusion[22] = SK_BETA[0]*(P[22][0]*SK_BETA[5] + P[22][1]*SK_BETA[4] - P[22][4]*SK_BETA[1] + P[22][5]*SK_BETA[2] + P[22][2]*SK_BETA[6] + P[22][6]*SK_BETA[3] - P[22][3]*SK_BETA[7] + P[22][22]*SK_BETA[1] - P[22][23]*SK_BETA[2]);
403  Kfusion[23] = SK_BETA[0]*(P[23][0]*SK_BETA[5] + P[23][1]*SK_BETA[4] - P[23][4]*SK_BETA[1] + P[23][5]*SK_BETA[2] + P[23][2]*SK_BETA[6] + P[23][6]*SK_BETA[3] - P[23][3]*SK_BETA[7] + P[23][22]*SK_BETA[1] - P[23][23]*SK_BETA[2]);
404  } else {
405  // zero indexes 22 to 23 = 2*4 bytes
406  memset(&Kfusion[22], 0, 8);
407  }
408 
409  // calculate predicted sideslip angle and innovation using small angle approximation
410  innovBeta = vel_rel_wind.y / vel_rel_wind.x;
411 
412  // reject measurement if greater than 3-sigma inconsistency
413  if (innovBeta > 0.5f) {
414  return;
415  }
416 
417  // correct the state vector
418  for (uint8_t j= 0; j<=stateIndexLim; j++) {
419  statesArray[j] = statesArray[j] - Kfusion[j] * innovBeta;
420  }
421  stateStruct.quat.normalize();
422 
423  // correct the covariance P = (I - K*H)*P
424  // take advantage of the empty columns in KH to reduce the
425  // number of operations
426  for (unsigned i = 0; i<=stateIndexLim; i++) {
427  for (unsigned j = 0; j<=6; j++) {
428  KH[i][j] = Kfusion[i] * H_BETA[j];
429  }
430  for (unsigned j = 7; j<=21; j++) {
431  KH[i][j] = 0.0f;
432  }
433  for (unsigned j = 22; j<=23; j++) {
434  KH[i][j] = Kfusion[i] * H_BETA[j];
435  }
436  }
437  for (unsigned j = 0; j<=stateIndexLim; j++) {
438  for (unsigned i = 0; i<=stateIndexLim; i++) {
439  ftype res = 0;
440  res += KH[i][0] * P[0][j];
441  res += KH[i][1] * P[1][j];
442  res += KH[i][2] * P[2][j];
443  res += KH[i][3] * P[3][j];
444  res += KH[i][4] * P[4][j];
445  res += KH[i][5] * P[5][j];
446  res += KH[i][6] * P[6][j];
447  res += KH[i][22] * P[22][j];
448  res += KH[i][23] * P[23][j];
449  KHP[i][j] = res;
450  }
451  }
452  for (unsigned i = 0; i<=stateIndexLim; i++) {
453  for (unsigned j = 0; j<=stateIndexLim; j++) {
454  P[i][j] = P[i][j] - KHP[i][j];
455  }
456  }
457  }
458 
459  // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
460  ForceSymmetry();
461  ConstrainVariances();
462 
463  // stop the performance timer
464  hal.util->perf_end(_perf_FuseSideslip);
465 }
466 
467 /********************************************************
468 * MISC FUNCTIONS *
469 ********************************************************/
470 
471 
472 #endif // HAL_CPU_CLASS
NavEKF3_core::KHP
Matrix24 KHP
Definition: AP_NavEKF3_core.h:816
NavEKF3_core::posTimeout
bool posTimeout
Definition: AP_NavEKF3_core.h:806
norm
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
NavEKF3_core::q2
ftype q2
Definition: AP_NavEKF3_core.h:1210
NavEKF3_core::statesArray
Vector24 statesArray
Definition: AP_NavEKF3_core.h:422
NavEKF3_core::lastPosPassTime_ms
uint32_t lastPosPassTime_ms
Definition: AP_NavEKF3_core.h:860
NavEKF3_core::badIMUdata
bool badIMUdata
Definition: AP_NavEKF3_core.h:811
NavEKF3::tasRetryTime_ms
const uint16_t tasRetryTime_ms
Definition: AP_NavEKF3.h:432
NavEKF3_core::q3
ftype q3
Definition: AP_NavEKF3_core.h:1211
NavEKF3_core::P
Matrix24 P
Definition: AP_NavEKF3_core.h:817
AP_HAL.h
NavEKF3_core::tas_elements::time_ms
uint32_t time_ms
Definition: AP_NavEKF3_core.h:482
NavEKF3_core::readAirSpdData
void readAirSpdData()
Definition: AP_NavEKF3_Measurements.cpp:690
NavEKF3_core::airSpdFusionDelayed
bool airSpdFusionDelayed
Definition: AP_NavEKF3_core.h:933
NavEKF3_core::Kfusion
Vector28 Kfusion
Definition: AP_NavEKF3_core.h:814
NavEKF3_core::assume_zero_sideslip
bool assume_zero_sideslip(void) const
Definition: AP_NavEKF3_Control.cpp:462
NavEKF3_core::_perf_FuseAirspeed
AP_HAL::Util::perf_counter_t _perf_FuseAirspeed
Definition: AP_NavEKF3_core.h:1233
NavEKF3::_easNoise
AP_Float _easNoise
Definition: AP_NavEKF3.h:374
NavEKF3_core::_ahrs
const AP_AHRS * _ahrs
Definition: AP_NavEKF3_core.h:417
NavEKF3_core::state_elements::quat
Quaternion quat
Definition: AP_NavEKF3_core.h:424
AP_HAL::HAL::util
AP_HAL::Util * util
Definition: HAL.h:115
NavEKF3_core::FuseSideslip
void FuseSideslip()
Definition: AP_NavEKF3_AirDataFusion.cpp:261
NavEKF3_core::lastTasPassTime_ms
uint32_t lastTasPassTime_ms
Definition: AP_NavEKF3_core.h:862
NavEKF3_core::sideSlipFusionDelayed
bool sideSlipFusionDelayed
Definition: AP_NavEKF3_core.h:934
NavEKF3_core::magFusePerformed
bool magFusePerformed
Definition: AP_NavEKF3_core.h:848
NavEKF3_core::tasDataDelayed
tas_elements tasDataDelayed
Definition: AP_NavEKF3_core.h:911
NavEKF3_core::faultStatus
struct NavEKF3_core::@153 faultStatus
NavEKF3_core::tasTimeout
bool tasTimeout
Definition: AP_NavEKF3_core.h:809
NavEKF3_core::state_elements::velocity
Vector3f velocity
Definition: AP_NavEKF3_core.h:425
NavEKF3_core::_perf_FuseSideslip
AP_HAL::Util::perf_counter_t _perf_FuseSideslip
Definition: AP_NavEKF3_core.h:1234
MAX
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
NavEKF3_core::stateStruct
struct NavEKF3_core::state_elements & stateStruct
NavEKF3_core::inhibitDelAngBiasStates
bool inhibitDelAngBiasStates
Definition: AP_NavEKF3_core.h:881
NavEKF3_core::tasDataToFuse
bool tasDataToFuse
Definition: AP_NavEKF3_core.h:856
NavEKF3_core::prevTasStep_ms
uint32_t prevTasStep_ms
Definition: AP_NavEKF3_core.h:850
NavEKF3_core::inhibitMagStates
bool inhibitMagStates
Definition: AP_NavEKF3_core.h:879
NavEKF3_core::use_compass
bool use_compass(void) const
Definition: AP_NavEKF3_Control.cpp:454
NavEKF3_core::ForceSymmetry
void ForceSymmetry()
Definition: AP_NavEKF3_core.cpp:1408
Vector2::y
T y
Definition: vector2.h:37
NavEKF3_core::SelectBetaFusion
void SelectBetaFusion()
Definition: AP_NavEKF3_AirDataFusion.cpp:231
NavEKF3_core::tasHealth
bool tasHealth
Definition: AP_NavEKF3_core.h:804
NavEKF3_core::FuseAirspeed
void FuseAirspeed()
Definition: AP_NavEKF3_AirDataFusion.cpp:25
AP_HAL::HAL
Definition: HAL.h:26
NavEKF3_core::innovVtas
ftype innovVtas
Definition: AP_NavEKF3_core.h:846
f
#define f(i)
NavEKF3_core::ftype
float ftype
Definition: AP_NavEKF3_core.h:365
Vector3::y
T y
Definition: vector3.h:67
NavEKF3_core::q0
ftype q0
Definition: AP_NavEKF3_core.h:1208
NavEKF3::_tasInnovGate
AP_Int16 _tasInnovGate
Definition: AP_NavEKF3.h:389
NavEKF3_core::state_elements::wind_vel
Vector2f wind_vel
Definition: AP_NavEKF3_core.h:431
AP_HAL::Util::perf_end
virtual void perf_end(perf_counter_t h)
Definition: Util.h:104
Vector3::z
T z
Definition: vector3.h:67
NavEKF3_core::stateIndexLim
uint8_t stateIndexLim
Definition: AP_NavEKF3_core.h:897
NavEKF3_core::tasTestRatio
float tasTestRatio
Definition: AP_NavEKF3_core.h:877
AP_NavEKF3_core.h
NavEKF3_core::inhibitWindStates
bool inhibitWindStates
Definition: AP_NavEKF3_core.h:878
NavEKF3_core::prevTnb
Matrix3f prevTnb
Definition: AP_NavEKF3_core.h:825
NavEKF3::betaAvg_ms
const uint16_t betaAvg_ms
Definition: AP_NavEKF3.h:437
hal
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
AP_AHRS.h
AP_Vehicle.h
NavEKF3_core::varInnovVtas
ftype varInnovVtas
Definition: AP_NavEKF3_core.h:847
NavEKF3_core::inhibitDelVelBiasStates
bool inhibitDelVelBiasStates
Definition: AP_NavEKF3_core.h:880
NavEKF3_core::tas_elements::tas
float tas
Definition: AP_NavEKF3_core.h:481
constrain_float
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
Vector2::x
T x
Definition: vector2.h:37
Quaternion::normalize
void normalize()
Definition: quaternion.cpp:297
AP_NavEKF3.h
NavEKF3_core::KH
Matrix24 KH
Definition: AP_NavEKF3_core.h:815
AP_HAL::Util::perf_begin
virtual void perf_begin(perf_counter_t h)
Definition: Util.h:103
NavEKF3_core::prevBetaStep_ms
uint32_t prevBetaStep_ms
Definition: AP_NavEKF3_core.h:851
NavEKF3_core::Vector8
ftype Vector8[8]
Definition: AP_NavEKF3_core.h:398
NavEKF3_core::Vector13
ftype Vector13[13]
Definition: AP_NavEKF3_core.h:402
sq
float sq(const T val)
Definition: AP_Math.h:170
NavEKF3_core::statesInitialised
bool statesInitialised
Definition: AP_NavEKF3_core.h:799
NavEKF3_core::Vector24
ftype Vector24[24]
Definition: AP_NavEKF3_core.h:408
NavEKF3_core::tasDataNew
tas_elements tasDataNew
Definition: AP_NavEKF3_core.h:910
NavEKF3_core::SelectTasFusion
void SelectTasFusion()
Definition: AP_NavEKF3_AirDataFusion.cpp:200
NavEKF3::posRetryTimeNoVel_ms
const uint16_t posRetryTimeNoVel_ms
Definition: AP_NavEKF3.h:429
NavEKF3_core::ConstrainVariances
void ConstrainVariances()
Definition: AP_NavEKF3_core.cpp:1423
AP_AHRS::get_EAS2TAS
float get_EAS2TAS(void) const
Definition: AP_AHRS.h:290
NavEKF3_core::dtIMUavg
ftype dtIMUavg
Definition: AP_NavEKF3_core.h:829
NavEKF3_core::q1
ftype q1
Definition: AP_NavEKF3_core.h:1209
NavEKF3_core::frontend
NavEKF3 * frontend
Definition: AP_NavEKF3_core.h:359
NavEKF3_core::CovarianceInit
void CovarianceInit()
Definition: AP_NavEKF3_core.cpp:484
NavEKF3_core::imuSampleTime_ms
uint32_t imuSampleTime_ms
Definition: AP_NavEKF3_core.h:855
Vector3::x
T x
Definition: vector3.h:67
NavEKF3_core::useAirspeed
bool useAirspeed(void) const
Definition: AP_NavEKF3_Control.cpp:405
Generated on Sun Jun 17 2018 14:18:50 for APM:Libraries by   doxygen 1.8.13