PX4 Firmware
PX4 Autopilot Software http://px4.io
covariance.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Estimation and Control Library (ECL). 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 ECL 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 covariance.cpp
36  * Contains functions for initialising, predicting and updating the state
37  * covariance matrix
38  *
39  * @author Roman Bast <bastroman@gmail.com>
40  *
41  */
42 
43 #include "ekf.h"
44 
45 #include <ecl.h>
46 #include <math.h>
47 #include <mathlib/mathlib.h>
48 
49 void Ekf::initialiseCovariance()
50 {
51  for (unsigned i = 0; i < _k_num_states; i++) {
52  for (unsigned j = 0; j < _k_num_states; j++) {
53  P[i][j] = 0.0f;
54  }
55  }
56 
57  _delta_angle_bias_var_accum.setZero();
58  _delta_vel_bias_var_accum.setZero();
59 
60  // calculate average prediction time step in sec
61  float dt = FILTER_UPDATE_PERIOD_S;
62 
63  // define the initial angle uncertainty as variances for a rotation vector
64  Vector3f rot_vec_var;
65  rot_vec_var(2) = rot_vec_var(1) = rot_vec_var(0) = sq(_params.initial_tilt_err);
66 
67  // update the quaternion state covariances
68  initialiseQuatCovariances(rot_vec_var);
69 
70  // velocity
71  P[4][4] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
72  P[5][5] = P[4][4];
73  P[6][6] = sq(1.5f) * P[4][4];
74 
75  // position
76  P[7][7] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
77  P[8][8] = P[7][7];
78 
79  if (_control_status.flags.rng_hgt) {
80  P[9][9] = sq(fmaxf(_params.range_noise, 0.01f));
81 
82  } else if (_control_status.flags.gps_hgt) {
83  float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
84  float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
85  P[9][9] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
86 
87  } else {
88  P[9][9] = sq(fmaxf(_params.baro_noise, 0.01f));
89  }
90 
91  // gyro bias
92  P[10][10] = sq(_params.switch_on_gyro_bias * dt);
93  P[11][11] = P[10][10];
94  P[12][12] = P[10][10];
95 
96  // accel bias
97  _prev_dvel_bias_var(0) = P[13][13] = sq(_params.switch_on_accel_bias * dt);
98  _prev_dvel_bias_var(1) = P[14][14] = P[13][13];
99  _prev_dvel_bias_var(2) = P[15][15] = P[13][13];
100 
101  // record IMU bias state covariance reset time - used to prevent resets being performed too often
102  _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;
103 
104  // variances for optional states
105 
106  // earth frame and body frame magnetic field
107  // set to observation variance
108  for (uint8_t index = 16; index <= 21; index ++) {
109  P[index][index] = sq(_params.mag_noise);
110  }
111 
112  // save covariance data for re-use when auto-switching between heading and 3-axis fusion
113  saveMagCovData();
114 
115  // wind
116  P[22][22] = sq(_params.initial_wind_uncertainty);
117  P[23][23] = sq(_params.initial_wind_uncertainty);
118 
119 }
120 
121 void Ekf::get_pos_var(Vector3f &pos_var)
122 {
123  pos_var(0) = P[7][7];
124  pos_var(1) = P[8][8];
125  pos_var(2) = P[9][9];
126 }
127 
128 void Ekf::get_vel_var(Vector3f &vel_var)
129 {
130  vel_var(0) = P[4][4];
131  vel_var(1) = P[5][5];
132  vel_var(2) = P[6][6];
133 }
134 
135 void Ekf::predictCovariance()
136 {
137  // assign intermediate state variables
138  float q0 = _state.quat_nominal(0);
139  float q1 = _state.quat_nominal(1);
140  float q2 = _state.quat_nominal(2);
141  float q3 = _state.quat_nominal(3);
142 
143  float dax = _imu_sample_delayed.delta_ang(0);
144  float day = _imu_sample_delayed.delta_ang(1);
145  float daz = _imu_sample_delayed.delta_ang(2);
146 
147  float dvx = _imu_sample_delayed.delta_vel(0);
148  float dvy = _imu_sample_delayed.delta_vel(1);
149  float dvz = _imu_sample_delayed.delta_vel(2);
150 
151  float dax_b = _state.gyro_bias(0);
152  float day_b = _state.gyro_bias(1);
153  float daz_b = _state.gyro_bias(2);
154 
155  float dvx_b = _state.accel_bias(0);
156  float dvy_b = _state.accel_bias(1);
157  float dvz_b = _state.accel_bias(2);
158 
159  float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
160  float dt_inv = 1.0f / dt;
161 
162  // compute noise variance for stationary processes
163  float process_noise[_k_num_states] = {};
164 
165  // convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
166  float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
167 
168  // convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
169  float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
170 
171  // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
172  float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
173  float beta = 1.0f - alpha;
174  _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
175  _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt);
176  _accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0);
177  _accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1);
178  _accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2);
179 
180  if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
181  || _accel_mag_filt > _params.acc_bias_learn_acc_lim
182  || _bad_vert_accel_detected) {
183 
184  // store the bias state variances to be reinstated later
185  if (!_accel_bias_inhibit) {
186  _prev_dvel_bias_var(0) = P[13][13];
187  _prev_dvel_bias_var(1) = P[14][14];
188  _prev_dvel_bias_var(2) = P[15][15];
189  }
190 
191  _accel_bias_inhibit = true;
192 
193  } else {
194  if (_accel_bias_inhibit) {
195  // reinstate the bias state variances
196  P[13][13] = _prev_dvel_bias_var(0);
197  P[14][14] = _prev_dvel_bias_var(1);
198  P[15][15] = _prev_dvel_bias_var(2);
199 
200  } else {
201  // store the bias state variances to be reinstated later
202  _prev_dvel_bias_var(0) = P[13][13];
203  _prev_dvel_bias_var(1) = P[14][14];
204  _prev_dvel_bias_var(2) = P[15][15];
205  }
206 
207  _accel_bias_inhibit = false;
208  }
209 
210  // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
211  float mag_I_sig;
212 
213  if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][18]) < 0.1f) {
214  mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
215 
216  } else {
217  mag_I_sig = 0.0f;
218  }
219 
220  // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
221  float mag_B_sig;
222 
223  if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
224  mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);
225 
226  } else {
227  mag_B_sig = 0.0f;
228  }
229 
230  float wind_vel_sig;
231 
232  // Calculate low pass filtered height rate
233  float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant
234  _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
235 
236  // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
237  if (_control_status.flags.wind && (P[22][22] + P[23][23]) < sq(_params.initial_wind_uncertainty)) {
238  wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f) * (1.0f + _params.wind_vel_p_noise_scaler * fabsf(_height_rate_lpf));
239 
240  } else {
241  wind_vel_sig = 0.0f;
242  }
243 
244  // Construct the process noise variance diagonal for those states with a stationary process model
245  // These are kinematic states and their error growth is controlled separately by the IMU noise variances
246  for (unsigned i = 0; i <= 9; i++) {
247  process_noise[i] = 0.0f;
248  }
249 
250  // delta angle bias states
251  process_noise[12] = process_noise[11] = process_noise[10] = sq(d_ang_bias_sig);
252  // delta_velocity bias states
253  process_noise[15] = process_noise[14] = process_noise[13] = sq(d_vel_bias_sig);
254  // earth frame magnetic field states
255  process_noise[18] = process_noise[17] = process_noise[16] = sq(mag_I_sig);
256  // body frame magnetic field states
257  process_noise[21] = process_noise[20] = process_noise[19] = sq(mag_B_sig);
258  // wind velocity states
259  process_noise[23] = process_noise[22] = sq(wind_vel_sig);
260 
261  // assign IMU noise variances
262  // inputs to the system are 3 delta angles and 3 delta velocities
263  float daxVar, dayVar, dazVar;
264  float dvxVar, dvyVar, dvzVar;
265  float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f);
266  daxVar = dayVar = dazVar = sq(dt * gyro_noise);
267  float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f);
268 
269  if (_bad_vert_accel_detected) {
270  // Increase accelerometer process noise if bad accel data is detected. Measurement errors due to
271  // vibration induced clipping commonly reach an equivalent 0.5g offset.
272  accel_noise = BADACC_BIAS_PNOISE;
273  }
274 
275  dvxVar = dvyVar = dvzVar = sq(dt * accel_noise);
276 
277  // predict the covariance
278 
279  // intermediate calculations
280  float SF[21];
281  SF[0] = dvz - dvz_b;
282  SF[1] = dvy - dvy_b;
283  SF[2] = dvx - dvx_b;
284  SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0];
285  SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2];
286  SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1];
287  SF[6] = day/2 - day_b/2;
288  SF[7] = daz/2 - daz_b/2;
289  SF[8] = dax/2 - dax_b/2;
290  SF[9] = dax_b/2 - dax/2;
291  SF[10] = daz_b/2 - daz/2;
292  SF[11] = day_b/2 - day/2;
293  SF[12] = 2*q1*SF[1];
294  SF[13] = 2*q0*SF[0];
295  SF[14] = q1/2;
296  SF[15] = q2/2;
297  SF[16] = q3/2;
298  SF[17] = sq(q3);
299  SF[18] = sq(q2);
300  SF[19] = sq(q1);
301  SF[20] = sq(q0);
302 
303  float SG[8];
304  SG[0] = q0/2;
305  SG[1] = sq(q3);
306  SG[2] = sq(q2);
307  SG[3] = sq(q1);
308  SG[4] = sq(q0);
309  SG[5] = 2*q2*q3;
310  SG[6] = 2*q1*q3;
311  SG[7] = 2*q1*q2;
312 
313  float SQ[11];
314  SQ[0] = dvzVar*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyVar*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxVar*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
315  SQ[1] = dvzVar*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxVar*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyVar*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
316  SQ[2] = dvzVar*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyVar*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxVar*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
317  SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4;
318  SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4;
319  SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4;
320  SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4;
321  SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2;
322  SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4;
323  SQ[9] = sq(SG[0]);
324  SQ[10] = sq(q1);
325 
326  float SPP[11] = {};
327  SPP[0] = SF[12] + SF[13] - 2*q2*SF[2];
328  SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
329  SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
330  SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
331  SPP[4] = 2*q0*q2 - 2*q1*q3;
332  SPP[5] = 2*q0*q1 - 2*q2*q3;
333  SPP[6] = 2*q0*q3 - 2*q1*q2;
334  SPP[7] = 2*q0*q1 + 2*q2*q3;
335  SPP[8] = 2*q0*q3 + 2*q1*q2;
336  SPP[9] = 2*q0*q2 + 2*q1*q3;
337  SPP[10] = SF[16];
338 
339  // covariance update
340  float nextP[24][24];
341 
342  // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
343  nextP[0][0] = P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[11]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[10]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) + SF[15]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) + SPP[10]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4;
344  nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10] + SF[8]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[7]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[11]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SF[15]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) + SPP[10]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]))/2;
345  nextP[1][1] = P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] + daxVar*SQ[9] - (P[10][1]*q0)/2 + SF[8]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[11]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SF[15]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) + SPP[10]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) + (dayVar*sq(q3))/4 + (dazVar*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2))/2;
346  nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10] + SF[6]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[10]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[8]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SF[14]*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]) - SPP[10]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - (q0*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]))/2;
347  nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[10]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SF[14]*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2) - SPP[10]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2))/2;
348  nextP[2][2] = P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] + dayVar*SQ[9] + (dazVar*SQ[10])/4 - (P[11][2]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[10]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SF[14]*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2) - SPP[10]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) + (daxVar*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2))/2;
349  nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10] + SF[7]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[6]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) + SF[9]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[15]*(P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10]) - SF[14]*(P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10]) - (q0*(P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10]))/2;
350  nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2 + SF[7]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[6]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[15]*(P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2) - SF[14]*(P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2))/2;
351  nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2 + SF[7]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[6]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[15]*(P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2) - SF[14]*(P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2))/2;
352  nextP[3][3] = P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] + (dayVar*SQ[10])/4 + dazVar*SQ[9] - (P[12][3]*q0)/2 + SF[7]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[6]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[15]*(P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2) - SF[14]*(P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2) + (daxVar*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2))/2;
353  nextP[0][4] = P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10] + SF[5]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[4]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SPP[3]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[6]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[9]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
354  nextP[1][4] = P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[4]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[6]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[9]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
355  nextP[2][4] = P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[4]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[6]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[9]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
356  nextP[3][4] = P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[4]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[6]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[9]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
357  nextP[4][4] = P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9] + dvyVar*sq(SG[7] - 2*q0*q3) + dvzVar*sq(SG[6] + 2*q0*q2) + SF[5]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[4]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SPP[3]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[6]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[9]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]) + dvxVar*sq(SG[1] + SG[2] - SG[3] - SG[4]);
358  nextP[0][5] = P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10] + SF[4]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SF[3]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[5]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) - SPP[0]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SPP[8]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) + SPP[2]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) + SPP[5]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
359  nextP[1][5] = P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SF[3]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SPP[8]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) + SPP[2]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) + SPP[5]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
360  nextP[2][5] = P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SF[3]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SPP[8]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) + SPP[2]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) + SPP[5]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
361  nextP[3][5] = P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SF[3]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SPP[8]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) + SPP[2]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) + SPP[5]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
362  nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9] + SF[4]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SF[3]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[5]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) - SPP[0]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SPP[8]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) + SPP[2]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) + SPP[5]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
363  nextP[5][5] = P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5] + dvxVar*sq(SG[7] + 2*q0*q3) + dvzVar*sq(SG[5] - 2*q0*q1) + SF[4]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SF[3]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[5]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) - SPP[0]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SPP[8]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) + SPP[2]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) + SPP[5]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]) + dvyVar*sq(SG[1] - SG[2] + SG[3] - SG[4]);
364  nextP[0][6] = P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10] + SF[4]*(P[0][1] + P[1][1]*SF[9] + P[2][1]*SF[11] + P[3][1]*SF[10] + P[10][1]*SF[14] + P[11][1]*SF[15] + P[12][1]*SPP[10]) - SF[5]*(P[0][2] + P[1][2]*SF[9] + P[2][2]*SF[11] + P[3][2]*SF[10] + P[10][2]*SF[14] + P[11][2]*SF[15] + P[12][2]*SPP[10]) + SF[3]*(P[0][3] + P[1][3]*SF[9] + P[2][3]*SF[11] + P[3][3]*SF[10] + P[10][3]*SF[14] + P[11][3]*SF[15] + P[12][3]*SPP[10]) + SPP[0]*(P[0][0] + P[1][0]*SF[9] + P[2][0]*SF[11] + P[3][0]*SF[10] + P[10][0]*SF[14] + P[11][0]*SF[15] + P[12][0]*SPP[10]) + SPP[4]*(P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10]) - SPP[7]*(P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10]) - SPP[1]*(P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10]);
365  nextP[1][6] = P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2 + SF[4]*(P[1][1] + P[0][1]*SF[8] + P[2][1]*SF[7] + P[3][1]*SF[11] - P[12][1]*SF[15] + P[11][1]*SPP[10] - (P[10][1]*q0)/2) - SF[5]*(P[1][2] + P[0][2]*SF[8] + P[2][2]*SF[7] + P[3][2]*SF[11] - P[12][2]*SF[15] + P[11][2]*SPP[10] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[8] + P[2][3]*SF[7] + P[3][3]*SF[11] - P[12][3]*SF[15] + P[11][3]*SPP[10] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[8] + P[2][0]*SF[7] + P[3][0]*SF[11] - P[12][0]*SF[15] + P[11][0]*SPP[10] - (P[10][0]*q0)/2) + SPP[4]*(P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2) - SPP[7]*(P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2) - SPP[1]*(P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2);
366  nextP[2][6] = P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2 + SF[4]*(P[2][1] + P[0][1]*SF[6] + P[1][1]*SF[10] + P[3][1]*SF[8] + P[12][1]*SF[14] - P[10][1]*SPP[10] - (P[11][1]*q0)/2) - SF[5]*(P[2][2] + P[0][2]*SF[6] + P[1][2]*SF[10] + P[3][2]*SF[8] + P[12][2]*SF[14] - P[10][2]*SPP[10] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[6] + P[1][3]*SF[10] + P[3][3]*SF[8] + P[12][3]*SF[14] - P[10][3]*SPP[10] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[6] + P[1][0]*SF[10] + P[3][0]*SF[8] + P[12][0]*SF[14] - P[10][0]*SPP[10] - (P[11][0]*q0)/2) + SPP[4]*(P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2) - SPP[7]*(P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2) - SPP[1]*(P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2);
367  nextP[3][6] = P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2 + SF[4]*(P[3][1] + P[0][1]*SF[7] + P[1][1]*SF[6] + P[2][1]*SF[9] + P[10][1]*SF[15] - P[11][1]*SF[14] - (P[12][1]*q0)/2) - SF[5]*(P[3][2] + P[0][2]*SF[7] + P[1][2]*SF[6] + P[2][2]*SF[9] + P[10][2]*SF[15] - P[11][2]*SF[14] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[7] + P[1][3]*SF[6] + P[2][3]*SF[9] + P[10][3]*SF[15] - P[11][3]*SF[14] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[7] + P[1][0]*SF[6] + P[2][0]*SF[9] + P[10][0]*SF[15] - P[11][0]*SF[14] - (P[12][0]*q0)/2) + SPP[4]*(P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2) - SPP[7]*(P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2) - SPP[1]*(P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2);
368  nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9] + SF[4]*(P[4][1] + P[0][1]*SF[5] + P[1][1]*SF[3] - P[3][1]*SF[4] + P[2][1]*SPP[0] + P[13][1]*SPP[3] + P[14][1]*SPP[6] - P[15][1]*SPP[9]) - SF[5]*(P[4][2] + P[0][2]*SF[5] + P[1][2]*SF[3] - P[3][2]*SF[4] + P[2][2]*SPP[0] + P[13][2]*SPP[3] + P[14][2]*SPP[6] - P[15][2]*SPP[9]) + SF[3]*(P[4][3] + P[0][3]*SF[5] + P[1][3]*SF[3] - P[3][3]*SF[4] + P[2][3]*SPP[0] + P[13][3]*SPP[3] + P[14][3]*SPP[6] - P[15][3]*SPP[9]) + SPP[0]*(P[4][0] + P[0][0]*SF[5] + P[1][0]*SF[3] - P[3][0]*SF[4] + P[2][0]*SPP[0] + P[13][0]*SPP[3] + P[14][0]*SPP[6] - P[15][0]*SPP[9]) + SPP[4]*(P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9]) - SPP[7]*(P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9]) - SPP[1]*(P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9]);
369  nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5] + SF[4]*(P[5][1] + P[0][1]*SF[4] + P[2][1]*SF[3] + P[3][1]*SF[5] - P[1][1]*SPP[0] - P[13][1]*SPP[8] + P[14][1]*SPP[2] + P[15][1]*SPP[5]) - SF[5]*(P[5][2] + P[0][2]*SF[4] + P[2][2]*SF[3] + P[3][2]*SF[5] - P[1][2]*SPP[0] - P[13][2]*SPP[8] + P[14][2]*SPP[2] + P[15][2]*SPP[5]) + SF[3]*(P[5][3] + P[0][3]*SF[4] + P[2][3]*SF[3] + P[3][3]*SF[5] - P[1][3]*SPP[0] - P[13][3]*SPP[8] + P[14][3]*SPP[2] + P[15][3]*SPP[5]) + SPP[0]*(P[5][0] + P[0][0]*SF[4] + P[2][0]*SF[3] + P[3][0]*SF[5] - P[1][0]*SPP[0] - P[13][0]*SPP[8] + P[14][0]*SPP[2] + P[15][0]*SPP[5]) + SPP[4]*(P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5]) - SPP[7]*(P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5]) - SPP[1]*(P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5]);
370  nextP[6][6] = P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1] + dvxVar*sq(SG[6] - 2*q0*q2) + dvyVar*sq(SG[5] + 2*q0*q1) + SF[4]*(P[6][1] + P[1][1]*SF[4] - P[2][1]*SF[5] + P[3][1]*SF[3] + P[0][1]*SPP[0] + P[13][1]*SPP[4] - P[14][1]*SPP[7] - P[15][1]*SPP[1]) - SF[5]*(P[6][2] + P[1][2]*SF[4] - P[2][2]*SF[5] + P[3][2]*SF[3] + P[0][2]*SPP[0] + P[13][2]*SPP[4] - P[14][2]*SPP[7] - P[15][2]*SPP[1]) + SF[3]*(P[6][3] + P[1][3]*SF[4] - P[2][3]*SF[5] + P[3][3]*SF[3] + P[0][3]*SPP[0] + P[13][3]*SPP[4] - P[14][3]*SPP[7] - P[15][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[4] - P[2][0]*SF[5] + P[3][0]*SF[3] + P[0][0]*SPP[0] + P[13][0]*SPP[4] - P[14][0]*SPP[7] - P[15][0]*SPP[1]) + SPP[4]*(P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1]) - SPP[7]*(P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1]) - SPP[1]*(P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1]) + dvzVar*sq(SG[1] - SG[2] - SG[3] + SG[4]);
371  nextP[0][7] = P[0][7] + P[1][7]*SF[9] + P[2][7]*SF[11] + P[3][7]*SF[10] + P[10][7]*SF[14] + P[11][7]*SF[15] + P[12][7]*SPP[10] + dt*(P[0][4] + P[1][4]*SF[9] + P[2][4]*SF[11] + P[3][4]*SF[10] + P[10][4]*SF[14] + P[11][4]*SF[15] + P[12][4]*SPP[10]);
372  nextP[1][7] = P[1][7] + P[0][7]*SF[8] + P[2][7]*SF[7] + P[3][7]*SF[11] - P[12][7]*SF[15] + P[11][7]*SPP[10] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[8] + P[2][4]*SF[7] + P[3][4]*SF[11] - P[12][4]*SF[15] + P[11][4]*SPP[10] - (P[10][4]*q0)/2);
373  nextP[2][7] = P[2][7] + P[0][7]*SF[6] + P[1][7]*SF[10] + P[3][7]*SF[8] + P[12][7]*SF[14] - P[10][7]*SPP[10] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[6] + P[1][4]*SF[10] + P[3][4]*SF[8] + P[12][4]*SF[14] - P[10][4]*SPP[10] - (P[11][4]*q0)/2);
374  nextP[3][7] = P[3][7] + P[0][7]*SF[7] + P[1][7]*SF[6] + P[2][7]*SF[9] + P[10][7]*SF[15] - P[11][7]*SF[14] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[7] + P[1][4]*SF[6] + P[2][4]*SF[9] + P[10][4]*SF[15] - P[11][4]*SF[14] - (P[12][4]*q0)/2);
375  nextP[4][7] = P[4][7] + P[0][7]*SF[5] + P[1][7]*SF[3] - P[3][7]*SF[4] + P[2][7]*SPP[0] + P[13][7]*SPP[3] + P[14][7]*SPP[6] - P[15][7]*SPP[9] + dt*(P[4][4] + P[0][4]*SF[5] + P[1][4]*SF[3] - P[3][4]*SF[4] + P[2][4]*SPP[0] + P[13][4]*SPP[3] + P[14][4]*SPP[6] - P[15][4]*SPP[9]);
376  nextP[5][7] = P[5][7] + P[0][7]*SF[4] + P[2][7]*SF[3] + P[3][7]*SF[5] - P[1][7]*SPP[0] - P[13][7]*SPP[8] + P[14][7]*SPP[2] + P[15][7]*SPP[5] + dt*(P[5][4] + P[0][4]*SF[4] + P[2][4]*SF[3] + P[3][4]*SF[5] - P[1][4]*SPP[0] - P[13][4]*SPP[8] + P[14][4]*SPP[2] + P[15][4]*SPP[5]);
377  nextP[6][7] = P[6][7] + P[1][7]*SF[4] - P[2][7]*SF[5] + P[3][7]*SF[3] + P[0][7]*SPP[0] + P[13][7]*SPP[4] - P[14][7]*SPP[7] - P[15][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[4] - P[2][4]*SF[5] + P[3][4]*SF[3] + P[0][4]*SPP[0] + P[13][4]*SPP[4] - P[14][4]*SPP[7] - P[15][4]*SPP[1]);
378  nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
379  nextP[0][8] = P[0][8] + P[1][8]*SF[9] + P[2][8]*SF[11] + P[3][8]*SF[10] + P[10][8]*SF[14] + P[11][8]*SF[15] + P[12][8]*SPP[10] + dt*(P[0][5] + P[1][5]*SF[9] + P[2][5]*SF[11] + P[3][5]*SF[10] + P[10][5]*SF[14] + P[11][5]*SF[15] + P[12][5]*SPP[10]);
380  nextP[1][8] = P[1][8] + P[0][8]*SF[8] + P[2][8]*SF[7] + P[3][8]*SF[11] - P[12][8]*SF[15] + P[11][8]*SPP[10] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[8] + P[2][5]*SF[7] + P[3][5]*SF[11] - P[12][5]*SF[15] + P[11][5]*SPP[10] - (P[10][5]*q0)/2);
381  nextP[2][8] = P[2][8] + P[0][8]*SF[6] + P[1][8]*SF[10] + P[3][8]*SF[8] + P[12][8]*SF[14] - P[10][8]*SPP[10] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[6] + P[1][5]*SF[10] + P[3][5]*SF[8] + P[12][5]*SF[14] - P[10][5]*SPP[10] - (P[11][5]*q0)/2);
382  nextP[3][8] = P[3][8] + P[0][8]*SF[7] + P[1][8]*SF[6] + P[2][8]*SF[9] + P[10][8]*SF[15] - P[11][8]*SF[14] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[7] + P[1][5]*SF[6] + P[2][5]*SF[9] + P[10][5]*SF[15] - P[11][5]*SF[14] - (P[12][5]*q0)/2);
383  nextP[4][8] = P[4][8] + P[0][8]*SF[5] + P[1][8]*SF[3] - P[3][8]*SF[4] + P[2][8]*SPP[0] + P[13][8]*SPP[3] + P[14][8]*SPP[6] - P[15][8]*SPP[9] + dt*(P[4][5] + P[0][5]*SF[5] + P[1][5]*SF[3] - P[3][5]*SF[4] + P[2][5]*SPP[0] + P[13][5]*SPP[3] + P[14][5]*SPP[6] - P[15][5]*SPP[9]);
384  nextP[5][8] = P[5][8] + P[0][8]*SF[4] + P[2][8]*SF[3] + P[3][8]*SF[5] - P[1][8]*SPP[0] - P[13][8]*SPP[8] + P[14][8]*SPP[2] + P[15][8]*SPP[5] + dt*(P[5][5] + P[0][5]*SF[4] + P[2][5]*SF[3] + P[3][5]*SF[5] - P[1][5]*SPP[0] - P[13][5]*SPP[8] + P[14][5]*SPP[2] + P[15][5]*SPP[5]);
385  nextP[6][8] = P[6][8] + P[1][8]*SF[4] - P[2][8]*SF[5] + P[3][8]*SF[3] + P[0][8]*SPP[0] + P[13][8]*SPP[4] - P[14][8]*SPP[7] - P[15][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[4] - P[2][5]*SF[5] + P[3][5]*SF[3] + P[0][5]*SPP[0] + P[13][5]*SPP[4] - P[14][5]*SPP[7] - P[15][5]*SPP[1]);
386  nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
387  nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
388  nextP[0][9] = P[0][9] + P[1][9]*SF[9] + P[2][9]*SF[11] + P[3][9]*SF[10] + P[10][9]*SF[14] + P[11][9]*SF[15] + P[12][9]*SPP[10] + dt*(P[0][6] + P[1][6]*SF[9] + P[2][6]*SF[11] + P[3][6]*SF[10] + P[10][6]*SF[14] + P[11][6]*SF[15] + P[12][6]*SPP[10]);
389  nextP[1][9] = P[1][9] + P[0][9]*SF[8] + P[2][9]*SF[7] + P[3][9]*SF[11] - P[12][9]*SF[15] + P[11][9]*SPP[10] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[8] + P[2][6]*SF[7] + P[3][6]*SF[11] - P[12][6]*SF[15] + P[11][6]*SPP[10] - (P[10][6]*q0)/2);
390  nextP[2][9] = P[2][9] + P[0][9]*SF[6] + P[1][9]*SF[10] + P[3][9]*SF[8] + P[12][9]*SF[14] - P[10][9]*SPP[10] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[6] + P[1][6]*SF[10] + P[3][6]*SF[8] + P[12][6]*SF[14] - P[10][6]*SPP[10] - (P[11][6]*q0)/2);
391  nextP[3][9] = P[3][9] + P[0][9]*SF[7] + P[1][9]*SF[6] + P[2][9]*SF[9] + P[10][9]*SF[15] - P[11][9]*SF[14] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[7] + P[1][6]*SF[6] + P[2][6]*SF[9] + P[10][6]*SF[15] - P[11][6]*SF[14] - (P[12][6]*q0)/2);
392  nextP[4][9] = P[4][9] + P[0][9]*SF[5] + P[1][9]*SF[3] - P[3][9]*SF[4] + P[2][9]*SPP[0] + P[13][9]*SPP[3] + P[14][9]*SPP[6] - P[15][9]*SPP[9] + dt*(P[4][6] + P[0][6]*SF[5] + P[1][6]*SF[3] - P[3][6]*SF[4] + P[2][6]*SPP[0] + P[13][6]*SPP[3] + P[14][6]*SPP[6] - P[15][6]*SPP[9]);
393  nextP[5][9] = P[5][9] + P[0][9]*SF[4] + P[2][9]*SF[3] + P[3][9]*SF[5] - P[1][9]*SPP[0] - P[13][9]*SPP[8] + P[14][9]*SPP[2] + P[15][9]*SPP[5] + dt*(P[5][6] + P[0][6]*SF[4] + P[2][6]*SF[3] + P[3][6]*SF[5] - P[1][6]*SPP[0] - P[13][6]*SPP[8] + P[14][6]*SPP[2] + P[15][6]*SPP[5]);
394  nextP[6][9] = P[6][9] + P[1][9]*SF[4] - P[2][9]*SF[5] + P[3][9]*SF[3] + P[0][9]*SPP[0] + P[13][9]*SPP[4] - P[14][9]*SPP[7] - P[15][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[4] - P[2][6]*SF[5] + P[3][6]*SF[3] + P[0][6]*SPP[0] + P[13][6]*SPP[4] - P[14][6]*SPP[7] - P[15][6]*SPP[1]);
395  nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
396  nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
397  nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
398  nextP[0][10] = P[0][10] + P[1][10]*SF[9] + P[2][10]*SF[11] + P[3][10]*SF[10] + P[10][10]*SF[14] + P[11][10]*SF[15] + P[12][10]*SPP[10];
399  nextP[1][10] = P[1][10] + P[0][10]*SF[8] + P[2][10]*SF[7] + P[3][10]*SF[11] - P[12][10]*SF[15] + P[11][10]*SPP[10] - (P[10][10]*q0)/2;
400  nextP[2][10] = P[2][10] + P[0][10]*SF[6] + P[1][10]*SF[10] + P[3][10]*SF[8] + P[12][10]*SF[14] - P[10][10]*SPP[10] - (P[11][10]*q0)/2;
401  nextP[3][10] = P[3][10] + P[0][10]*SF[7] + P[1][10]*SF[6] + P[2][10]*SF[9] + P[10][10]*SF[15] - P[11][10]*SF[14] - (P[12][10]*q0)/2;
402  nextP[4][10] = P[4][10] + P[0][10]*SF[5] + P[1][10]*SF[3] - P[3][10]*SF[4] + P[2][10]*SPP[0] + P[13][10]*SPP[3] + P[14][10]*SPP[6] - P[15][10]*SPP[9];
403  nextP[5][10] = P[5][10] + P[0][10]*SF[4] + P[2][10]*SF[3] + P[3][10]*SF[5] - P[1][10]*SPP[0] - P[13][10]*SPP[8] + P[14][10]*SPP[2] + P[15][10]*SPP[5];
404  nextP[6][10] = P[6][10] + P[1][10]*SF[4] - P[2][10]*SF[5] + P[3][10]*SF[3] + P[0][10]*SPP[0] + P[13][10]*SPP[4] - P[14][10]*SPP[7] - P[15][10]*SPP[1];
405  nextP[7][10] = P[7][10] + P[4][10]*dt;
406  nextP[8][10] = P[8][10] + P[5][10]*dt;
407  nextP[9][10] = P[9][10] + P[6][10]*dt;
408  nextP[10][10] = P[10][10];
409  nextP[0][11] = P[0][11] + P[1][11]*SF[9] + P[2][11]*SF[11] + P[3][11]*SF[10] + P[10][11]*SF[14] + P[11][11]*SF[15] + P[12][11]*SPP[10];
410  nextP[1][11] = P[1][11] + P[0][11]*SF[8] + P[2][11]*SF[7] + P[3][11]*SF[11] - P[12][11]*SF[15] + P[11][11]*SPP[10] - (P[10][11]*q0)/2;
411  nextP[2][11] = P[2][11] + P[0][11]*SF[6] + P[1][11]*SF[10] + P[3][11]*SF[8] + P[12][11]*SF[14] - P[10][11]*SPP[10] - (P[11][11]*q0)/2;
412  nextP[3][11] = P[3][11] + P[0][11]*SF[7] + P[1][11]*SF[6] + P[2][11]*SF[9] + P[10][11]*SF[15] - P[11][11]*SF[14] - (P[12][11]*q0)/2;
413  nextP[4][11] = P[4][11] + P[0][11]*SF[5] + P[1][11]*SF[3] - P[3][11]*SF[4] + P[2][11]*SPP[0] + P[13][11]*SPP[3] + P[14][11]*SPP[6] - P[15][11]*SPP[9];
414  nextP[5][11] = P[5][11] + P[0][11]*SF[4] + P[2][11]*SF[3] + P[3][11]*SF[5] - P[1][11]*SPP[0] - P[13][11]*SPP[8] + P[14][11]*SPP[2] + P[15][11]*SPP[5];
415  nextP[6][11] = P[6][11] + P[1][11]*SF[4] - P[2][11]*SF[5] + P[3][11]*SF[3] + P[0][11]*SPP[0] + P[13][11]*SPP[4] - P[14][11]*SPP[7] - P[15][11]*SPP[1];
416  nextP[7][11] = P[7][11] + P[4][11]*dt;
417  nextP[8][11] = P[8][11] + P[5][11]*dt;
418  nextP[9][11] = P[9][11] + P[6][11]*dt;
419  nextP[10][11] = P[10][11];
420  nextP[11][11] = P[11][11];
421  nextP[0][12] = P[0][12] + P[1][12]*SF[9] + P[2][12]*SF[11] + P[3][12]*SF[10] + P[10][12]*SF[14] + P[11][12]*SF[15] + P[12][12]*SPP[10];
422  nextP[1][12] = P[1][12] + P[0][12]*SF[8] + P[2][12]*SF[7] + P[3][12]*SF[11] - P[12][12]*SF[15] + P[11][12]*SPP[10] - (P[10][12]*q0)/2;
423  nextP[2][12] = P[2][12] + P[0][12]*SF[6] + P[1][12]*SF[10] + P[3][12]*SF[8] + P[12][12]*SF[14] - P[10][12]*SPP[10] - (P[11][12]*q0)/2;
424  nextP[3][12] = P[3][12] + P[0][12]*SF[7] + P[1][12]*SF[6] + P[2][12]*SF[9] + P[10][12]*SF[15] - P[11][12]*SF[14] - (P[12][12]*q0)/2;
425  nextP[4][12] = P[4][12] + P[0][12]*SF[5] + P[1][12]*SF[3] - P[3][12]*SF[4] + P[2][12]*SPP[0] + P[13][12]*SPP[3] + P[14][12]*SPP[6] - P[15][12]*SPP[9];
426  nextP[5][12] = P[5][12] + P[0][12]*SF[4] + P[2][12]*SF[3] + P[3][12]*SF[5] - P[1][12]*SPP[0] - P[13][12]*SPP[8] + P[14][12]*SPP[2] + P[15][12]*SPP[5];
427  nextP[6][12] = P[6][12] + P[1][12]*SF[4] - P[2][12]*SF[5] + P[3][12]*SF[3] + P[0][12]*SPP[0] + P[13][12]*SPP[4] - P[14][12]*SPP[7] - P[15][12]*SPP[1];
428  nextP[7][12] = P[7][12] + P[4][12]*dt;
429  nextP[8][12] = P[8][12] + P[5][12]*dt;
430  nextP[9][12] = P[9][12] + P[6][12]*dt;
431  nextP[10][12] = P[10][12];
432  nextP[11][12] = P[11][12];
433  nextP[12][12] = P[12][12];
434 
435  // add process noise that is not from the IMU
436  for (unsigned i = 0; i <= 9; i++) {
437  nextP[i][i] += process_noise[i];
438  }
439 
440  // process noise contribution for delta angle states can be very small compared to
441  // the variances, therefore use algorithm to minimise numerical error
442  for (unsigned i = 10; i <=12; i++) {
443  const int index = i-10;
444  nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_angle_bias_var_accum(index));
445  }
446 
447  // Don't calculate these covariance terms if IMU delta velocity bias estimation is inhibited
448  if (!(_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) && !_accel_bias_inhibit) {
449 
450  // calculate variances and upper diagonal covariances for IMU delta velocity bias states
451  nextP[0][13] = P[0][13] + P[1][13]*SF[9] + P[2][13]*SF[11] + P[3][13]*SF[10] + P[10][13]*SF[14] + P[11][13]*SF[15] + P[12][13]*SPP[10];
452  nextP[1][13] = P[1][13] + P[0][13]*SF[8] + P[2][13]*SF[7] + P[3][13]*SF[11] - P[12][13]*SF[15] + P[11][13]*SPP[10] - (P[10][13]*q0)/2;
453  nextP[2][13] = P[2][13] + P[0][13]*SF[6] + P[1][13]*SF[10] + P[3][13]*SF[8] + P[12][13]*SF[14] - P[10][13]*SPP[10] - (P[11][13]*q0)/2;
454  nextP[3][13] = P[3][13] + P[0][13]*SF[7] + P[1][13]*SF[6] + P[2][13]*SF[9] + P[10][13]*SF[15] - P[11][13]*SF[14] - (P[12][13]*q0)/2;
455  nextP[4][13] = P[4][13] + P[0][13]*SF[5] + P[1][13]*SF[3] - P[3][13]*SF[4] + P[2][13]*SPP[0] + P[13][13]*SPP[3] + P[14][13]*SPP[6] - P[15][13]*SPP[9];
456  nextP[5][13] = P[5][13] + P[0][13]*SF[4] + P[2][13]*SF[3] + P[3][13]*SF[5] - P[1][13]*SPP[0] - P[13][13]*SPP[8] + P[14][13]*SPP[2] + P[15][13]*SPP[5];
457  nextP[6][13] = P[6][13] + P[1][13]*SF[4] - P[2][13]*SF[5] + P[3][13]*SF[3] + P[0][13]*SPP[0] + P[13][13]*SPP[4] - P[14][13]*SPP[7] - P[15][13]*SPP[1];
458  nextP[7][13] = P[7][13] + P[4][13]*dt;
459  nextP[8][13] = P[8][13] + P[5][13]*dt;
460  nextP[9][13] = P[9][13] + P[6][13]*dt;
461  nextP[10][13] = P[10][13];
462  nextP[11][13] = P[11][13];
463  nextP[12][13] = P[12][13];
464  nextP[13][13] = P[13][13];
465  nextP[0][14] = P[0][14] + P[1][14]*SF[9] + P[2][14]*SF[11] + P[3][14]*SF[10] + P[10][14]*SF[14] + P[11][14]*SF[15] + P[12][14]*SPP[10];
466  nextP[1][14] = P[1][14] + P[0][14]*SF[8] + P[2][14]*SF[7] + P[3][14]*SF[11] - P[12][14]*SF[15] + P[11][14]*SPP[10] - (P[10][14]*q0)/2;
467  nextP[2][14] = P[2][14] + P[0][14]*SF[6] + P[1][14]*SF[10] + P[3][14]*SF[8] + P[12][14]*SF[14] - P[10][14]*SPP[10] - (P[11][14]*q0)/2;
468  nextP[3][14] = P[3][14] + P[0][14]*SF[7] + P[1][14]*SF[6] + P[2][14]*SF[9] + P[10][14]*SF[15] - P[11][14]*SF[14] - (P[12][14]*q0)/2;
469  nextP[4][14] = P[4][14] + P[0][14]*SF[5] + P[1][14]*SF[3] - P[3][14]*SF[4] + P[2][14]*SPP[0] + P[13][14]*SPP[3] + P[14][14]*SPP[6] - P[15][14]*SPP[9];
470  nextP[5][14] = P[5][14] + P[0][14]*SF[4] + P[2][14]*SF[3] + P[3][14]*SF[5] - P[1][14]*SPP[0] - P[13][14]*SPP[8] + P[14][14]*SPP[2] + P[15][14]*SPP[5];
471  nextP[6][14] = P[6][14] + P[1][14]*SF[4] - P[2][14]*SF[5] + P[3][14]*SF[3] + P[0][14]*SPP[0] + P[13][14]*SPP[4] - P[14][14]*SPP[7] - P[15][14]*SPP[1];
472  nextP[7][14] = P[7][14] + P[4][14]*dt;
473  nextP[8][14] = P[8][14] + P[5][14]*dt;
474  nextP[9][14] = P[9][14] + P[6][14]*dt;
475  nextP[10][14] = P[10][14];
476  nextP[11][14] = P[11][14];
477  nextP[12][14] = P[12][14];
478  nextP[13][14] = P[13][14];
479  nextP[14][14] = P[14][14];
480  nextP[0][15] = P[0][15] + P[1][15]*SF[9] + P[2][15]*SF[11] + P[3][15]*SF[10] + P[10][15]*SF[14] + P[11][15]*SF[15] + P[12][15]*SPP[10];
481  nextP[1][15] = P[1][15] + P[0][15]*SF[8] + P[2][15]*SF[7] + P[3][15]*SF[11] - P[12][15]*SF[15] + P[11][15]*SPP[10] - (P[10][15]*q0)/2;
482  nextP[2][15] = P[2][15] + P[0][15]*SF[6] + P[1][15]*SF[10] + P[3][15]*SF[8] + P[12][15]*SF[14] - P[10][15]*SPP[10] - (P[11][15]*q0)/2;
483  nextP[3][15] = P[3][15] + P[0][15]*SF[7] + P[1][15]*SF[6] + P[2][15]*SF[9] + P[10][15]*SF[15] - P[11][15]*SF[14] - (P[12][15]*q0)/2;
484  nextP[4][15] = P[4][15] + P[0][15]*SF[5] + P[1][15]*SF[3] - P[3][15]*SF[4] + P[2][15]*SPP[0] + P[13][15]*SPP[3] + P[14][15]*SPP[6] - P[15][15]*SPP[9];
485  nextP[5][15] = P[5][15] + P[0][15]*SF[4] + P[2][15]*SF[3] + P[3][15]*SF[5] - P[1][15]*SPP[0] - P[13][15]*SPP[8] + P[14][15]*SPP[2] + P[15][15]*SPP[5];
486  nextP[6][15] = P[6][15] + P[1][15]*SF[4] - P[2][15]*SF[5] + P[3][15]*SF[3] + P[0][15]*SPP[0] + P[13][15]*SPP[4] - P[14][15]*SPP[7] - P[15][15]*SPP[1];
487  nextP[7][15] = P[7][15] + P[4][15]*dt;
488  nextP[8][15] = P[8][15] + P[5][15]*dt;
489  nextP[9][15] = P[9][15] + P[6][15]*dt;
490  nextP[10][15] = P[10][15];
491  nextP[11][15] = P[11][15];
492  nextP[12][15] = P[12][15];
493  nextP[13][15] = P[13][15];
494  nextP[14][15] = P[14][15];
495  nextP[15][15] = P[15][15];
496 
497  // add process noise that is not from the IMU
498  // process noise contributiton for delta velocity states can be very small compared to
499  // the variances, therefore use algorithm to minimise numerical error
500  for (unsigned i = 13; i <= 15; i++) {
501  const int index = i-13;
502  nextP[i][i] = kahanSummation(nextP[i][i], process_noise[i], _delta_vel_bias_var_accum(index));
503  }
504 
505  } else {
506  // Inhibit delta velocity bias learning by zeroing the covariance terms
507  zeroRows(nextP, 13, 15);
508  zeroCols(nextP, 13, 15);
509  _delta_vel_bias_var_accum.setZero();
510  }
511 
512  // Don't do covariance prediction on magnetic field states unless we are using 3-axis fusion
513  if (_control_status.flags.mag_3D) {
514  // calculate variances and upper diagonal covariances for earth and body magnetic field states
515  nextP[0][16] = P[0][16] + P[1][16]*SF[9] + P[2][16]*SF[11] + P[3][16]*SF[10] + P[10][16]*SF[14] + P[11][16]*SF[15] + P[12][16]*SPP[10];
516  nextP[1][16] = P[1][16] + P[0][16]*SF[8] + P[2][16]*SF[7] + P[3][16]*SF[11] - P[12][16]*SF[15] + P[11][16]*SPP[10] - (P[10][16]*q0)/2;
517  nextP[2][16] = P[2][16] + P[0][16]*SF[6] + P[1][16]*SF[10] + P[3][16]*SF[8] + P[12][16]*SF[14] - P[10][16]*SPP[10] - (P[11][16]*q0)/2;
518  nextP[3][16] = P[3][16] + P[0][16]*SF[7] + P[1][16]*SF[6] + P[2][16]*SF[9] + P[10][16]*SF[15] - P[11][16]*SF[14] - (P[12][16]*q0)/2;
519  nextP[4][16] = P[4][16] + P[0][16]*SF[5] + P[1][16]*SF[3] - P[3][16]*SF[4] + P[2][16]*SPP[0] + P[13][16]*SPP[3] + P[14][16]*SPP[6] - P[15][16]*SPP[9];
520  nextP[5][16] = P[5][16] + P[0][16]*SF[4] + P[2][16]*SF[3] + P[3][16]*SF[5] - P[1][16]*SPP[0] - P[13][16]*SPP[8] + P[14][16]*SPP[2] + P[15][16]*SPP[5];
521  nextP[6][16] = P[6][16] + P[1][16]*SF[4] - P[2][16]*SF[5] + P[3][16]*SF[3] + P[0][16]*SPP[0] + P[13][16]*SPP[4] - P[14][16]*SPP[7] - P[15][16]*SPP[1];
522  nextP[7][16] = P[7][16] + P[4][16]*dt;
523  nextP[8][16] = P[8][16] + P[5][16]*dt;
524  nextP[9][16] = P[9][16] + P[6][16]*dt;
525  nextP[10][16] = P[10][16];
526  nextP[11][16] = P[11][16];
527  nextP[12][16] = P[12][16];
528  nextP[13][16] = P[13][16];
529  nextP[14][16] = P[14][16];
530  nextP[15][16] = P[15][16];
531  nextP[16][16] = P[16][16];
532  nextP[0][17] = P[0][17] + P[1][17]*SF[9] + P[2][17]*SF[11] + P[3][17]*SF[10] + P[10][17]*SF[14] + P[11][17]*SF[15] + P[12][17]*SPP[10];
533  nextP[1][17] = P[1][17] + P[0][17]*SF[8] + P[2][17]*SF[7] + P[3][17]*SF[11] - P[12][17]*SF[15] + P[11][17]*SPP[10] - (P[10][17]*q0)/2;
534  nextP[2][17] = P[2][17] + P[0][17]*SF[6] + P[1][17]*SF[10] + P[3][17]*SF[8] + P[12][17]*SF[14] - P[10][17]*SPP[10] - (P[11][17]*q0)/2;
535  nextP[3][17] = P[3][17] + P[0][17]*SF[7] + P[1][17]*SF[6] + P[2][17]*SF[9] + P[10][17]*SF[15] - P[11][17]*SF[14] - (P[12][17]*q0)/2;
536  nextP[4][17] = P[4][17] + P[0][17]*SF[5] + P[1][17]*SF[3] - P[3][17]*SF[4] + P[2][17]*SPP[0] + P[13][17]*SPP[3] + P[14][17]*SPP[6] - P[15][17]*SPP[9];
537  nextP[5][17] = P[5][17] + P[0][17]*SF[4] + P[2][17]*SF[3] + P[3][17]*SF[5] - P[1][17]*SPP[0] - P[13][17]*SPP[8] + P[14][17]*SPP[2] + P[15][17]*SPP[5];
538  nextP[6][17] = P[6][17] + P[1][17]*SF[4] - P[2][17]*SF[5] + P[3][17]*SF[3] + P[0][17]*SPP[0] + P[13][17]*SPP[4] - P[14][17]*SPP[7] - P[15][17]*SPP[1];
539  nextP[7][17] = P[7][17] + P[4][17]*dt;
540  nextP[8][17] = P[8][17] + P[5][17]*dt;
541  nextP[9][17] = P[9][17] + P[6][17]*dt;
542  nextP[10][17] = P[10][17];
543  nextP[11][17] = P[11][17];
544  nextP[12][17] = P[12][17];
545  nextP[13][17] = P[13][17];
546  nextP[14][17] = P[14][17];
547  nextP[15][17] = P[15][17];
548  nextP[16][17] = P[16][17];
549  nextP[17][17] = P[17][17];
550  nextP[0][18] = P[0][18] + P[1][18]*SF[9] + P[2][18]*SF[11] + P[3][18]*SF[10] + P[10][18]*SF[14] + P[11][18]*SF[15] + P[12][18]*SPP[10];
551  nextP[1][18] = P[1][18] + P[0][18]*SF[8] + P[2][18]*SF[7] + P[3][18]*SF[11] - P[12][18]*SF[15] + P[11][18]*SPP[10] - (P[10][18]*q0)/2;
552  nextP[2][18] = P[2][18] + P[0][18]*SF[6] + P[1][18]*SF[10] + P[3][18]*SF[8] + P[12][18]*SF[14] - P[10][18]*SPP[10] - (P[11][18]*q0)/2;
553  nextP[3][18] = P[3][18] + P[0][18]*SF[7] + P[1][18]*SF[6] + P[2][18]*SF[9] + P[10][18]*SF[15] - P[11][18]*SF[14] - (P[12][18]*q0)/2;
554  nextP[4][18] = P[4][18] + P[0][18]*SF[5] + P[1][18]*SF[3] - P[3][18]*SF[4] + P[2][18]*SPP[0] + P[13][18]*SPP[3] + P[14][18]*SPP[6] - P[15][18]*SPP[9];
555  nextP[5][18] = P[5][18] + P[0][18]*SF[4] + P[2][18]*SF[3] + P[3][18]*SF[5] - P[1][18]*SPP[0] - P[13][18]*SPP[8] + P[14][18]*SPP[2] + P[15][18]*SPP[5];
556  nextP[6][18] = P[6][18] + P[1][18]*SF[4] - P[2][18]*SF[5] + P[3][18]*SF[3] + P[0][18]*SPP[0] + P[13][18]*SPP[4] - P[14][18]*SPP[7] - P[15][18]*SPP[1];
557  nextP[7][18] = P[7][18] + P[4][18]*dt;
558  nextP[8][18] = P[8][18] + P[5][18]*dt;
559  nextP[9][18] = P[9][18] + P[6][18]*dt;
560  nextP[10][18] = P[10][18];
561  nextP[11][18] = P[11][18];
562  nextP[12][18] = P[12][18];
563  nextP[13][18] = P[13][18];
564  nextP[14][18] = P[14][18];
565  nextP[15][18] = P[15][18];
566  nextP[16][18] = P[16][18];
567  nextP[17][18] = P[17][18];
568  nextP[18][18] = P[18][18];
569  nextP[0][19] = P[0][19] + P[1][19]*SF[9] + P[2][19]*SF[11] + P[3][19]*SF[10] + P[10][19]*SF[14] + P[11][19]*SF[15] + P[12][19]*SPP[10];
570  nextP[1][19] = P[1][19] + P[0][19]*SF[8] + P[2][19]*SF[7] + P[3][19]*SF[11] - P[12][19]*SF[15] + P[11][19]*SPP[10] - (P[10][19]*q0)/2;
571  nextP[2][19] = P[2][19] + P[0][19]*SF[6] + P[1][19]*SF[10] + P[3][19]*SF[8] + P[12][19]*SF[14] - P[10][19]*SPP[10] - (P[11][19]*q0)/2;
572  nextP[3][19] = P[3][19] + P[0][19]*SF[7] + P[1][19]*SF[6] + P[2][19]*SF[9] + P[10][19]*SF[15] - P[11][19]*SF[14] - (P[12][19]*q0)/2;
573  nextP[4][19] = P[4][19] + P[0][19]*SF[5] + P[1][19]*SF[3] - P[3][19]*SF[4] + P[2][19]*SPP[0] + P[13][19]*SPP[3] + P[14][19]*SPP[6] - P[15][19]*SPP[9];
574  nextP[5][19] = P[5][19] + P[0][19]*SF[4] + P[2][19]*SF[3] + P[3][19]*SF[5] - P[1][19]*SPP[0] - P[13][19]*SPP[8] + P[14][19]*SPP[2] + P[15][19]*SPP[5];
575  nextP[6][19] = P[6][19] + P[1][19]*SF[4] - P[2][19]*SF[5] + P[3][19]*SF[3] + P[0][19]*SPP[0] + P[13][19]*SPP[4] - P[14][19]*SPP[7] - P[15][19]*SPP[1];
576  nextP[7][19] = P[7][19] + P[4][19]*dt;
577  nextP[8][19] = P[8][19] + P[5][19]*dt;
578  nextP[9][19] = P[9][19] + P[6][19]*dt;
579  nextP[10][19] = P[10][19];
580  nextP[11][19] = P[11][19];
581  nextP[12][19] = P[12][19];
582  nextP[13][19] = P[13][19];
583  nextP[14][19] = P[14][19];
584  nextP[15][19] = P[15][19];
585  nextP[16][19] = P[16][19];
586  nextP[17][19] = P[17][19];
587  nextP[18][19] = P[18][19];
588  nextP[19][19] = P[19][19];
589  nextP[0][20] = P[0][20] + P[1][20]*SF[9] + P[2][20]*SF[11] + P[3][20]*SF[10] + P[10][20]*SF[14] + P[11][20]*SF[15] + P[12][20]*SPP[10];
590  nextP[1][20] = P[1][20] + P[0][20]*SF[8] + P[2][20]*SF[7] + P[3][20]*SF[11] - P[12][20]*SF[15] + P[11][20]*SPP[10] - (P[10][20]*q0)/2;
591  nextP[2][20] = P[2][20] + P[0][20]*SF[6] + P[1][20]*SF[10] + P[3][20]*SF[8] + P[12][20]*SF[14] - P[10][20]*SPP[10] - (P[11][20]*q0)/2;
592  nextP[3][20] = P[3][20] + P[0][20]*SF[7] + P[1][20]*SF[6] + P[2][20]*SF[9] + P[10][20]*SF[15] - P[11][20]*SF[14] - (P[12][20]*q0)/2;
593  nextP[4][20] = P[4][20] + P[0][20]*SF[5] + P[1][20]*SF[3] - P[3][20]*SF[4] + P[2][20]*SPP[0] + P[13][20]*SPP[3] + P[14][20]*SPP[6] - P[15][20]*SPP[9];
594  nextP[5][20] = P[5][20] + P[0][20]*SF[4] + P[2][20]*SF[3] + P[3][20]*SF[5] - P[1][20]*SPP[0] - P[13][20]*SPP[8] + P[14][20]*SPP[2] + P[15][20]*SPP[5];
595  nextP[6][20] = P[6][20] + P[1][20]*SF[4] - P[2][20]*SF[5] + P[3][20]*SF[3] + P[0][20]*SPP[0] + P[13][20]*SPP[4] - P[14][20]*SPP[7] - P[15][20]*SPP[1];
596  nextP[7][20] = P[7][20] + P[4][20]*dt;
597  nextP[8][20] = P[8][20] + P[5][20]*dt;
598  nextP[9][20] = P[9][20] + P[6][20]*dt;
599  nextP[10][20] = P[10][20];
600  nextP[11][20] = P[11][20];
601  nextP[12][20] = P[12][20];
602  nextP[13][20] = P[13][20];
603  nextP[14][20] = P[14][20];
604  nextP[15][20] = P[15][20];
605  nextP[16][20] = P[16][20];
606  nextP[17][20] = P[17][20];
607  nextP[18][20] = P[18][20];
608  nextP[19][20] = P[19][20];
609  nextP[20][20] = P[20][20];
610  nextP[0][21] = P[0][21] + P[1][21]*SF[9] + P[2][21]*SF[11] + P[3][21]*SF[10] + P[10][21]*SF[14] + P[11][21]*SF[15] + P[12][21]*SPP[10];
611  nextP[1][21] = P[1][21] + P[0][21]*SF[8] + P[2][21]*SF[7] + P[3][21]*SF[11] - P[12][21]*SF[15] + P[11][21]*SPP[10] - (P[10][21]*q0)/2;
612  nextP[2][21] = P[2][21] + P[0][21]*SF[6] + P[1][21]*SF[10] + P[3][21]*SF[8] + P[12][21]*SF[14] - P[10][21]*SPP[10] - (P[11][21]*q0)/2;
613  nextP[3][21] = P[3][21] + P[0][21]*SF[7] + P[1][21]*SF[6] + P[2][21]*SF[9] + P[10][21]*SF[15] - P[11][21]*SF[14] - (P[12][21]*q0)/2;
614  nextP[4][21] = P[4][21] + P[0][21]*SF[5] + P[1][21]*SF[3] - P[3][21]*SF[4] + P[2][21]*SPP[0] + P[13][21]*SPP[3] + P[14][21]*SPP[6] - P[15][21]*SPP[9];
615  nextP[5][21] = P[5][21] + P[0][21]*SF[4] + P[2][21]*SF[3] + P[3][21]*SF[5] - P[1][21]*SPP[0] - P[13][21]*SPP[8] + P[14][21]*SPP[2] + P[15][21]*SPP[5];
616  nextP[6][21] = P[6][21] + P[1][21]*SF[4] - P[2][21]*SF[5] + P[3][21]*SF[3] + P[0][21]*SPP[0] + P[13][21]*SPP[4] - P[14][21]*SPP[7] - P[15][21]*SPP[1];
617  nextP[7][21] = P[7][21] + P[4][21]*dt;
618  nextP[8][21] = P[8][21] + P[5][21]*dt;
619  nextP[9][21] = P[9][21] + P[6][21]*dt;
620  nextP[10][21] = P[10][21];
621  nextP[11][21] = P[11][21];
622  nextP[12][21] = P[12][21];
623  nextP[13][21] = P[13][21];
624  nextP[14][21] = P[14][21];
625  nextP[15][21] = P[15][21];
626  nextP[16][21] = P[16][21];
627  nextP[17][21] = P[17][21];
628  nextP[18][21] = P[18][21];
629  nextP[19][21] = P[19][21];
630  nextP[20][21] = P[20][21];
631  nextP[21][21] = P[21][21];
632 
633  // add process noise that is not from the IMU
634  for (unsigned i = 16; i <= 21; i++) {
635  nextP[i][i] += process_noise[i];
636  }
637 
638  }
639 
640  // Don't do covariance prediction on wind states unless we are using them
641  if (_control_status.flags.wind) {
642 
643  // calculate variances and upper diagonal covariances for wind states
644  nextP[0][22] = P[0][22] + P[1][22]*SF[9] + P[2][22]*SF[11] + P[3][22]*SF[10] + P[10][22]*SF[14] + P[11][22]*SF[15] + P[12][22]*SPP[10];
645  nextP[1][22] = P[1][22] + P[0][22]*SF[8] + P[2][22]*SF[7] + P[3][22]*SF[11] - P[12][22]*SF[15] + P[11][22]*SPP[10] - (P[10][22]*q0)/2;
646  nextP[2][22] = P[2][22] + P[0][22]*SF[6] + P[1][22]*SF[10] + P[3][22]*SF[8] + P[12][22]*SF[14] - P[10][22]*SPP[10] - (P[11][22]*q0)/2;
647  nextP[3][22] = P[3][22] + P[0][22]*SF[7] + P[1][22]*SF[6] + P[2][22]*SF[9] + P[10][22]*SF[15] - P[11][22]*SF[14] - (P[12][22]*q0)/2;
648  nextP[4][22] = P[4][22] + P[0][22]*SF[5] + P[1][22]*SF[3] - P[3][22]*SF[4] + P[2][22]*SPP[0] + P[13][22]*SPP[3] + P[14][22]*SPP[6] - P[15][22]*SPP[9];
649  nextP[5][22] = P[5][22] + P[0][22]*SF[4] + P[2][22]*SF[3] + P[3][22]*SF[5] - P[1][22]*SPP[0] - P[13][22]*SPP[8] + P[14][22]*SPP[2] + P[15][22]*SPP[5];
650  nextP[6][22] = P[6][22] + P[1][22]*SF[4] - P[2][22]*SF[5] + P[3][22]*SF[3] + P[0][22]*SPP[0] + P[13][22]*SPP[4] - P[14][22]*SPP[7] - P[15][22]*SPP[1];
651  nextP[7][22] = P[7][22] + P[4][22]*dt;
652  nextP[8][22] = P[8][22] + P[5][22]*dt;
653  nextP[9][22] = P[9][22] + P[6][22]*dt;
654  nextP[10][22] = P[10][22];
655  nextP[11][22] = P[11][22];
656  nextP[12][22] = P[12][22];
657  nextP[13][22] = P[13][22];
658  nextP[14][22] = P[14][22];
659  nextP[15][22] = P[15][22];
660  nextP[16][22] = P[16][22];
661  nextP[17][22] = P[17][22];
662  nextP[18][22] = P[18][22];
663  nextP[19][22] = P[19][22];
664  nextP[20][22] = P[20][22];
665  nextP[21][22] = P[21][22];
666  nextP[22][22] = P[22][22];
667  nextP[0][23] = P[0][23] + P[1][23]*SF[9] + P[2][23]*SF[11] + P[3][23]*SF[10] + P[10][23]*SF[14] + P[11][23]*SF[15] + P[12][23]*SPP[10];
668  nextP[1][23] = P[1][23] + P[0][23]*SF[8] + P[2][23]*SF[7] + P[3][23]*SF[11] - P[12][23]*SF[15] + P[11][23]*SPP[10] - (P[10][23]*q0)/2;
669  nextP[2][23] = P[2][23] + P[0][23]*SF[6] + P[1][23]*SF[10] + P[3][23]*SF[8] + P[12][23]*SF[14] - P[10][23]*SPP[10] - (P[11][23]*q0)/2;
670  nextP[3][23] = P[3][23] + P[0][23]*SF[7] + P[1][23]*SF[6] + P[2][23]*SF[9] + P[10][23]*SF[15] - P[11][23]*SF[14] - (P[12][23]*q0)/2;
671  nextP[4][23] = P[4][23] + P[0][23]*SF[5] + P[1][23]*SF[3] - P[3][23]*SF[4] + P[2][23]*SPP[0] + P[13][23]*SPP[3] + P[14][23]*SPP[6] - P[15][23]*SPP[9];
672  nextP[5][23] = P[5][23] + P[0][23]*SF[4] + P[2][23]*SF[3] + P[3][23]*SF[5] - P[1][23]*SPP[0] - P[13][23]*SPP[8] + P[14][23]*SPP[2] + P[15][23]*SPP[5];
673  nextP[6][23] = P[6][23] + P[1][23]*SF[4] - P[2][23]*SF[5] + P[3][23]*SF[3] + P[0][23]*SPP[0] + P[13][23]*SPP[4] - P[14][23]*SPP[7] - P[15][23]*SPP[1];
674  nextP[7][23] = P[7][23] + P[4][23]*dt;
675  nextP[8][23] = P[8][23] + P[5][23]*dt;
676  nextP[9][23] = P[9][23] + P[6][23]*dt;
677  nextP[10][23] = P[10][23];
678  nextP[11][23] = P[11][23];
679  nextP[12][23] = P[12][23];
680  nextP[13][23] = P[13][23];
681  nextP[14][23] = P[14][23];
682  nextP[15][23] = P[15][23];
683  nextP[16][23] = P[16][23];
684  nextP[17][23] = P[17][23];
685  nextP[18][23] = P[18][23];
686  nextP[19][23] = P[19][23];
687  nextP[20][23] = P[20][23];
688  nextP[21][23] = P[21][23];
689  nextP[22][23] = P[22][23];
690  nextP[23][23] = P[23][23];
691 
692  // add process noise that is not from the IMU
693  for (unsigned i = 22; i <= 23; i++) {
694  nextP[i][i] += process_noise[i];
695  }
696 
697  }
698 
699  // stop position covariance growth if our total position variance reaches 100m
700  // this can happen if we lose gps for some time
701  if ((P[7][7] + P[8][8]) > 1e4f) {
702  for (uint8_t i = 7; i <= 8; i++) {
703  for (uint8_t j = 0; j < _k_num_states; j++) {
704  nextP[i][j] = P[i][j];
705  nextP[j][i] = P[j][i];
706  }
707  }
708  }
709 
710  // covariance matrix is symmetrical, so copy upper half to lower half
711  for (unsigned row = 1; row < _k_num_states; row++) {
712  for (unsigned column = 0 ; column < row; column++) {
713  P[row][column] = P[column][row] = nextP[column][row];
714  }
715  }
716 
717  // copy variances (diagonals)
718  for (unsigned i = 0; i < _k_num_states; i++) {
719  P[i][i] = nextP[i][i];
720  }
721 
722  // fix gross errors in the covariance matrix and ensure rows and
723  // columns for un-used states are zero
724  fixCovarianceErrors();
725 
726 }
727 
728 void Ekf::fixCovarianceErrors()
729 {
730  // NOTE: This limiting is a last resort and should not be relied on
731  // TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions
732  // and set corresponding entries in Q to zero when states exceed 50% of the limit
733  // Covariance diagonal limits. Use same values for states which
734  // belong to the same group (e.g. vel_x, vel_y, vel_z)
735  float P_lim[8] = {};
736  P_lim[0] = 1.0f; // quaternion max var
737  P_lim[1] = 1e6f; // velocity max var
738  P_lim[2] = 1e6f; // positiion max var
739  P_lim[3] = 1.0f; // gyro bias max var
740  P_lim[4] = 1.0f; // delta velocity z bias max var
741  P_lim[5] = 1.0f; // earth mag field max var
742  P_lim[6] = 1.0f; // body mag field max var
743  P_lim[7] = 1e6f; // wind max var
744 
745  for (int i = 0; i <= 3; i++) {
746  // quaternion states
747  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]);
748  }
749 
750  for (int i = 4; i <= 6; i++) {
751  // NED velocity states
752  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]);
753  }
754 
755  for (int i = 7; i <= 9; i++) {
756  // NED position states
757  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]);
758  }
759 
760  for (int i = 10; i <= 12; i++) {
761  // gyro bias states
762  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]);
763  }
764 
765  // force symmetry on the quaternion, velocity and position state covariances
766  makeSymmetrical(P, 0, 12);
767 
768  // the following states are optional and are deactivated when not required
769  // by ensuring the corresponding covariance matrix values are kept at zero
770 
771  // accelerometer bias states
772  if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) {
773  zeroRows(P, 13, 15);
774  zeroCols(P, 13, 15);
775 
776  } else {
777  // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
778  const float minSafeStateVar = 1e-9f;
779  float maxStateVar = minSafeStateVar;
780  bool resetRequired = false;
781 
782  for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
783  if (P[stateIndex][stateIndex] > maxStateVar) {
784  maxStateVar = P[stateIndex][stateIndex];
785 
786  } else if (P[stateIndex][stateIndex] < minSafeStateVar) {
787  resetRequired = true;
788  }
789  }
790 
791  // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
792  // not exceed 100 and the minimum variance must not fall below the target minimum
793  // Also limit variance to a maximum equivalent to a 0.1g uncertainty
794  const float minStateVarTarget = 5E-8f;
795  float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
796 
797  for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
798  P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg));
799  }
800 
801  // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
802  if (resetRequired) {
803  float delVelBiasVar[3];
804 
805  // store all delta velocity bias variances
806  for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
807  delVelBiasVar[stateIndex - 13] = P[stateIndex][stateIndex];
808  }
809 
810  // reset all delta velocity bias covariances
811  zeroCols(P, 13, 15);
812 
813  // restore all delta velocity bias variances
814  for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
815  P[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13];
816  }
817  }
818 
819  // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
820  // calculate accel bias term aligned with the gravity vector
821  float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
822  float down_dvel_bias = 0.0f;
823 
824  for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
825  down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
826  }
827 
828  // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
829  bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim
830  && down_dvel_bias * _vel_pos_innov[2] < 0.0f
831  && down_dvel_bias * _vel_pos_innov[5] < 0.0f);
832 
833  // record the pass/fail
834  if (!bad_acc_bias) {
835  _fault_status.flags.bad_acc_bias = false;
836  _time_acc_bias_check = _time_last_imu;
837 
838  } else {
839  _fault_status.flags.bad_acc_bias = true;
840  }
841 
842  // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
843  // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
844  if (_time_last_imu - _time_acc_bias_check > (uint64_t)7e6) {
845  float varX = P[13][13];
846  float varY = P[14][14];
847  float varZ = P[15][15];
848  zeroRows(P, 13, 15);
849  zeroCols(P, 13, 15);
850  P[13][13] = varX;
851  P[14][14] = varY;
852  P[15][15] = varZ;
853  _time_acc_bias_check = _time_last_imu;
854  _fault_status.flags.bad_acc_bias = false;
855  ECL_WARN_TIMESTAMPED("EKF invalid accel bias - resetting covariance");
856 
857  } else {
858  // ensure the covariance values are symmetrical
859  makeSymmetrical(P, 13, 15);
860  }
861 
862  }
863 
864  // magnetic field states
865  if (!_control_status.flags.mag_3D) {
866  zeroRows(P, 16, 21);
867  zeroCols(P, 16, 21);
868 
869  } else {
870  // constrain variances
871  for (int i = 16; i <= 18; i++) {
872  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]);
873  }
874 
875  for (int i = 19; i <= 21; i++) {
876  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]);
877  }
878 
879  // force symmetry
880  makeSymmetrical(P, 16, 21);
881  }
882 
883  // wind velocity states
884  if (!_control_status.flags.wind) {
885  zeroRows(P, 22, 23);
886  zeroCols(P, 22, 23);
887 
888  } else {
889  // constrain variances
890  for (int i = 22; i <= 23; i++) {
891  P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]);
892  }
893 
894  // force symmetry
895  makeSymmetrical(P, 22, 23);
896  }
897 }
898 
899 void Ekf::resetMagRelatedCovariances()
900 {
901  // set the quaternion covariance terms to zero
902  zeroRows(P, 0, 3);
903  zeroCols(P, 0, 3);
904 
905  // set the magnetic field covariance terms to zero
906  clearMagCov();
907 
908  // set the field state variance to the observation variance
909  for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
910  P[rc_index][rc_index] = sq(_params.mag_noise);
911  }
912 
913  // save covariance data for re-use when auto-switching between heading and 3-axis fusion
914  saveMagCovData();
915 }
916 
917 void Ekf::clearMagCov()
918 {
919  zeroMagCov();
920  _mag_decl_cov_reset = false;
921 }
922 
923 void Ekf::zeroMagCov()
924 {
925  zeroRows(P, 16, 21);
926  zeroCols(P, 16, 21);
927 }
928 
929 void Ekf::resetWindCovariance()
930 {
931  // set the wind covariance terms to zero
932  zeroRows(P, 22, 23);
933  zeroCols(P, 22, 23);
934 
935  if (_tas_data_ready && (_imu_sample_delayed.time_us - _airspeed_sample_delayed.time_us < (uint64_t)5e5)) {
936  // Use airspeed and zer sideslip assumption to set initial covariance values for wind states
937 
938  // calculate the wind speed and bearing
939  float spd = sqrtf(sq(_state.wind_vel(0)) + sq(_state.wind_vel(1)));
940  float yaw = atan2f(_state.wind_vel(1), _state.wind_vel(0));
941 
942  // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
943  // used to calculate the initial wind speed
944  float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
945  float R_yaw = sq(math::radians(10.0f));
946 
947  // calculate the variance and covariance terms for the wind states
948  float cos_yaw = cosf(yaw);
949  float sin_yaw = sinf(yaw);
950  float cos_yaw_2 = sq(cos_yaw);
951  float sin_yaw_2 = sq(sin_yaw);
952  float sin_cos_yaw = sin_yaw * cos_yaw;
953  float spd_2 = sq(spd);
954  P[22][22] = R_yaw * spd_2 * sin_yaw_2 + R_spd * cos_yaw_2;
955  P[22][23] = - R_yaw * sin_cos_yaw * spd_2 + R_spd * sin_cos_yaw;
956  P[23][22] = P[22][23];
957  P[23][23] = R_yaw * spd_2 * cos_yaw_2 + R_spd * sin_yaw_2;
958 
959  // Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
960  P[22][22] += P[4][4];
961  P[23][23] += P[5][5];
962 
963  } else {
964  // without airspeed, start with a small initial uncertainty to improve the initial estimate
965  P[22][22] = sq(1.0f);
966  P[23][23] = sq(1.0f);
967 
968  }
969 }
math::constrain
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
estimator::filter_control_status_u::flags
struct estimator::filter_control_status_u::@60 flags
estimator::imuSample::delta_ang
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
Definition: common.h:107
estimator::parameters::switch_on_gyro_bias
float switch_on_gyro_bias
1-sigma gyro bias uncertainty at switch on (rad/sec)
Definition: common.h:252
Ekf::clearMagCov
void clearMagCov()
Definition: covariance.cpp:917
ecl.h
Adapter / shim layer for system calls needed by ECL.
Ekf::_state
stateSample _state
state struct of the ekf running at the delayed time horizon
Definition: ekf.h:288
Ekf::initialiseQuatCovariances
void initialiseQuatCovariances(Vector3f &rot_vec_var)
Definition: ekf_helper.cpp:1433
BADACC_BIAS_PNOISE
#define BADACC_BIAS_PNOISE
The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) ...
Definition: common.h:215
MASK_INHIBIT_ACC_BIAS
#define MASK_INHIBIT_ACC_BIAS
set to true to inhibit estimation of accelerometer delta velocity bias
Definition: common.h:191
Ekf::_vel_pos_innov
float _vel_pos_innov[6]
NED velocity and position innovations: 0-2 vel (m/sec), 3-5 pos (m)
Definition: ekf.h:372
estimator::stateSample::accel_bias
Vector3f accel_bias
delta velocity bias estimate in m/s
Definition: common.h:372
Ekf::get_pos_var
void get_pos_var(Vector3f &pos_var) override
Definition: covariance.cpp:121
Ekf::_delta_vel_bias_var_accum
Vector3f _delta_vel_bias_var_accum
kahan summation algorithm accumulator for delta velocity bias variance
Definition: ekf.h:369
Ekf::_last_imu_bias_cov_reset_us
uint64_t _last_imu_bias_cov_reset_us
time the last reset of IMU delta angle and velocity state covariances was performed (uSec) ...
Definition: ekf.h:344
SQ
float SQ[4]
Definition: YawCovariance.c:16
estimator::parameters::acc_bias_learn_acc_lim
float acc_bias_learn_acc_lim
learning is disabled if the magnitude of the IMU acceleration vector is greater than this (m/sec**2) ...
Definition: common.h:337
estimator::parameters::gps_pos_noise
float gps_pos_noise
minimum allowed observation noise for gps position fusion (m)
Definition: common.h:259
estimator::filter_control_status_u::rng_hgt
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
Definition: common.h:451
SG
float SG[3]
Definition: YawCovariance.c:11
Ekf::initialiseCovariance
void initialiseCovariance()
Definition: covariance.cpp:49
estimator::parameters::accel_bias_p_noise
float accel_bias_p_noise
process noise for IMU accelerometer bias prediction (m/sec**3)
Definition: common.h:243
EstimatorInterface::_airspeed_sample_delayed
airspeedSample _airspeed_sample_delayed
Definition: estimator_interface.h:445
Ekf::_delta_angle_bias_var_accum
Vector3f _delta_angle_bias_var_accum
kahan summation algorithm accumulator for delta angle bias variance
Definition: ekf.h:370
Ekf::kahanSummation
float kahanSummation(float sum_previous, float input, float &accumulator) const
Definition: ekf_helper.cpp:1762
Ekf::_mag_decl_cov_reset
bool _mag_decl_cov_reset
true after the fuseDeclination() function has been used to modify the earth field covariances after a...
Definition: ekf.h:364
Ekf::zeroMagCov
void zeroMagCov()
Definition: covariance.cpp:923
estimator::fault_status_u::flags
struct estimator::fault_status_u::@57 flags
estimator::gpsSample::vacc
float vacc
1-std vertical position error (m)
Definition: common.h:120
estimator::parameters::switch_on_accel_bias
float switch_on_accel_bias
1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
Definition: common.h:253
estimator::parameters::gps_vel_noise
float gps_vel_noise
minimum allowed observation noise for gps velocity fusion (m/sec)
Definition: common.h:258
estimator::parameters::fusion_mode
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
Definition: common.h:222
estimator::parameters::wind_vel_p_noise_scaler
float wind_vel_p_noise_scaler
scaling of wind process noise with vertical velocity
Definition: common.h:247
Ekf::P
float P[_k_num_states][_k_num_states]
state covariance matrix
Definition: ekf.h:367
Ekf::zeroCols
void zeroCols(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:1249
CONSTANTS_ONE_G
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
Ekf::_accel_bias_inhibit
bool _accel_bias_inhibit
true when the accel bias learning is being inhibited
Definition: ekf.h:450
estimator::parameters::eas_noise
float eas_noise
EAS measurement noise standard deviation used for airspeed fusion (m/s)
Definition: common.h:281
Ekf::resetMagRelatedCovariances
void resetMagRelatedCovariances()
Definition: covariance.cpp:899
Ekf::saveMagCovData
void saveMagCovData()
Definition: ekf_helper.cpp:1733
estimator::parameters::mage_p_noise
float mage_p_noise
process noise for earth magnetic field prediction (Gauss/sec)
Definition: common.h:244
Ekf::_prev_dvel_bias_var
Vector3f _prev_dvel_bias_var
saved delta velocity XYZ bias variances (m/sec)**2
Definition: ekf.h:454
Ekf::_R_to_earth
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
Definition: ekf.h:348
estimator::airspeedSample::time_us
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:144
estimator::fault_status_u::bad_acc_bias
bool bad_acc_bias
15 - true if bad delta velocity bias estimates have been detected
Definition: common.h:395
estimator::parameters::wind_vel_p_noise
float wind_vel_p_noise
process noise for wind velocity prediction (m/sec**2)
Definition: common.h:246
estimator::parameters::baro_noise
float baro_noise
observation noise for barometric height fusion (m)
Definition: common.h:261
estimator::parameters::acc_bias_lim
float acc_bias_lim
maximum accel bias magnitude (m/sec**2)
Definition: common.h:336
estimator::parameters::mag_noise
float mag_noise
measurement noise used for 3-axis magnetoemeter fusion (Gauss)
Definition: common.h:270
estimator::parameters::initial_wind_uncertainty
float initial_wind_uncertainty
1-sigma initial uncertainty in wind velocity (m/sec)
Definition: common.h:255
estimator::airspeedSample::eas2tas
float eas2tas
equivalent to true airspeed factor
Definition: common.h:143
estimator::imuSample::time_us
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
math::radians
constexpr T radians(T degrees)
Definition: Limits.hpp:85
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
EstimatorInterface::_control_status
filter_control_status_u _control_status
Definition: estimator_interface.h:564
estimator::stateSample::vel
Vector3f vel
NED velocity in earth frame in m/s.
Definition: common.h:369
estimator::parameters::acc_bias_learn_gyr_lim
float acc_bias_learn_gyr_lim
learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) ...
Definition: common.h:338
estimator::parameters::accel_noise
float accel_noise
IMU acceleration noise use for covariance prediction (m/sec**2)
Definition: common.h:239
estimator::parameters::acc_bias_learn_tc
float acc_bias_learn_tc
time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes ...
Definition: common.h:339
estimator::parameters::gyro_bias_p_noise
float gyro_bias_p_noise
process noise for IMU rate gyro bias prediction (rad/sec**2)
Definition: common.h:242
estimator::parameters::range_noise
float range_noise
observation noise for range finder measurements (m)
Definition: common.h:289
Ekf::sq
static constexpr float sq(float var)
Definition: ekf.h:695
Ekf::_tas_data_ready
bool _tas_data_ready
true when new true airspeed data has fallen behind the fusion time horizon and is available to be fus...
Definition: ekf.h:323
estimator::parameters::gyro_noise
float gyro_noise
IMU angular rate noise used for covariance prediction (rad/sec)
Definition: common.h:238
EstimatorInterface::_fault_status
fault_status_u _fault_status
Definition: estimator_interface.h:551
Ekf::fixCovarianceErrors
void fixCovarianceErrors()
Definition: covariance.cpp:728
Ekf::_time_acc_bias_check
uint64_t _time_acc_bias_check
last time the accel bias check passed (uSec)
Definition: ekf.h:341
ECL_WARN_TIMESTAMPED
#define ECL_WARN_TIMESTAMPED
Definition: ecl.h:94
estimator::parameters::initial_tilt_err
float initial_tilt_err
1-sigma tilt error after initial alignment using gravity vector (rad)
Definition: common.h:254
estimator::stateSample::quat_nominal
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
Ekf::resetWindCovariance
void resetWindCovariance()
Definition: covariance.cpp:929
Ekf::get_vel_var
void get_vel_var(Vector3f &vel_var) override
Definition: covariance.cpp:128
matrix::Vector3f
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Ekf::makeSymmetrical
void makeSymmetrical(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:773
Ekf::_accel_mag_filt
float _accel_mag_filt
acceleration magnitude after application of a decaying envelope filter (rad/sec)
Definition: ekf.h:452
estimator::filter_control_status_u::mag_3D
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
Definition: common.h:446
Ekf::zeroRows
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:1239
dt
float dt
Definition: px4io.c:73
estimator::filter_control_status_u::wind
uint32_t wind
8 - true when wind velocity is being estimated
Definition: common.h:449
estimator::filter_control_status_u::gps_hgt
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
Definition: common.h:452
Ekf::predictCovariance
void predictCovariance()
Definition: covariance.cpp:135
Ekf::_ang_rate_mag_filt
float _ang_rate_mag_filt
angular rate magnitude after application of a decaying envelope filter (rad/sec)
Definition: ekf.h:453
estimator::parameters::magb_p_noise
float magb_p_noise
process noise for body magnetic field prediction (Gauss/sec)
Definition: common.h:245
estimator::parameters::pos_noaid_noise
float pos_noaid_noise
observation noise for non-aiding position fusion (m)
Definition: common.h:260
EstimatorInterface::FILTER_UPDATE_PERIOD_S
static constexpr float FILTER_UPDATE_PERIOD_S
Definition: estimator_interface.h:411
Ekf::_bad_vert_accel_detected
bool _bad_vert_accel_detected
true when bad vertical accelerometer data has been detected
Definition: ekf.h:479
estimator::stateSample::wind_vel
Vector2f wind_vel
wind velocity in m/s
Definition: common.h:375
estimator::imuSample::delta_vel
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Definition: common.h:108
Ekf::_k_num_states
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
Ekf::_accel_vec_filt
Vector3f _accel_vec_filt
acceleration vector after application of a low pass filter (m/sec**2)
Definition: ekf.h:451
estimator::stateSample::gyro_bias
Vector3f gyro_bias
delta angle bias estimate in rad
Definition: common.h:371
Ekf::_dt_ekf_avg
float _dt_ekf_avg
average update rate of the ekf
Definition: ekf.h:285
estimator::imuSample::delta_ang_dt
float delta_ang_dt
delta angle integration period (sec)
Definition: common.h:109
ekf.h
Class for core functions for ekf attitude and position estimator.
Ekf::_height_rate_lpf
float _height_rate_lpf
Definition: ekf.h:489