APM:Libraries
AP_NavEKF3_core.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 #include <GCS_MAVLink/GCS.h>
10 
11 extern const AP_HAL::HAL& hal;
12 
13 // constructor
14 NavEKF3_core::NavEKF3_core(void) :
15  stateStruct(*reinterpret_cast<struct state_elements *>(&statesArray)),
16 
17  _perf_UpdateFilter(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_UpdateFilter")),
18  _perf_CovariancePrediction(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_CovariancePrediction")),
19  _perf_FuseVelPosNED(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseVelPosNED")),
20  _perf_FuseMagnetometer(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseMagnetometer")),
21  _perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseAirspeed")),
22  _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseSideslip")),
23  _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")),
24  _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")),
25  _perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom"))
26 {
27  _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0");
28  _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1");
29  _perf_test[2] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test2");
30  _perf_test[3] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test3");
31  _perf_test[4] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test4");
32  _perf_test[5] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test5");
33  _perf_test[6] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test6");
34  _perf_test[7] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test7");
35  _perf_test[8] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test8");
36  _perf_test[9] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test9");
37  firstInitTime_ms = 0;
38  lastInitFailReport_ms = 0;
39 }
40 
41 // setup this core backend
42 bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index)
43 {
44  frontend = _frontend;
45  imu_index = _imu_index;
46  core_index = _core_index;
47  _ahrs = frontend->_ahrs;
48 
49  /*
50  The imu_buffer_length needs to cope with the worst case sensor delay at the
51  maximum fusion rate of 100Hz. Non-imu data coming in at faster than 100Hz is
52  downsampled. For 50Hz main loop rate we need a shorter buffer.
53  */
54 
55  // Calculate the expected EKF time step
56  if (AP::ins().get_sample_rate() > 0) {
57  dtEkfAvg = 1.0f / AP::ins().get_sample_rate();
58  dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT);
59  } else {
60  return false;
61  }
62 
63  // find the maximum time delay for all potential sensors
64  uint16_t maxTimeDelay_ms = MAX(_frontend->_hgtDelay_ms ,
65  MAX(_frontend->_flowDelay_ms ,
66  MAX(_frontend->_rngBcnDelay_ms ,
67  MAX(_frontend->magDelay_ms ,
68  (uint16_t)(EKF_TARGET_DT_MS)
69  ))));
70 
71  // GPS sensing can have large delays and should not be included if disabled
72  if (_frontend->_fusionModeGPS != 3) {
73  // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
74  float gps_delay_sec = 0;
75  if (!AP::gps().get_lag(gps_delay_sec)) {
76  if (AP_HAL::millis() - lastInitFailReport_ms > 10000) {
77  lastInitFailReport_ms = AP_HAL::millis();
78  // provide an escalating series of messages
79  if (AP_HAL::millis() > 30000) {
80  gcs().send_text(MAV_SEVERITY_ERROR, "EKF3 waiting for GPS config data");
81  } else if (AP_HAL::millis() > 15000) {
82  gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 waiting for GPS config data");
83  } else {
84  gcs().send_text(MAV_SEVERITY_INFO, "EKF3 waiting for GPS config data");
85  }
86  }
87  return false;
88  }
89  // limit the time delay value from the GPS library to a max of 250 msec which is the max value the EKF has been tested for.
90  maxTimeDelay_ms = MAX(maxTimeDelay_ms , MIN((uint16_t)(gps_delay_sec * 1000.0f),250));
91  }
92 
93  // airspeed sensing can have large delays and should not be included if disabled
94  if (_ahrs->airspeed_sensor_enabled()) {
95  maxTimeDelay_ms = MAX(maxTimeDelay_ms , _frontend->tasDelay_ms);
96  }
97 
98  // calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
99  imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_MS)) + 1;
100 
101  // set the observation buffer length to handle the minimum time of arrival between observations in combination
102  // with the worst case delay from current time to ekf fusion time
103  // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
104  uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilf((float)maxTimeDelay_ms * 0.5f));
105  obs_buffer_length = (ekf_delay_ms / _frontend->sensorIntervalMin_ms) + 1;
106 
107  // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
108  obs_buffer_length = MIN(obs_buffer_length,imu_buffer_length);
109 
110  if(!storedGPS.init(obs_buffer_length)) {
111  return false;
112  }
113  if(!storedMag.init(obs_buffer_length)) {
114  return false;
115  }
116  if(!storedBaro.init(obs_buffer_length)) {
117  return false;
118  }
119  if(!storedTAS.init(obs_buffer_length)) {
120  return false;
121  }
122  if(!storedOF.init(obs_buffer_length)) {
123  return false;
124  }
125  if(!storedBodyOdm.init(obs_buffer_length)) {
126  return false;
127  }
128  if(!storedWheelOdm.init(imu_buffer_length)) { // initialise to same length of IMU to allow for multiple wheel sensors
129  return false;
130  }
131  // Note: the use of dual range finders potentially doubles the amount of data to be stored
132  if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
133  return false;
134  }
135  // Note: range beacon data is read one beacon at a time and can arrive at a high rate
136  if(!storedRangeBeacon.init(imu_buffer_length)) {
137  return false;
138  }
139  if(!storedIMU.init(imu_buffer_length)) {
140  return false;
141  }
142  if(!storedOutput.init(imu_buffer_length)) {
143  return false;
144  }
145  gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u buffers, IMU=%u , OBS=%u , dt=%6.4f",(unsigned)imu_index,(unsigned)imu_buffer_length,(unsigned)obs_buffer_length,(double)dtEkfAvg);
146  return true;
147 }
148 
149 
150 /********************************************************
151 * INIT FUNCTIONS *
152 ********************************************************/
153 
154 // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
155 void NavEKF3_core::InitialiseVariables()
156 {
157  // calculate the nominal filter update rate
158  const AP_InertialSensor &ins = AP::ins();
159  localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
160  localFilterTimeStep_ms = MAX(localFilterTimeStep_ms, (uint8_t)EKF_TARGET_DT_MS);
161 
162  // initialise time stamps
163  imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
164  lastHealthyMagTime_ms = imuSampleTime_ms;
165  prevTasStep_ms = imuSampleTime_ms;
166  prevBetaStep_ms = imuSampleTime_ms;
167  lastMagUpdate_us = 0;
168  lastBaroReceived_ms = imuSampleTime_ms;
169  lastVelPassTime_ms = 0;
170  lastPosPassTime_ms = 0;
171  lastHgtPassTime_ms = 0;
172  lastTasPassTime_ms = 0;
173  lastSynthYawTime_ms = imuSampleTime_ms;
174  lastTimeGpsReceived_ms = 0;
175  secondLastGpsTime_ms = 0;
176  lastDecayTime_ms = imuSampleTime_ms;
177  timeAtLastAuxEKF_ms = imuSampleTime_ms;
178  flowValidMeaTime_ms = imuSampleTime_ms;
179  rngValidMeaTime_ms = imuSampleTime_ms;
180  flowMeaTime_ms = 0;
181  prevFlowFuseTime_ms = 0;
182  gndHgtValidTime_ms = 0;
183  ekfStartTime_ms = imuSampleTime_ms;
184  lastGpsVelFail_ms = 0;
185  lastGpsAidBadTime_ms = 0;
186  timeTasReceived_ms = 0;
187  magYawResetTimer_ms = imuSampleTime_ms;
188  lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;
189  lastPosReset_ms = 0;
190  lastVelReset_ms = 0;
191  lastPosResetD_ms = 0;
192  lastRngMeasTime_ms = 0;
193  terrainHgtStableSet_ms = 0;
194 
195  // initialise other variables
196  gpsNoiseScaler = 1.0f;
197  hgtTimeout = true;
198  magTimeout = false;
199  allMagSensorsFailed = false;
200  tasTimeout = true;
201  badMagYaw = false;
202  badIMUdata = false;
203  finalInflightYawInit = false;
204  finalInflightMagInit = false;
205  dtIMUavg = ins.get_loop_delta_t();
206  dtEkfAvg = EKF_TARGET_DT;
207  dt = 0;
208  velDotNEDfilt.zero();
209  lastKnownPositionNE.zero();
210  prevTnb.zero();
211  memset(&P[0][0], 0, sizeof(P));
212  memset(&nextP[0][0], 0, sizeof(nextP));
213  flowDataValid = false;
214  rangeDataToFuse = false;
215  fuseOptFlowData = false;
216  Popt = 0.0f;
217  terrainState = 0.0f;
218  prevPosN = stateStruct.position.x;
219  prevPosE = stateStruct.position.y;
220  inhibitGndState = false;
221  flowGyroBias.x = 0;
222  flowGyroBias.y = 0;
223  heldVelNE.zero();
224  PV_AidingMode = AID_NONE;
225  PV_AidingModePrev = AID_NONE;
226  posTimeout = true;
227  velTimeout = true;
228  memset(&faultStatus, 0, sizeof(faultStatus));
229  hgtRate = 0.0f;
230  mag_state.q0 = 1;
231  mag_state.DCM.identity();
232  onGround = true;
233  prevOnGround = true;
234  inFlight = false;
235  prevInFlight = false;
236  manoeuvring = false;
237  inhibitWindStates = true;
238  inhibitMagStates = true;
239  inhibitDelVelBiasStates = true;
240  inhibitDelAngBiasStates = true;
241  gndOffsetValid = false;
242  validOrigin = false;
243  takeoffExpectedSet_ms = 0;
244  expectGndEffectTakeoff = false;
245  touchdownExpectedSet_ms = 0;
246  expectGndEffectTouchdown = false;
247  gpsSpdAccuracy = 0.0f;
248  gpsPosAccuracy = 0.0f;
249  gpsHgtAccuracy = 0.0f;
250  baroHgtOffset = 0.0f;
251  yawResetAngle = 0.0f;
252  lastYawReset_ms = 0;
253  tiltAlignComplete = false;
254  yawAlignComplete = false;
255  stateIndexLim = 23;
256  baroStoreIndex = 0;
257  rangeStoreIndex = 0;
258  magStoreIndex = 0;
259  last_gps_idx = 0;
260  tasStoreIndex = 0;
261  ofStoreIndex = 0;
262  delAngCorrection.zero();
263  velErrintegral.zero();
264  posErrintegral.zero();
265  gpsGoodToAlign = false;
266  gpsNotAvailable = true;
267  motorsArmed = false;
268  prevMotorsArmed = false;
269  innovationIncrement = 0;
270  lastInnovation = 0;
271  memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
272  gpsSpdAccPass = false;
273  ekfInnovationsPass = false;
274  sAccFilterState1 = 0.0f;
275  sAccFilterState2 = 0.0f;
276  lastGpsCheckTime_ms = 0;
277  lastInnovPassTime_ms = 0;
278  lastInnovFailTime_ms = 0;
279  gpsAccuracyGood = false;
280  memset(&gpsloc_prev, 0, sizeof(gpsloc_prev));
281  gpsDriftNE = 0.0f;
282  gpsVertVelFilt = 0.0f;
283  gpsHorizVelFilt = 0.0f;
284  memset(&statesArray, 0, sizeof(statesArray));
285  posDownDerivative = 0.0f;
286  posDown = 0.0f;
287  posVelFusionDelayed = false;
288  optFlowFusionDelayed = false;
289  flowFusionActive = false;
290  airSpdFusionDelayed = false;
291  sideSlipFusionDelayed = false;
292  posResetNE.zero();
293  velResetNE.zero();
294  posResetD = 0.0f;
295  hgtInnovFiltState = 0.0f;
296  if (_ahrs->get_compass()) {
297  magSelectIndex = _ahrs->get_compass()->get_primary();
298  }
299  imuDataDownSampledNew.delAng.zero();
300  imuDataDownSampledNew.delVel.zero();
301  imuDataDownSampledNew.delAngDT = 0.0f;
302  imuDataDownSampledNew.delVelDT = 0.0f;
303  runUpdates = false;
304  framesSincePredict = 0;
305  lastMagOffsetsValid = false;
306  magStateResetRequest = false;
307  magStateInitComplete = false;
308  magYawResetRequest = false;
309  gpsYawResetRequest = false;
310  posDownAtLastMagReset = stateStruct.position.z;
311  yawInnovAtLastMagReset = 0.0f;
312  quatAtLastMagReset = stateStruct.quat;
313  magFieldLearned = false;
314  delAngBiasLearned = false;
315  memset(&filterStatus, 0, sizeof(filterStatus));
316  gpsInhibit = false;
317  activeHgtSource = 0;
318  memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));
319  memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms));
320  memset(&storedRngMeas, 0, sizeof(storedRngMeas));
321  terrainHgtStable = true;
322  ekfOriginHgtVar = 0.0f;
323  ekfGpsRefHgt = 0.0;
324  velOffsetNED.zero();
325  posOffsetNED.zero();
326  posResetSource = DEFAULT;
327  velResetSource = DEFAULT;
328 
329  // range beacon fusion variables
330  memset(&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
331  memset(&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
332  rngBcnStoreIndex = 0;
333  lastRngBcnPassTime_ms = 0;
334  rngBcnTestRatio = 0.0f;
335  rngBcnHealth = false;
336  rngBcnTimeout = true;
337  varInnovRngBcn = 0.0f;
338  innovRngBcn = 0.0f;
339  memset(&lastTimeRngBcn_ms, 0, sizeof(lastTimeRngBcn_ms));
340  rngBcnDataToFuse = false;
341  beaconVehiclePosNED.zero();
342  beaconVehiclePosErr = 1.0f;
343  rngBcnLast3DmeasTime_ms = 0;
344  rngBcnGoodToAlign = false;
345  lastRngBcnChecked = 0;
346  receiverPos.zero();
347  memset(&receiverPosCov, 0, sizeof(receiverPosCov));
348  rngBcnAlignmentStarted = false;
349  rngBcnAlignmentCompleted = false;
350  lastBeaconIndex = 0;
351  rngBcnPosSum.zero();
352  numBcnMeas = 0;
353  rngSum = 0.0f;
354  N_beacons = 0;
355  maxBcnPosD = 0.0f;
356  minBcnPosD = 0.0f;
357  bcnPosDownOffsetMax = 0.0f;
358  bcnPosOffsetMaxVar = 0.0f;
359  maxOffsetStateChangeFilt = 0.0f;
360  bcnPosDownOffsetMin = 0.0f;
361  bcnPosOffsetMinVar = 0.0f;
362  minOffsetStateChangeFilt = 0.0f;
363  rngBcnFuseDataReportIndex = 0;
364  memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
365  bcnPosOffsetNED.zero();
366  bcnOriginEstInit = false;
367 
368  // body frame displacement fusion
369  memset(&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
370  memset(&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
371  lastbodyVelPassTime_ms = 0;
372  memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio));
373  memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel));
374  memset(&innovBodyVel, 0, sizeof(innovBodyVel));
375  prevBodyVelFuseTime_ms = 0;
376  bodyOdmMeasTime_ms = 0;
377  bodyVelFusionDelayed = false;
378  bodyVelFusionActive = false;
379  usingWheelSensors = false;
380  wheelOdmMeasTime_ms = 0;
381 
382  // zero data buffers
383  storedIMU.reset();
384  storedGPS.reset();
385  storedMag.reset();
386  storedBaro.reset();
387  storedTAS.reset();
388  storedRange.reset();
389  storedOutput.reset();
390  storedRangeBeacon.reset();
391  storedBodyOdm.reset();
392  storedWheelOdm.reset();
393 }
394 
395 // Initialise the states from accelerometer and magnetometer data (if present)
396 // This method can only be used when the vehicle is static
397 bool NavEKF3_core::InitialiseFilterBootstrap(void)
398 {
399  // If we are a plane and don't have GPS lock then don't initialise
400  if (assume_zero_sideslip() && AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
401  statesInitialised = false;
402  return false;
403  }
404 
405  // read all the sensors required to start the EKF the states
406  readIMUData();
407  readMagData();
408  readGpsData();
409  readBaroData();
410 
411  if (statesInitialised) {
412  // we are initialised, but we don't return true until the IMU
413  // buffer has been filled. This prevents a timing
414  // vulnerability with a pause in IMU data during filter startup
415  return storedIMU.is_filled();
416  }
417 
418  // accumulate enough sensor data to fill the buffers
419  if (firstInitTime_ms == 0) {
420  firstInitTime_ms = imuSampleTime_ms;
421  return false;
422  } else if (imuSampleTime_ms - firstInitTime_ms < 1000) {
423  return false;
424  }
425 
426  // set re-used variables to zero
427  InitialiseVariables();
428 
429  // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
430  Vector3f initAccVec;
431 
432  // TODO we should average accel readings over several cycles
433  initAccVec = AP::ins().get_accel(imu_index);
434 
435  // normalise the acceleration vector
436  float pitch=0, roll=0;
437  if (initAccVec.length() > 0.001f) {
438  initAccVec.normalize();
439 
440  // calculate initial pitch angle
441  pitch = asinf(initAccVec.x);
442 
443  // calculate initial roll angle
444  roll = atan2f(-initAccVec.y , -initAccVec.z);
445  }
446 
447  // calculate initial roll and pitch orientation
448  stateStruct.quat.from_euler(roll, pitch, 0.0f);
449 
450  // initialise dynamic states
451  stateStruct.velocity.zero();
452  stateStruct.position.zero();
453 
454  // initialise static process model states
455  stateStruct.gyro_bias.zero();
456  stateStruct.accel_bias.zero();
457  stateStruct.wind_vel.zero();
458  stateStruct.earth_magfield.zero();
459  stateStruct.body_magfield.zero();
460 
461  // set the position, velocity and height
462  ResetVelocity();
463  ResetPosition();
464  ResetHeight();
465 
466  // define Earth rotation vector in the NED navigation frame
467  calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);
468 
469  // initialise the covariance matrix
470  CovarianceInit();
471 
472  // reset the output predictor states
473  StoreOutputReset();
474 
475  // set to true now that states have be initialised
476  statesInitialised = true;
477  gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initialised",(unsigned)imu_index);
478 
479  // we initially return false to wait for the IMU buffer to fill
480  return false;
481 }
482 
483 // initialise the covariance matrix
484 void NavEKF3_core::CovarianceInit()
485 {
486  // zero the matrix
487  memset(P, 0, sizeof(P));
488 
489  // define the initial angle uncertainty as variances for a rotation vector
490  Vector3f rot_vec_var;
491  rot_vec_var.x = rot_vec_var.y = rot_vec_var.z = sq(0.1f);
492 
493  // update the quaternion state covariances
494  initialiseQuatCovariances(rot_vec_var);
495 
496  // velocities
497  P[4][4] = sq(frontend->_gpsHorizVelNoise);
498  P[5][5] = P[4][4];
499  P[6][6] = sq(frontend->_gpsVertVelNoise);
500  // positions
501  P[7][7] = sq(frontend->_gpsHorizPosNoise);
502  P[8][8] = P[7][7];
503  P[9][9] = sq(frontend->_baroAltNoise);
504  // gyro delta angle biases
505  P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
506  P[11][11] = P[10][10];
507  P[12][12] = P[10][10];
508  // delta velocity biases
509  P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
510  P[14][14] = P[13][13];
511  P[15][15] = P[13][13];
512  // earth magnetic field
513  P[16][16] = 0.0f;
514  P[17][17] = P[16][16];
515  P[18][18] = P[16][16];
516  // body magnetic field
517  P[19][19] = 0.0f;
518  P[20][20] = P[19][19];
519  P[21][21] = P[19][19];
520  // wind velocities
521  P[22][22] = 0.0f;
522  P[23][23] = P[22][22];
523 
524 
525  // optical flow ground height covariance
526  Popt = 0.25f;
527 
528 }
529 
530 /********************************************************
531 * UPDATE FUNCTIONS *
532 ********************************************************/
533 // Update Filter States - this should be called whenever new IMU data is available
534 void NavEKF3_core::UpdateFilter(bool predict)
535 {
536  // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
537  startPredictEnabled = predict;
538 
539  // don't run filter updates if states have not been initialised
540  if (!statesInitialised) {
541  return;
542  }
543 
544  // start the timer used for load measurement
545 #if EK3_DISABLE_INTERRUPTS
546  irqstate_t istate = irqsave();
547 #endif
548  hal.util->perf_begin(_perf_UpdateFilter);
549 
550  // TODO - in-flight restart method
551 
552  // Check arm status and perform required checks and mode changes
553  controlFilterModes();
554 
555  // read IMU data as delta angles and velocities
556  readIMUData();
557 
558  // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
559  if (runUpdates) {
560  // Predict states using IMU data from the delayed time horizon
561  UpdateStrapdownEquationsNED();
562 
563  // Predict the covariance growth
564  CovariancePrediction();
565 
566  // Update states using magnetometer data
567  SelectMagFusion();
568 
569  // Update states using GPS and altimeter data
570  SelectVelPosFusion();
571 
572  // Update states using range beacon data
573  SelectRngBcnFusion();
574 
575  // Update states using optical flow data
576  SelectFlowFusion();
577 
578  // Update states using body frame odometry data
579  SelectBodyOdomFusion();
580 
581  // Update states using airspeed data
582  SelectTasFusion();
583 
584  // Update states using sideslip constraint assumption for fly-forward vehicles
585  SelectBetaFusion();
586 
587  // Update the filter status
588  updateFilterStatus();
589  }
590 
591  // Wind output forward from the fusion to output time horizon
592  calcOutputStates();
593 
594  // stop the timer used for load measurement
595  hal.util->perf_end(_perf_UpdateFilter);
596 #if EK3_DISABLE_INTERRUPTS
597  irqrestore(istate);
598 #endif
599 }
600 
601 void NavEKF3_core::correctDeltaAngle(Vector3f &delAng, float delAngDT)
602 {
603  delAng -= stateStruct.gyro_bias * (delAngDT / dtEkfAvg);
604 }
605 
606 void NavEKF3_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT)
607 {
608  delVel -= stateStruct.accel_bias * (delVelDT / dtEkfAvg);
609 }
610 
611 /*
612  * Update the quaternion, velocity and position states using delayed IMU measurements
613  * because the EKF is running on a delayed time horizon. Note that the quaternion is
614  * not used by the EKF equations, which instead estimate the error in the attitude of
615  * the vehicle when each observtion is fused. This attitude error is then used to correct
616  * the quaternion.
617 */
618 void NavEKF3_core::UpdateStrapdownEquationsNED()
619 {
620  // update the quaternion states by rotating from the previous attitude through
621  // the delta angle rotation quaternion and normalise
622  // apply correction for earth's rotation rate
623  // % * - and + operators have been overloaded
624  stateStruct.quat.rotate(delAngCorrected - prevTnb * earthRateNED*imuDataDelayed.delAngDT);
625  stateStruct.quat.normalize();
626 
627  // transform body delta velocities to delta velocities in the nav frame
628  // use the nav frame from previous time step as the delta velocities
629  // have been rotated into that frame
630  // * and + operators have been overloaded
631  Vector3f delVelNav; // delta velocity vector in earth axes
632  delVelNav = prevTnb.mul_transpose(delVelCorrected);
633  delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
634 
635  // calculate the body to nav cosine matrix
636  stateStruct.quat.inverse().rotation_matrix(prevTnb);
637 
638  // calculate the rate of change of velocity (used for launch detect and other functions)
639  velDotNED = delVelNav / imuDataDelayed.delVelDT;
640 
641  // apply a first order lowpass filter
642  velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
643 
644  // calculate a magnitude of the filtered nav acceleration (required for GPS
645  // variance estimation)
646  accNavMag = velDotNEDfilt.length();
647  accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y);
648 
649  // if we are not aiding, then limit the horizontal magnitude of acceleration
650  // to prevent large manoeuvre transients disturbing the attitude
651  if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {
652  float gain = 5.0f/accNavMagHoriz;
653  delVelNav.x *= gain;
654  delVelNav.y *= gain;
655  }
656 
657  // save velocity for use in trapezoidal integration for position calcuation
658  Vector3f lastVelocity = stateStruct.velocity;
659 
660  // sum delta velocities to get velocity
661  stateStruct.velocity += delVelNav;
662 
663  // apply a trapezoidal integration to velocities to calculate position
664  stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
665 
666  // accumulate the bias delta angle and time since last reset by an OF measurement arrival
667  delAngBodyOF += delAngCorrected;
668  delTimeOF += imuDataDelayed.delAngDT;
669 
670  // limit states to protect against divergence
671  ConstrainStates();
672 
673  // If main filter velocity states are valid, update the range beacon receiver position states
674  if (filterStatus.flags.horiz_vel) {
675  receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
676  }
677 }
678 
679 /*
680  * Propagate PVA solution forward from the fusion time horizon to the current time horizon
681  * using simple observer which performs two functions:
682  * 1) Corrects for the delayed time horizon used by the EKF.
683  * 2) Applies a LPF to state corrections to prevent 'stepping' in states due to measurement
684  * fusion introducing unwanted noise into the control loops.
685  * The inspiration for using a complementary filter to correct for time delays in the EKF
686  * is based on the work by A Khosravian.
687  *
688  * “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
689  * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
690 */
691 void NavEKF3_core::calcOutputStates()
692 {
693  // apply corrections to the IMU data
694  Vector3f delAngNewCorrected = imuDataNew.delAng;
695  Vector3f delVelNewCorrected = imuDataNew.delVel;
696  correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT);
697  correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT);
698 
699  // apply corrections to track EKF solution
700  Vector3f delAng = delAngNewCorrected + delAngCorrection;
701 
702  // convert the rotation vector to its equivalent quaternion
703  Quaternion deltaQuat;
704  deltaQuat.from_axis_angle(delAng);
705 
706  // update the quaternion states by rotating from the previous attitude through
707  // the delta angle rotation quaternion and normalise
708  outputDataNew.quat *= deltaQuat;
709  outputDataNew.quat.normalize();
710 
711  // calculate the body to nav cosine matrix
712  Matrix3f Tbn_temp;
713  outputDataNew.quat.rotation_matrix(Tbn_temp);
714 
715  // transform body delta velocities to delta velocities in the nav frame
716  Vector3f delVelNav = Tbn_temp*delVelNewCorrected;
717  delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
718 
719  // save velocity for use in trapezoidal integration for position calcuation
720  Vector3f lastVelocity = outputDataNew.velocity;
721 
722  // sum delta velocities to get velocity
723  outputDataNew.velocity += delVelNav;
724 
725  // apply a trapezoidal integration to velocities to calculate position
726  outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
727 
728  // If the IMU accelerometer is offset from the body frame origin, then calculate corrections
729  // that can be added to the EKF velocity and position outputs so that they represent the velocity
730  // and position of the body frame origin.
731  // Note the * operator has been overloaded to operate as a dot product
732  if (!accelPosOffset.is_zero()) {
733  // calculate the average angular rate across the last IMU update
734  // note delAngDT is prevented from being zero in readIMUData()
735  Vector3f angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
736 
737  // Calculate the velocity of the body frame origin relative to the IMU in body frame
738  // and rotate into earth frame. Note % operator has been overloaded to perform a cross product
739  Vector3f velBodyRelIMU = angRate % (- accelPosOffset);
740  velOffsetNED = Tbn_temp * velBodyRelIMU;
741 
742  // calculate the earth frame position of the body frame origin relative to the IMU
743  posOffsetNED = Tbn_temp * (- accelPosOffset);
744  } else {
745  velOffsetNED.zero();
746  posOffsetNED.zero();
747  }
748 
749  // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
750  if (runUpdates) {
751  // store the states at the output time horizon
752  storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
753 
754  // recall the states from the fusion time horizon
755  outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
756 
757  // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
758 
759  // divide the demanded quaternion by the estimated to get the error
760  Quaternion quatErr = stateStruct.quat / outputDataDelayed.quat;
761 
762  // Convert to a delta rotation using a small angle approximation
763  quatErr.normalize();
764  Vector3f deltaAngErr;
765  float scaler;
766  if (quatErr[0] >= 0.0f) {
767  scaler = 2.0f;
768  } else {
769  scaler = -2.0f;
770  }
771  deltaAngErr.x = scaler * quatErr[1];
772  deltaAngErr.y = scaler * quatErr[2];
773  deltaAngErr.z = scaler * quatErr[3];
774 
775  // calculate a gain that provides tight tracking of the estimator states and
776  // adjust for changes in time delay to maintain consistent damping ratio of ~0.7
777  float timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
778  timeDelay = MAX(timeDelay, dtIMUavg);
779  float errorGain = 0.5f / timeDelay;
780 
781  // calculate a correction to the delta angle
782  // that will cause the INS to track the EKF quaternions
783  delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
784 
785  // calculate velocity and position tracking errors
786  Vector3f velErr = (stateStruct.velocity - outputDataDelayed.velocity);
787  Vector3f posErr = (stateStruct.position - outputDataDelayed.position);
788 
789  // collect magnitude tracking error for diagnostics
790  outputTrackError.x = deltaAngErr.length();
791  outputTrackError.y = velErr.length();
792  outputTrackError.z = posErr.length();
793 
794  // convert user specified time constant from centi-seconds to seconds
795  float tauPosVel = constrain_float(0.01f*(float)frontend->_tauVelPosOutput, 0.1f, 0.5f);
796 
797  // calculate a gain to track the EKF position states with the specified time constant
798  float velPosGain = dtEkfAvg / constrain_float(tauPosVel, dtEkfAvg, 10.0f);
799 
800  // use a PI feedback to calculate a correction that will be applied to the output state history
801  posErrintegral += posErr;
802  velErrintegral += velErr;
803  Vector3f velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f;
804  Vector3f posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f;
805 
806  // loop through the output filter state history and apply the corrections to the velocity and position states
807  // this method is too expensive to use for the attitude states due to the quaternion operations required
808  // but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
809  // to be used
810  output_elements outputStates;
811  for (unsigned index=0; index < imu_buffer_length; index++) {
812  outputStates = storedOutput[index];
813 
814  // a constant velocity correction is applied
815  outputStates.velocity += velCorrection;
816 
817  // a constant position correction is applied
818  outputStates.position += posCorrection;
819 
820  // push the updated data to the buffer
821  storedOutput[index] = outputStates;
822  }
823 
824  // update output state to corrected values
825  outputDataNew = storedOutput[storedIMU.get_youngest_index()];
826 
827  }
828 }
829 
830 /*
831  * Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox.
832  * The script file used to generate these and other equations in this filter can be found here:
833  * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
834 */
835 void NavEKF3_core::CovariancePrediction()
836 {
837  hal.util->perf_begin(_perf_CovariancePrediction);
838  float daxVar; // X axis delta angle noise variance rad^2
839  float dayVar; // Y axis delta angle noise variance rad^2
840  float dazVar; // Z axis delta angle noise variance rad^2
841  float dvxVar; // X axis delta velocity variance noise (m/s)^2
842  float dvyVar; // Y axis delta velocity variance noise (m/s)^2
843  float dvzVar; // Z axis delta velocity variance noise (m/s)^2
844  float dvx; // X axis delta velocity (m/s)
845  float dvy; // Y axis delta velocity (m/s)
846  float dvz; // Z axis delta velocity (m/s)
847  float dax; // X axis delta angle (rad)
848  float day; // Y axis delta angle (rad)
849  float daz; // Z axis delta angle (rad)
850  float q0; // attitude quaternion
851  float q1; // attitude quaternion
852  float q2; // attitude quaternion
853  float q3; // attitude quaternion
854  float dax_b; // X axis delta angle measurement bias (rad)
855  float day_b; // Y axis delta angle measurement bias (rad)
856  float daz_b; // Z axis delta angle measurement bias (rad)
857  float dvx_b; // X axis delta velocity measurement bias (rad)
858  float dvy_b; // Y axis delta velocity measurement bias (rad)
859  float dvz_b; // Z axis delta velocity measurement bias (rad)
860 
861  // Calculate the time step used by the covariance prediction as an average of the gyro and accel integration period
862  // Constrain to prevent bad timing jitter causing numerical conditioning problems with the covariance prediction
863  dt = constrain_float(0.5f*(imuDataDelayed.delAngDT+imuDataDelayed.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
864 
865  // use filtered height rate to increase wind process noise when climbing or descending
866  // this allows for wind gradient effects.Filter height rate using a 10 second time constant filter
867  float alpha = 0.1f * dt;
868  hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
869 
870  // calculate covariance prediction process noise added to diagonals of predicted covariance matrix
871  // error growth of first 10 kinematic states is built into auto-code for covariance prediction and driven by IMU noise parameters
872  Vector14 processNoiseVariance = {};
873 
874  if (!inhibitDelAngBiasStates) {
875  float dAngBiasVar = sq(sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f));
876  for (uint8_t i=0; i<=2; i++) processNoiseVariance[i] = dAngBiasVar;
877  }
878 
879  if (!inhibitDelVelBiasStates) {
880  float dVelBiasVar = sq(sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f));
881  for (uint8_t i=3; i<=5; i++) {
882  uint8_t stateIndex = i + 10;
883  if (P[stateIndex][stateIndex] > 1E-8f) {
884  processNoiseVariance[i] = dVelBiasVar;
885  } else {
886  // increase the process noise variance up to a maximum of 100 x the nominal value if the variance is below the target minimum
887  processNoiseVariance[i] = 10.0f * dVelBiasVar * (1e-8f / fmaxf(P[stateIndex][stateIndex],1e-9f));
888  }
889  }
890  }
891 
892  if (!inhibitMagStates) {
893  float magEarthVar = sq(dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f));
894  float magBodyVar = sq(dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f));
895  for (uint8_t i=6; i<=8; i++) processNoiseVariance[i] = magEarthVar;
896  for (uint8_t i=9; i<=11; i++) processNoiseVariance[i] = magBodyVar;
897  }
898 
899  if (!inhibitWindStates) {
900  float windVelVar = sq(dt * constrain_float(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_float(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsf(hgtRate)));
901  for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
902  }
903 
904  // set variables used to calculate covariance growth
905  dvx = imuDataDelayed.delVel.x;
906  dvy = imuDataDelayed.delVel.y;
907  dvz = imuDataDelayed.delVel.z;
908  dax = imuDataDelayed.delAng.x;
909  day = imuDataDelayed.delAng.y;
910  daz = imuDataDelayed.delAng.z;
911  q0 = stateStruct.quat[0];
912  q1 = stateStruct.quat[1];
913  q2 = stateStruct.quat[2];
914  q3 = stateStruct.quat[3];
915  dax_b = stateStruct.gyro_bias.x;
916  day_b = stateStruct.gyro_bias.y;
917  daz_b = stateStruct.gyro_bias.z;
918  dvx_b = stateStruct.accel_bias.x;
919  dvy_b = stateStruct.accel_bias.y;
920  dvz_b = stateStruct.accel_bias.z;
921  float _gyrNoise = constrain_float(frontend->_gyrNoise, 0.0f, 1.0f);
922  daxVar = dayVar = dazVar = sq(dt*_gyrNoise);
923  float _accNoise = constrain_float(frontend->_accNoise, 0.0f, 10.0f);
924  dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
925 
926  // calculate the predicted covariance due to inertial sensor error propagation
927  // we calculate the lower diagonal and copy to take advantage of symmetry
928 
929  // intermediate calculations
930  Vector21 SF;
931  SF[0] = dvz - dvz_b;
932  SF[1] = dvy - dvy_b;
933  SF[2] = dvx - dvx_b;
934  SF[3] = 2*q1*SF[2] + 2*q2*SF[1] + 2*q3*SF[0];
935  SF[4] = 2*q0*SF[1] - 2*q1*SF[0] + 2*q3*SF[2];
936  SF[5] = 2*q0*SF[2] + 2*q2*SF[0] - 2*q3*SF[1];
937  SF[6] = day/2 - day_b/2;
938  SF[7] = daz/2 - daz_b/2;
939  SF[8] = dax/2 - dax_b/2;
940  SF[9] = dax_b/2 - dax/2;
941  SF[10] = daz_b/2 - daz/2;
942  SF[11] = day_b/2 - day/2;
943  SF[12] = 2*q1*SF[1];
944  SF[13] = 2*q0*SF[0];
945  SF[14] = q1/2;
946  SF[15] = q2/2;
947  SF[16] = q3/2;
948  SF[17] = sq(q3);
949  SF[18] = sq(q2);
950  SF[19] = sq(q1);
951  SF[20] = sq(q0);
952 
953  Vector8 SG;
954  SG[0] = q0/2;
955  SG[1] = sq(q3);
956  SG[2] = sq(q2);
957  SG[3] = sq(q1);
958  SG[4] = sq(q0);
959  SG[5] = 2*q2*q3;
960  SG[6] = 2*q1*q3;
961  SG[7] = 2*q1*q2;
962 
963  Vector11 SQ;
964  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);
965  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);
966  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]);
967  SQ[3] = (dayVar*q1*SG[0])/2 - (dazVar*q1*SG[0])/2 - (daxVar*q2*q3)/4;
968  SQ[4] = (dazVar*q2*SG[0])/2 - (daxVar*q2*SG[0])/2 - (dayVar*q1*q3)/4;
969  SQ[5] = (daxVar*q3*SG[0])/2 - (dayVar*q3*SG[0])/2 - (dazVar*q1*q2)/4;
970  SQ[6] = (daxVar*q1*q2)/4 - (dazVar*q3*SG[0])/2 - (dayVar*q1*q2)/4;
971  SQ[7] = (dazVar*q1*q3)/4 - (daxVar*q1*q3)/4 - (dayVar*q2*SG[0])/2;
972  SQ[8] = (dayVar*q2*q3)/4 - (daxVar*q1*SG[0])/2 - (dazVar*q2*q3)/4;
973  SQ[9] = sq(SG[0]);
974  SQ[10] = sq(q1);
975 
976  Vector11 SPP;
977  SPP[0] = SF[12] + SF[13] - 2*q2*SF[2];
978  SPP[1] = SF[17] - SF[18] - SF[19] + SF[20];
979  SPP[2] = SF[17] - SF[18] + SF[19] - SF[20];
980  SPP[3] = SF[17] + SF[18] - SF[19] - SF[20];
981  SPP[4] = 2*q0*q2 - 2*q1*q3;
982  SPP[5] = 2*q0*q1 - 2*q2*q3;
983  SPP[6] = 2*q0*q3 - 2*q1*q2;
984  SPP[7] = 2*q0*q1 + 2*q2*q3;
985  SPP[8] = 2*q0*q3 + 2*q1*q2;
986  SPP[9] = 2*q0*q2 + 2*q1*q3;
987  SPP[10] = SF[16];
988 
989 
990  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;
991  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;
992  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;
993  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;
994  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;
995  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;
996  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;
997  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;
998  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;
999  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;
1000  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]);
1001  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);
1002  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);
1003  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);
1004  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]);
1005  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]);
1006  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);
1007  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);
1008  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);
1009  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]);
1010  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]);
1011  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]);
1012  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);
1013  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);
1014  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);
1015  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]);
1016  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]);
1017  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]);
1018  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]);
1019  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);
1020  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);
1021  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);
1022  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]);
1023  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]);
1024  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]);
1025  nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
1026  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]);
1027  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);
1028  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);
1029  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);
1030  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]);
1031  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]);
1032  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]);
1033  nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
1034  nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
1035  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]);
1036  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);
1037  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);
1038  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);
1039  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]);
1040  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]);
1041  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]);
1042  nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
1043  nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
1044  nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
1045 
1046  if (stateIndexLim > 9) {
1047  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];
1048  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;
1049  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;
1050  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;
1051  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];
1052  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];
1053  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];
1054  nextP[7][10] = P[7][10] + P[4][10]*dt;
1055  nextP[8][10] = P[8][10] + P[5][10]*dt;
1056  nextP[9][10] = P[9][10] + P[6][10]*dt;
1057  nextP[10][10] = P[10][10];
1058  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];
1059  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;
1060  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;
1061  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;
1062  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];
1063  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];
1064  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];
1065  nextP[7][11] = P[7][11] + P[4][11]*dt;
1066  nextP[8][11] = P[8][11] + P[5][11]*dt;
1067  nextP[9][11] = P[9][11] + P[6][11]*dt;
1068  nextP[10][11] = P[10][11];
1069  nextP[11][11] = P[11][11];
1070  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];
1071  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;
1072  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;
1073  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;
1074  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];
1075  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];
1076  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];
1077  nextP[7][12] = P[7][12] + P[4][12]*dt;
1078  nextP[8][12] = P[8][12] + P[5][12]*dt;
1079  nextP[9][12] = P[9][12] + P[6][12]*dt;
1080  nextP[10][12] = P[10][12];
1081  nextP[11][12] = P[11][12];
1082  nextP[12][12] = P[12][12];
1083 
1084  if (stateIndexLim > 12) {
1085  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];
1086  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;
1087  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;
1088  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;
1089  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];
1090  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];
1091  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];
1092  nextP[7][13] = P[7][13] + P[4][13]*dt;
1093  nextP[8][13] = P[8][13] + P[5][13]*dt;
1094  nextP[9][13] = P[9][13] + P[6][13]*dt;
1095  nextP[10][13] = P[10][13];
1096  nextP[11][13] = P[11][13];
1097  nextP[12][13] = P[12][13];
1098  nextP[13][13] = P[13][13];
1099  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];
1100  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;
1101  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;
1102  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;
1103  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];
1104  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];
1105  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];
1106  nextP[7][14] = P[7][14] + P[4][14]*dt;
1107  nextP[8][14] = P[8][14] + P[5][14]*dt;
1108  nextP[9][14] = P[9][14] + P[6][14]*dt;
1109  nextP[10][14] = P[10][14];
1110  nextP[11][14] = P[11][14];
1111  nextP[12][14] = P[12][14];
1112  nextP[13][14] = P[13][14];
1113  nextP[14][14] = P[14][14];
1114  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];
1115  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;
1116  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;
1117  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;
1118  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];
1119  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];
1120  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];
1121  nextP[7][15] = P[7][15] + P[4][15]*dt;
1122  nextP[8][15] = P[8][15] + P[5][15]*dt;
1123  nextP[9][15] = P[9][15] + P[6][15]*dt;
1124  nextP[10][15] = P[10][15];
1125  nextP[11][15] = P[11][15];
1126  nextP[12][15] = P[12][15];
1127  nextP[13][15] = P[13][15];
1128  nextP[14][15] = P[14][15];
1129  nextP[15][15] = P[15][15];
1130 
1131  if (stateIndexLim > 15) {
1132  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];
1133  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;
1134  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;
1135  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;
1136  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];
1137  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];
1138  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];
1139  nextP[7][16] = P[7][16] + P[4][16]*dt;
1140  nextP[8][16] = P[8][16] + P[5][16]*dt;
1141  nextP[9][16] = P[9][16] + P[6][16]*dt;
1142  nextP[10][16] = P[10][16];
1143  nextP[11][16] = P[11][16];
1144  nextP[12][16] = P[12][16];
1145  nextP[13][16] = P[13][16];
1146  nextP[14][16] = P[14][16];
1147  nextP[15][16] = P[15][16];
1148  nextP[16][16] = P[16][16];
1149  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];
1150  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;
1151  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;
1152  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;
1153  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];
1154  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];
1155  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];
1156  nextP[7][17] = P[7][17] + P[4][17]*dt;
1157  nextP[8][17] = P[8][17] + P[5][17]*dt;
1158  nextP[9][17] = P[9][17] + P[6][17]*dt;
1159  nextP[10][17] = P[10][17];
1160  nextP[11][17] = P[11][17];
1161  nextP[12][17] = P[12][17];
1162  nextP[13][17] = P[13][17];
1163  nextP[14][17] = P[14][17];
1164  nextP[15][17] = P[15][17];
1165  nextP[16][17] = P[16][17];
1166  nextP[17][17] = P[17][17];
1167  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];
1168  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;
1169  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;
1170  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;
1171  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];
1172  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];
1173  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];
1174  nextP[7][18] = P[7][18] + P[4][18]*dt;
1175  nextP[8][18] = P[8][18] + P[5][18]*dt;
1176  nextP[9][18] = P[9][18] + P[6][18]*dt;
1177  nextP[10][18] = P[10][18];
1178  nextP[11][18] = P[11][18];
1179  nextP[12][18] = P[12][18];
1180  nextP[13][18] = P[13][18];
1181  nextP[14][18] = P[14][18];
1182  nextP[15][18] = P[15][18];
1183  nextP[16][18] = P[16][18];
1184  nextP[17][18] = P[17][18];
1185  nextP[18][18] = P[18][18];
1186  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];
1187  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;
1188  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;
1189  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;
1190  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];
1191  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];
1192  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];
1193  nextP[7][19] = P[7][19] + P[4][19]*dt;
1194  nextP[8][19] = P[8][19] + P[5][19]*dt;
1195  nextP[9][19] = P[9][19] + P[6][19]*dt;
1196  nextP[10][19] = P[10][19];
1197  nextP[11][19] = P[11][19];
1198  nextP[12][19] = P[12][19];
1199  nextP[13][19] = P[13][19];
1200  nextP[14][19] = P[14][19];
1201  nextP[15][19] = P[15][19];
1202  nextP[16][19] = P[16][19];
1203  nextP[17][19] = P[17][19];
1204  nextP[18][19] = P[18][19];
1205  nextP[19][19] = P[19][19];
1206  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];
1207  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;
1208  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;
1209  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;
1210  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];
1211  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];
1212  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];
1213  nextP[7][20] = P[7][20] + P[4][20]*dt;
1214  nextP[8][20] = P[8][20] + P[5][20]*dt;
1215  nextP[9][20] = P[9][20] + P[6][20]*dt;
1216  nextP[10][20] = P[10][20];
1217  nextP[11][20] = P[11][20];
1218  nextP[12][20] = P[12][20];
1219  nextP[13][20] = P[13][20];
1220  nextP[14][20] = P[14][20];
1221  nextP[15][20] = P[15][20];
1222  nextP[16][20] = P[16][20];
1223  nextP[17][20] = P[17][20];
1224  nextP[18][20] = P[18][20];
1225  nextP[19][20] = P[19][20];
1226  nextP[20][20] = P[20][20];
1227  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];
1228  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;
1229  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;
1230  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;
1231  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];
1232  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];
1233  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];
1234  nextP[7][21] = P[7][21] + P[4][21]*dt;
1235  nextP[8][21] = P[8][21] + P[5][21]*dt;
1236  nextP[9][21] = P[9][21] + P[6][21]*dt;
1237  nextP[10][21] = P[10][21];
1238  nextP[11][21] = P[11][21];
1239  nextP[12][21] = P[12][21];
1240  nextP[13][21] = P[13][21];
1241  nextP[14][21] = P[14][21];
1242  nextP[15][21] = P[15][21];
1243  nextP[16][21] = P[16][21];
1244  nextP[17][21] = P[17][21];
1245  nextP[18][21] = P[18][21];
1246  nextP[19][21] = P[19][21];
1247  nextP[20][21] = P[20][21];
1248  nextP[21][21] = P[21][21];
1249 
1250  if (stateIndexLim > 21) {
1251  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];
1252  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;
1253  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;
1254  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;
1255  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];
1256  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];
1257  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];
1258  nextP[7][22] = P[7][22] + P[4][22]*dt;
1259  nextP[8][22] = P[8][22] + P[5][22]*dt;
1260  nextP[9][22] = P[9][22] + P[6][22]*dt;
1261  nextP[10][22] = P[10][22];
1262  nextP[11][22] = P[11][22];
1263  nextP[12][22] = P[12][22];
1264  nextP[13][22] = P[13][22];
1265  nextP[14][22] = P[14][22];
1266  nextP[15][22] = P[15][22];
1267  nextP[16][22] = P[16][22];
1268  nextP[17][22] = P[17][22];
1269  nextP[18][22] = P[18][22];
1270  nextP[19][22] = P[19][22];
1271  nextP[20][22] = P[20][22];
1272  nextP[21][22] = P[21][22];
1273  nextP[22][22] = P[22][22];
1274  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];
1275  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;
1276  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;
1277  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;
1278  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];
1279  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];
1280  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];
1281  nextP[7][23] = P[7][23] + P[4][23]*dt;
1282  nextP[8][23] = P[8][23] + P[5][23]*dt;
1283  nextP[9][23] = P[9][23] + P[6][23]*dt;
1284  nextP[10][23] = P[10][23];
1285  nextP[11][23] = P[11][23];
1286  nextP[12][23] = P[12][23];
1287  nextP[13][23] = P[13][23];
1288  nextP[14][23] = P[14][23];
1289  nextP[15][23] = P[15][23];
1290  nextP[16][23] = P[16][23];
1291  nextP[17][23] = P[17][23];
1292  nextP[18][23] = P[18][23];
1293  nextP[19][23] = P[19][23];
1294  nextP[20][23] = P[20][23];
1295  nextP[21][23] = P[21][23];
1296  nextP[22][23] = P[22][23];
1297  nextP[23][23] = P[23][23];
1298  }
1299  }
1300  }
1301  }
1302 
1303  // add the general state process noise variances
1304  if (stateIndexLim > 9) {
1305  for (uint8_t i=10; i<=stateIndexLim; i++) {
1306  nextP[i][i] = nextP[i][i] + processNoiseVariance[i-10];
1307  }
1308  }
1309 
1310  // if the total position variance exceeds 1e4 (100m), then stop covariance
1311  // growth by setting the predicted to the previous values
1312  // This prevent an ill conditioned matrix from occurring for long periods
1313  // without GPS
1314  if ((P[7][7] + P[8][8]) > 1e4f) {
1315  for (uint8_t i=7; i<=8; i++)
1316  {
1317  for (uint8_t j=0; j<=stateIndexLim; j++)
1318  {
1319  nextP[i][j] = P[i][j];
1320  nextP[j][i] = P[j][i];
1321  }
1322  }
1323  }
1324 
1325  // covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP
1326  // to lower and upper half in P
1327  for (uint8_t row = 0; row <= stateIndexLim; row++) {
1328  // copy diagonals
1329  P[row][row] = nextP[row][row];
1330  // copy off diagonals
1331  for (uint8_t column = 0 ; column < row; column++) {
1332  P[row][column] = P[column][row] = nextP[column][row];
1333  }
1334  }
1335 
1336  // constrain values to prevent ill-conditioning
1337  ConstrainVariances();
1338 
1339  hal.util->perf_end(_perf_CovariancePrediction);
1340 }
1341 
1342 // zero specified range of rows in the state covariance matrix
1343 void NavEKF3_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
1344 {
1345  uint8_t row;
1346  for (row=first; row<=last; row++)
1347  {
1348  memset(&covMat[row][0], 0, sizeof(covMat[0][0])*24);
1349  }
1350 }
1351 
1352 // zero specified range of columns in the state covariance matrix
1353 void NavEKF3_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
1354 {
1355  uint8_t row;
1356  for (row=0; row<=23; row++)
1357  {
1358  memset(&covMat[row][first], 0, sizeof(covMat[0][0])*(1+last-first));
1359  }
1360 }
1361 
1362 // reset the output data to the current EKF state
1363 void NavEKF3_core::StoreOutputReset()
1364 {
1365  outputDataNew.quat = stateStruct.quat;
1366  outputDataNew.velocity = stateStruct.velocity;
1367  outputDataNew.position = stateStruct.position;
1368  // write current measurement to entire table
1369  for (uint8_t i=0; i<imu_buffer_length; i++) {
1370  storedOutput[i] = outputDataNew;
1371  }
1372  outputDataDelayed = outputDataNew;
1373  // reset the states for the complementary filter used to provide a vertical position dervative output
1374  posDown = stateStruct.position.z;
1375  posDownDerivative = stateStruct.velocity.z;
1376 }
1377 
1378 // Reset the stored output quaternion history to current EKF state
1379 void NavEKF3_core::StoreQuatReset()
1380 {
1381  outputDataNew.quat = stateStruct.quat;
1382  // write current measurement to entire table
1383  for (uint8_t i=0; i<imu_buffer_length; i++) {
1384  storedOutput[i].quat = outputDataNew.quat;
1385  }
1386  outputDataDelayed.quat = outputDataNew.quat;
1387 }
1388 
1389 // Rotate the stored output quaternion history through a quaternion rotation
1390 void NavEKF3_core::StoreQuatRotate(Quaternion deltaQuat)
1391 {
1392  outputDataNew.quat = outputDataNew.quat*deltaQuat;
1393  // write current measurement to entire table
1394  for (uint8_t i=0; i<imu_buffer_length; i++) {
1395  storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
1396  }
1397  outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
1398 }
1399 
1400 // calculate nav to body quaternions from body to nav rotation matrix
1401 void NavEKF3_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
1402 {
1403  // Calculate the body to nav cosine matrix
1404  quat.rotation_matrix(Tbn);
1405 }
1406 
1407 // force symmetry on the covariance matrix to prevent ill-conditioning
1408 void NavEKF3_core::ForceSymmetry()
1409 {
1410  for (uint8_t i=1; i<=stateIndexLim; i++)
1411  {
1412  for (uint8_t j=0; j<=i-1; j++)
1413  {
1414  float temp = 0.5f*(P[i][j] + P[j][i]);
1415  P[i][j] = temp;
1416  P[j][i] = temp;
1417  }
1418  }
1419 }
1420 
1421 // constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
1422 // if states are inactive, zero the corresponding off-diagonals
1423 void NavEKF3_core::ConstrainVariances()
1424 {
1425  for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0f); // attitude error
1426  for (uint8_t i=4; i<=6; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f); // velocities
1427  for (uint8_t i=7; i<=8; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e6f);
1428  P[9][9] = constrain_float(P[9][9],0.0f,1.0e6f); // vertical position
1429 
1430  if (!inhibitDelAngBiasStates) {
1431  for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_float(P[i][i],0.0f,sq(0.175f * dtEkfAvg));
1432  } else {
1433  zeroCols(P,10,12);
1434  zeroRows(P,10,12);
1435  }
1436 
1437  if (!inhibitDelVelBiasStates) {
1438  // limit delta velocity bias state variance levels and request a reset if below the safe minimum
1439  bool resetRequired = false;
1440  for (uint8_t i=13; i<=15; i++) {
1441  if (P[i][i] > 1E-9f) {
1442  // variance is above the safe minimum
1443  P[i][i] = fminf(P[i][i], sq(10.0f * dtEkfAvg));
1444  } else {
1445  // Set the variance to the target minimum and request a covariance reset
1446  P[i][i] = 1E-8f;
1447  resetRequired = true;
1448  }
1449  }
1450 
1451  // If any one axis is below the safe minimum, all delta velocity covariance terms must be reset to zero
1452  if (resetRequired) {
1453  float delVelBiasVar[3];
1454  // store all delta velocity bias variances
1455  for (uint8_t i=0; i<=2; i++) {
1456  delVelBiasVar[i] = P[i+13][i+13];
1457  }
1458  // reset all delta velocity bias covariances
1459  zeroCols(P,13,15);
1460  // restore all delta velocity bias variances
1461  for (uint8_t i=0; i<=2; i++) {
1462  P[i+13][i+13] = delVelBiasVar[i];
1463  }
1464  }
1465 
1466  } else {
1467  zeroCols(P,13,15);
1468  zeroRows(P,13,15);
1469  }
1470 
1471  if (!inhibitMagStates) {
1472  for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // earth magnetic field
1473  for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_float(P[i][i],0.0f,0.01f); // body magnetic field
1474  } else {
1475  zeroCols(P,16,21);
1476  zeroRows(P,16,21);
1477  }
1478 
1479  if (!inhibitWindStates) {
1480  for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_float(P[i][i],0.0f,1.0e3f);
1481  } else {
1482  zeroCols(P,22,23);
1483  zeroRows(P,22,23);
1484  }
1485 }
1486 
1487 // constrain states to prevent ill-conditioning
1488 void NavEKF3_core::ConstrainStates()
1489 {
1490  // quaternions are limited between +-1
1491  for (uint8_t i=0; i<=3; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
1492  // velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
1493  for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_float(statesArray[i],-5.0e2f,5.0e2f);
1494  // position limit 1000 km - TODO apply circular limit
1495  for (uint8_t i=7; i<=8; i++) statesArray[i] = constrain_float(statesArray[i],-1.0e6f,1.0e6f);
1496  // height limit covers home alt on everest through to home alt at SL and ballon drop
1497  stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
1498  // gyro bias limit (this needs to be set based on manufacturers specs)
1499  for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg);
1500  // the accelerometer bias limit is controlled by a user adjustable parameter
1501  for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg);
1502  // earth magnetic field limit
1503  for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
1504  // body magnetic field limit
1505  for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f);
1506  // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
1507  for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_float(statesArray[i],-100.0f,100.0f);
1508  // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum
1509  if (!inhibitGndState) {
1510  terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
1511  }
1512 }
1513 
1514 // calculate the NED earth spin vector in rad/sec
1515 void NavEKF3_core::calcEarthRateNED(Vector3f &omega, int32_t latitude) const
1516 {
1517  float lat_rad = radians(latitude*1.0e-7f);
1518  omega.x = earthRate*cosf(lat_rad);
1519  omega.y = 0;
1520  omega.z = -earthRate*sinf(lat_rad);
1521 }
1522 
1523 // initialise the earth magnetic field states using declination, suppled roll/pitch
1524 // and magnetometer measurements and return initial attitude quaternion
1525 Quaternion NavEKF3_core::calcQuatAndFieldStates(float roll, float pitch)
1526 {
1527  // declare local variables required to calculate initial orientation and magnetic field
1528  float yaw;
1529  Matrix3f Tbn;
1530  Vector3f initMagNED;
1531  Quaternion initQuat;
1532 
1533  if (use_compass()) {
1534  // calculate rotation matrix from body to NED frame
1535  Tbn.from_euler(roll, pitch, 0.0f);
1536 
1537  // read the magnetometer data
1538  readMagData();
1539 
1540  // rotate the magnetic field into NED axes
1541  initMagNED = Tbn * magDataDelayed.mag;
1542 
1543  // calculate heading of mag field rel to body heading
1544  float magHeading = atan2f(initMagNED.y, initMagNED.x);
1545 
1546  // get the magnetic declination
1547  float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
1548 
1549  // calculate yaw angle rel to true north
1550  yaw = magDecAng - magHeading;
1551 
1552  // calculate initial filter quaternion states using yaw from magnetometer
1553  // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset
1554  Vector3f tempEuler;
1555  stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
1556  // this check ensures we accumulate the resets that occur within a single iteration of the EKF
1557  if (imuSampleTime_ms != lastYawReset_ms) {
1558  yawResetAngle = 0.0f;
1559  }
1560  yawResetAngle += wrap_PI(yaw - tempEuler.z);
1561  lastYawReset_ms = imuSampleTime_ms;
1562  // calculate an initial quaternion using the new yaw value
1563  initQuat.from_euler(roll, pitch, yaw);
1564  // zero the attitude covariances because the correlations will now be invalid
1565  zeroAttCovOnly();
1566 
1567  // calculate initial Tbn matrix and rotate Mag measurements into NED
1568  // to set initial NED magnetic field states
1569  // don't do this if the earth field has already been learned
1570  if (!magFieldLearned) {
1571  initQuat.rotation_matrix(Tbn);
1572  stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
1573 
1574  // set the NE earth magnetic field states using the published declination
1575  // and set the corresponding variances and covariances
1576  alignMagStateDeclination();
1577 
1578  // set the remaining variances and covariances
1579  zeroRows(P,18,21);
1580  zeroCols(P,18,21);
1581  P[18][18] = sq(frontend->_magNoise);
1582  P[19][19] = P[18][18];
1583  P[20][20] = P[18][18];
1584  P[21][21] = P[18][18];
1585 
1586  }
1587 
1588  // record the fact we have initialised the magnetic field states
1589  recordMagReset();
1590 
1591  // clear mag state reset request
1592  magStateResetRequest = false;
1593 
1594  } else {
1595  // this function should not be called if there is no compass data but if is is, return the
1596  // current attitude
1597  initQuat = stateStruct.quat;
1598  }
1599 
1600  // return attitude quaternion
1601  return initQuat;
1602 }
1603 
1604 // zero the attitude covariances, but preserve the variances
1605 void NavEKF3_core::zeroAttCovOnly()
1606 {
1607  float varTemp[4];
1608  for (uint8_t index=0; index<=3; index++) {
1609  varTemp[index] = P[index][index];
1610  }
1611  zeroCols(P,0,3);
1612  zeroRows(P,0,3);
1613  for (uint8_t index=0; index<=3; index++) {
1614  P[index][index] = varTemp[index];
1615  }
1616 }
1617 
1618 // calculate the variances for the rotation vector equivalent
1619 Vector3f NavEKF3_core::calcRotVecVariances()
1620 {
1621  Vector3f rotVarVec;
1622  float q0 = stateStruct.quat[0];
1623  float q1 = stateStruct.quat[1];
1624  float q2 = stateStruct.quat[2];
1625  float q3 = stateStruct.quat[3];
1626  if (q0 < 0) {
1627  q0 = -q0;
1628  q1 = -q1;
1629  q2 = -q2;
1630  q3 = -q3;
1631  }
1632  float t2 = q0*q0;
1633  float t3 = acosf(q0);
1634  float t4 = -t2+1.0f;
1635  float t5 = t2-1.0f;
1636  if ((t4 > 1e-9f) && (t5 < -1e-9f)) {
1637  float t6 = 1.0f/t5;
1638  float t7 = q1*t6*2.0f;
1639  float t8 = 1.0f/powf(t4,1.5f);
1640  float t9 = q0*q1*t3*t8*2.0f;
1641  float t10 = t7+t9;
1642  float t11 = 1.0f/sqrtf(t4);
1643  float t12 = q2*t6*2.0f;
1644  float t13 = q0*q2*t3*t8*2.0f;
1645  float t14 = t12+t13;
1646  float t15 = q3*t6*2.0f;
1647  float t16 = q0*q3*t3*t8*2.0f;
1648  float t17 = t15+t16;
1649  rotVarVec.x = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f;
1650  rotVarVec.y = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f;
1651  rotVarVec.z = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f;
1652  } else {
1653  rotVarVec.x = 4.0f * P[1][1];
1654  rotVarVec.y = 4.0f * P[2][2];
1655  rotVarVec.z = 4.0f * P[3][3];
1656  }
1657 
1658  return rotVarVec;
1659 }
1660 
1661 // initialise the quaternion covariances using rotation vector variances
1662 void NavEKF3_core::initialiseQuatCovariances(Vector3f &rotVarVec)
1663 {
1664  // calculate an equivalent rotation vector from the quaternion
1665  float q0 = stateStruct.quat[0];
1666  float q1 = stateStruct.quat[1];
1667  float q2 = stateStruct.quat[2];
1668  float q3 = stateStruct.quat[3];
1669  if (q0 < 0) {
1670  q0 = -q0;
1671  q1 = -q1;
1672  q2 = -q2;
1673  q3 = -q3;
1674  }
1675  float delta = 2.0f*acosf(q0);
1676  float scaler;
1677  if (fabsf(delta) > 1e-6f) {
1678  scaler = (delta/sinf(delta*0.5f));
1679  } else {
1680  scaler = 2.0f;
1681  }
1682  float rotX = scaler*q1;
1683  float rotY = scaler*q2;
1684  float rotZ = scaler*q3;
1685 
1686  // autocode generated using matlab symbolic toolbox
1687  float t2 = rotX*rotX;
1688  float t4 = rotY*rotY;
1689  float t5 = rotZ*rotZ;
1690  float t6 = t2+t4+t5;
1691  if (t6 > 1e-9f) {
1692  float t7 = sqrtf(t6);
1693  float t8 = t7*0.5f;
1694  float t3 = sinf(t8);
1695  float t9 = t3*t3;
1696  float t10 = 1.0f/t6;
1697  float t11 = 1.0f/sqrtf(t6);
1698  float t12 = cosf(t8);
1699  float t13 = 1.0f/powf(t6,1.5f);
1700  float t14 = t3*t11;
1701  float t15 = rotX*rotY*t3*t13;
1702  float t16 = rotX*rotZ*t3*t13;
1703  float t17 = rotY*rotZ*t3*t13;
1704  float t18 = t2*t10*t12*0.5f;
1705  float t27 = t2*t3*t13;
1706  float t19 = t14+t18-t27;
1707  float t23 = rotX*rotY*t10*t12*0.5f;
1708  float t28 = t15-t23;
1709  float t20 = rotY*rotVarVec.y*t3*t11*t28*0.5f;
1710  float t25 = rotX*rotZ*t10*t12*0.5f;
1711  float t31 = t16-t25;
1712  float t21 = rotZ*rotVarVec.z*t3*t11*t31*0.5f;
1713  float t22 = t20+t21-rotX*rotVarVec.x*t3*t11*t19*0.5f;
1714  float t24 = t15-t23;
1715  float t26 = t16-t25;
1716  float t29 = t4*t10*t12*0.5f;
1717  float t34 = t3*t4*t13;
1718  float t30 = t14+t29-t34;
1719  float t32 = t5*t10*t12*0.5f;
1720  float t40 = t3*t5*t13;
1721  float t33 = t14+t32-t40;
1722  float t36 = rotY*rotZ*t10*t12*0.5f;
1723  float t39 = t17-t36;
1724  float t35 = rotZ*rotVarVec.z*t3*t11*t39*0.5f;
1725  float t37 = t15-t23;
1726  float t38 = t17-t36;
1727  float t41 = rotVarVec.x*(t15-t23)*(t16-t25);
1728  float t42 = t41-rotVarVec.y*t30*t39-rotVarVec.z*t33*t39;
1729  float t43 = t16-t25;
1730  float t44 = t17-t36;
1731 
1732  // zero all the quaternion covariances
1733  zeroRows(P,0,3);
1734  zeroCols(P,0,3);
1735 
1736  // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
1737  P[0][0] = rotVarVec.x*t2*t9*t10*0.25f+rotVarVec.y*t4*t9*t10*0.25f+rotVarVec.z*t5*t9*t10*0.25f;
1738  P[0][1] = t22;
1739  P[0][2] = t35+rotX*rotVarVec.x*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarVec.y*t3*t11*t30*0.5f;
1740  P[0][3] = rotX*rotVarVec.x*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarVec.z*t3*t11*t33*0.5f;
1741  P[1][0] = t22;
1742  P[1][1] = rotVarVec.x*(t19*t19)+rotVarVec.y*(t24*t24)+rotVarVec.z*(t26*t26);
1743  P[1][2] = rotVarVec.z*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30;
1744  P[1][3] = rotVarVec.y*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33;
1745  P[2][0] = t35-rotY*rotVarVec.y*t3*t11*t30*0.5f+rotX*rotVarVec.x*t3*t11*(t15-t23)*0.5f;
1746  P[2][1] = rotVarVec.z*(t16-t25)*(t17-t36)-rotVarVec.x*t19*t28-rotVarVec.y*t28*t30;
1747  P[2][2] = rotVarVec.y*(t30*t30)+rotVarVec.x*(t37*t37)+rotVarVec.z*(t38*t38);
1748  P[2][3] = t42;
1749  P[3][0] = rotZ*rotVarVec.z*t3*t11*t33*(-0.5f)+rotX*rotVarVec.x*t3*t11*(t16-t25)*0.5f+rotY*rotVarVec.y*t3*t11*(t17-t36)*0.5f;
1750  P[3][1] = rotVarVec.y*(t15-t23)*(t17-t36)-rotVarVec.x*t19*t31-rotVarVec.z*t31*t33;
1751  P[3][2] = t42;
1752  P[3][3] = rotVarVec.z*(t33*t33)+rotVarVec.x*(t43*t43)+rotVarVec.y*(t44*t44);
1753 
1754  } else {
1755  // the equations are badly conditioned so use a small angle approximation
1756  P[0][0] = 0.0f;
1757  P[0][1] = 0.0f;
1758  P[0][2] = 0.0f;
1759  P[0][3] = 0.0f;
1760  P[1][0] = 0.0f;
1761  P[1][1] = 0.25f*rotVarVec.x;
1762  P[1][2] = 0.0f;
1763  P[1][3] = 0.0f;
1764  P[2][0] = 0.0f;
1765  P[2][1] = 0.0f;
1766  P[2][2] = 0.25f*rotVarVec.y;
1767  P[2][3] = 0.0f;
1768  P[3][0] = 0.0f;
1769  P[3][1] = 0.0f;
1770  P[3][2] = 0.0f;
1771  P[3][3] = 0.25f*rotVarVec.z;
1772 
1773  }
1774 }
1775 
1776 #endif // HAL_CPU_CLASS
NavEKF3_core::posTimeout
bool posTimeout
Definition: AP_NavEKF3_core.h:806
t42
t42
Definition: calcH_MAG.c:41
t2
t2
Definition: calcH_MAG.c:1
NavEKF3_core::magTimeout
bool magTimeout
Definition: AP_NavEKF3_core.h:808
NavEKF3_core::magSelectIndex
uint8_t magSelectIndex
Definition: AP_NavEKF3_core.h:947
norm
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
Quaternion::to_euler
void to_euler(float &roll, float &pitch, float &yaw) const
Definition: quaternion.cpp:272
NavEKF3_core::calcQuatAndFieldStates
Quaternion calcQuatAndFieldStates(float roll, float pitch)
Definition: AP_NavEKF3_core.cpp:1525
NavEKF3_core::q2
ftype q2
Definition: AP_NavEKF3_core.h:1210
NavEKF3::_hgtDelay_ms
AP_Int16 _hgtDelay_ms
Definition: AP_NavEKF3.h:383
NavEKF3_core::imu_elements::delVelDT
float delVelDT
Definition: AP_NavEKF3_core.h:444
NavEKF3_core::readBaroData
void readBaroData()
Definition: AP_NavEKF3_Measurements.cpp:602
NavEKF3_core::statesArray
Vector24 statesArray
Definition: AP_NavEKF3_core.h:422
EKF_TARGET_DT
#define EKF_TARGET_DT
Definition: AP_NavEKF2_core.h:52
NavEKF3_core::bodyVelTestRatio
Vector3 bodyVelTestRatio
Definition: AP_NavEKF3_core.h:1073
NavEKF3_core::activeHgtSource
uint8_t activeHgtSource
Definition: AP_NavEKF3_core.h:1142
t33
t33
Definition: calcH_MAG.c:34
GYRO_BIAS_LIMIT
#define GYRO_BIAS_LIMIT
Definition: SoloGimbalEKF.cpp:28
NavEKF3_core::zeroCols
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
Definition: AP_NavEKF3_core.cpp:1353
NavEKF3::_magEarthProcessNoise
AP_Float _magEarthProcessNoise
Definition: AP_NavEKF3.h:377
NavEKF3_core::posResetD
float posResetD
Definition: AP_NavEKF3_core.h:941
NavEKF3_core::_perf_test
AP_HAL::Util::perf_counter_t _perf_test[10]
Definition: AP_NavEKF3_core.h:1238
NavEKF3_core::StoreOutputReset
void StoreOutputReset(void)
Definition: AP_NavEKF3_core.cpp:1363
NavEKF3_core::lastPosPassTime_ms
uint32_t lastPosPassTime_ms
Definition: AP_NavEKF3_core.h:860
NavEKF3_core::ofStoreIndex
uint8_t ofStoreIndex
Definition: AP_NavEKF3_core.h:1012
NavEKF3_core::ekfGpsRefHgt
double ekfGpsRefHgt
Definition: AP_NavEKF3_core.h:961
NavEKF3::_fusionModeGPS
AP_Int8 _fusionModeGPS
Definition: AP_NavEKF3.h:384
NavEKF3_core::rangeStoreIndex
uint8_t rangeStoreIndex
Definition: AP_NavEKF3_core.h:909
NavEKF3_core::gpsSpdAccuracy
float gpsSpdAccuracy
Definition: AP_NavEKF3_core.h:885
NavEKF3_core::usingWheelSensors
bool usingWheelSensors
Definition: AP_NavEKF3_core.h:1083
NavEKF3_core::prevOnGround
bool prevOnGround
Definition: AP_NavEKF3_core.h:834
t37
t37
Definition: calcH_MAG.c:37
NavEKF3_core::badIMUdata
bool badIMUdata
Definition: AP_NavEKF3_core.h:811
t41
t41
Definition: calcH_MAG.c:23
NavEKF3_core::terrainHgtStableSet_ms
uint32_t terrainHgtStableSet_ms
Definition: AP_NavEKF3_core.h:1066
NavEKF3_core::gndHgtValidTime_ms
uint32_t gndHgtValidTime_ms
Definition: AP_NavEKF3_core.h:1021
NavEKF3_core::dt
ftype dt
Definition: AP_NavEKF3_core.h:831
AP_HAL::Util::perf_alloc
virtual perf_counter_t perf_alloc(perf_counter_type t, const char *name)
Definition: Util.h:102
NavEKF3_core::prevFlowFuseTime_ms
uint32_t prevFlowFuseTime_ms
Definition: AP_NavEKF3_core.h:1033
NavEKF3_core::q3
ftype q3
Definition: AP_NavEKF3_core.h:1211
Compass::get_declination
float get_declination() const
Definition: AP_Compass.cpp:1114
NavEKF3_core::posVelFusionDelayed
bool posVelFusionDelayed
Definition: AP_NavEKF3_core.h:931
NavEKF3_core::storedRngMeasTime_ms
uint32_t storedRngMeasTime_ms[2][3]
Definition: AP_NavEKF3_core.h:1062
NavEKF3_core::mag_elements::mag
Vector3f mag
Definition: AP_NavEKF3_core.h:457
NavEKF3_core::readGpsData
void readGpsData()
Definition: AP_NavEKF3_Measurements.cpp:455
NavEKF3_core::optFlowFusionDelayed
bool optFlowFusionDelayed
Definition: AP_NavEKF3_core.h:932
NavEKF3_core::magStateInitComplete
bool magStateInitComplete
Definition: AP_NavEKF3_core.h:896
NavEKF3_core::prevMotorsArmed
bool prevMotorsArmed
Definition: AP_NavEKF3_core.h:930
NavEKF3_core::lastInnovPassTime_ms
uint32_t lastInnovPassTime_ms
Definition: AP_NavEKF3_core.h:1000
NavEKF3_core::motorsArmed
bool motorsArmed
Definition: AP_NavEKF3_core.h:929
NavEKF3_core::quatAtLastMagReset
Quaternion quatAtLastMagReset
Definition: AP_NavEKF3_core.h:1164
NavEKF3_core::storedBodyOdm
obs_ring_buffer_t< vel_odm_elements > storedBodyOdm
Definition: AP_NavEKF3_core.h:1069
NavEKF3_core::setup_core
bool setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _core_index)
Definition: AP_NavEKF3_core.cpp:42
NavEKF3_core::P
Matrix24 P
Definition: AP_NavEKF3_core.h:817
t26
t26
Definition: calcH_MAG.c:27
nav_filter_status::horiz_vel
bool horiz_vel
Definition: AP_Nav_Common.h:22
NavEKF3_core::quat2Tbn
void quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
Definition: AP_NavEKF3_core.cpp:1401
NavEKF3_core::gpsYawResetRequest
bool gpsYawResetRequest
Definition: AP_NavEKF3_core.h:1161
AP_HAL.h
NavEKF3_core::secondLastGpsTime_ms
uint32_t secondLastGpsTime_ms
Definition: AP_NavEKF3_core.h:865
NavEKF3_core::expectGndEffectTouchdown
bool expectGndEffectTouchdown
Definition: AP_NavEKF3_core.h:1152
NavEKF3_core::maxOffsetStateChangeFilt
float maxOffsetStateChangeFilt
Definition: AP_NavEKF3_core.h:1122
NavEKF3_core::fuseOptFlowData
bool fuseOptFlowData
Definition: AP_NavEKF3_core.h:1015
NavEKF3_core::imuDataDelayed
imu_elements imuDataDelayed
Definition: AP_NavEKF3_core.h:898
NavEKF3_core::onGround
bool onGround
Definition: AP_NavEKF3_core.h:833
NavEKF3_core::accNavMag
ftype accNavMag
Definition: AP_NavEKF3_core.h:826
NavEKF3_core::beaconVehiclePosErr
float beaconVehiclePosErr
Definition: AP_NavEKF3_core.h:1103
NavEKF3_core::airSpdFusionDelayed
bool airSpdFusionDelayed
Definition: AP_NavEKF3_core.h:933
NavEKF3::_wndVarHgtRateScale
AP_Float _wndVarHgtRateScale
Definition: AP_NavEKF3.h:376
GCS.h
Interface definition for the various Ground Control System.
NavEKF3_core::lastBeaconIndex
uint8_t lastBeaconIndex
Definition: AP_NavEKF3_core.h:1111
NavEKF3_core::storedRngMeas
float storedRngMeas[2][3]
Definition: AP_NavEKF3_core.h:1061
NavEKF3::_baroAltNoise
AP_Float _baroAltNoise
Definition: AP_NavEKF3.h:372
t16
t16
Definition: calcH_MAG.c:15
NavEKF3_core::ResetHeight
void ResetHeight(void)
Definition: AP_NavEKF3_PosVelFusion.cpp:139
NavEKF3_core::storedMag
obs_ring_buffer_t< mag_elements > storedMag
Definition: AP_NavEKF3_core.h:820
NavEKF3_core::SelectBodyOdomFusion
void SelectBodyOdomFusion()
Definition: AP_NavEKF3_PosVelFusion.cpp:1553
NavEKF3_core::storedTAS
obs_ring_buffer_t< tas_elements > storedTAS
Definition: AP_NavEKF3_core.h:822
NavEKF3_core::lastRngBcnChecked
uint8_t lastRngBcnChecked
Definition: AP_NavEKF3_core.h:1106
NavEKF3_core::assume_zero_sideslip
bool assume_zero_sideslip(void) const
Definition: AP_NavEKF3_Control.cpp:462
AP_AHRS::get_home
const struct Location & get_home(void) const
Definition: AP_AHRS.h:435
NavEKF3_core::Vector14
ftype Vector14[14]
Definition: AP_NavEKF3_core.h:403
t24
t24
Definition: calcH_MAG.c:24
NavEKF3_core::delAngBodyOF
Vector3f delAngBodyOF
Definition: AP_NavEKF3_core.h:1052
t34
t34
Definition: calcH_MAG.c:69
NavEKF3_core::bodyOdmDataDelayed
vel_odm_elements bodyOdmDataDelayed
Definition: AP_NavEKF3_core.h:1071
NavEKF3_core::wheelOdmMeasTime_ms
uint32_t wheelOdmMeasTime_ms
Definition: AP_NavEKF3_core.h:1082
NavEKF3_core::gpsAccuracyGood
bool gpsAccuracyGood
Definition: AP_NavEKF3_core.h:1002
NavEKF3_core::_ahrs
const AP_AHRS * _ahrs
Definition: AP_NavEKF3_core.h:417
t28
t28
Definition: calcH_MAG.c:29
NavEKF3_core::varInnovRngBcn
float varInnovRngBcn
Definition: AP_NavEKF3_core.h:1098
NavEKF3_core::correctDeltaAngle
void correctDeltaAngle(Vector3f &delAng, float delAngDT)
Definition: AP_NavEKF3_core.cpp:601
NavEKF3_core::heldVelNE
Vector2f heldVelNE
Definition: AP_NavEKF3_core.h:1043
NavEKF3_core::storedOutput
imu_ring_buffer_t< output_elements > storedOutput
Definition: AP_NavEKF3_core.h:824
NavEKF3_core::tasStoreIndex
uint8_t tasStoreIndex
Definition: AP_NavEKF3_core.h:912
NavEKF3_core::inhibitGndState
bool inhibitGndState
Definition: AP_NavEKF3_core.h:1032
NavEKF3_core::N_beacons
uint8_t N_beacons
Definition: AP_NavEKF3_core.h:1115
NavEKF3_core::SelectMagFusion
void SelectMagFusion()
Definition: AP_NavEKF3_MagFusion.cpp:219
NavEKF3_core::gpsHorizVelFilt
float gpsHorizVelFilt
Definition: AP_NavEKF3_core.h:992
NavEKF3_core::InitialGyroBiasUncertainty
float InitialGyroBiasUncertainty(void) const
Definition: AP_NavEKF3_GyroBias.cpp:29
NavEKF3::_gpsHorizPosNoise
AP_Float _gpsHorizPosNoise
Definition: AP_NavEKF3.h:371
NavEKF3_core::StoreQuatReset
void StoreQuatReset(void)
Definition: AP_NavEKF3_core.cpp:1379
NavEKF3_core::SelectVelPosFusion
void SelectVelPosFusion()
Definition: AP_NavEKF3_PosVelFusion.cpp:227
NavEKF3_core::yawResetAngle
float yawResetAngle
Definition: AP_NavEKF3_core.h:892
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
Matrix3::from_euler
void from_euler(float roll, float pitch, float yaw)
Definition: matrix3.cpp:26
NavEKF3::tasDelay_ms
const uint16_t tasDelay_ms
Definition: AP_NavEKF3.h:426
NavEKF3_core::timeTasReceived_ms
uint32_t timeTasReceived_ms
Definition: AP_NavEKF3_core.h:925
NavEKF3_core::lastVelPassTime_ms
uint32_t lastVelPassTime_ms
Definition: AP_NavEKF3_core.h:859
Quaternion::from_euler
void from_euler(float roll, float pitch, float yaw)
Definition: quaternion.cpp:130
NavEKF3_core::maxBcnPosD
float maxBcnPosD
Definition: AP_NavEKF3_core.h:1116
NavEKF3_core::rngBcnAlignmentStarted
bool rngBcnAlignmentStarted
Definition: AP_NavEKF3_core.h:1109
t35
t35
Definition: calcH_MAG.c:35
NavEKF3_core::lastTasPassTime_ms
uint32_t lastTasPassTime_ms
Definition: AP_NavEKF3_core.h:862
NavEKF3_core::imuDataNew
imu_elements imuDataNew
Definition: AP_NavEKF3_core.h:899
NavEKF3_core::lastPosReset_ms
uint32_t lastPosReset_ms
Definition: AP_NavEKF3_core.h:938
gcs
GCS & gcs()
Definition: GCS_Common.cpp:3172
NavEKF3_core::gndOffsetValid
bool gndOffsetValid
Definition: AP_NavEKF3_core.h:1051
NavEKF3_core::ResetVelocity
void ResetVelocity(void)
Definition: AP_NavEKF3_PosVelFusion.cpp:20
NavEKF3_core::gpsNotAvailable
bool gpsNotAvailable
Definition: AP_NavEKF3_core.h:882
NavEKF3_core::lastPosResetD_ms
uint32_t lastPosResetD_ms
Definition: AP_NavEKF3_core.h:942
AP_HAL::Util::PC_ELAPSED
Definition: Util.h:98
NavEKF3_core::lastbodyVelPassTime_ms
uint32_t lastbodyVelPassTime_ms
Definition: AP_NavEKF3_core.h:1072
NavEKF3_core::accelPosOffset
Vector3f accelPosOffset
Definition: AP_NavEKF3_core.h:1056
NavEKF3_core::minBcnPosD
float minBcnPosD
Definition: AP_NavEKF3_core.h:1117
NavEKF3_core::sideSlipFusionDelayed
bool sideSlipFusionDelayed
Definition: AP_NavEKF3_core.h:934
t31
t31
Definition: calcH_MAG.c:26
NavEKF3::_magNoise
AP_Float _magNoise
Definition: AP_NavEKF3.h:373
NavEKF3_core::PV_AidingMode
AidingMode PV_AidingMode
Definition: AP_NavEKF3_core.h:1048
NavEKF3_core::baroHgtOffset
float baroHgtOffset
Definition: AP_NavEKF3_core.h:1059
NavEKF3_core::state_elements::earth_magfield
Vector3f earth_magfield
Definition: AP_NavEKF3_core.h:429
NavEKF3_core::UpdateStrapdownEquationsNED
void UpdateStrapdownEquationsNED()
Definition: AP_NavEKF3_core.cpp:618
NavEKF3_core::faultStatus
struct NavEKF3_core::@153 faultStatus
NavEKF3_core::Vector21
ftype Vector21[21]
Definition: AP_NavEKF3_core.h:405
NavEKF3_core::tasTimeout
bool tasTimeout
Definition: AP_NavEKF3_core.h:809
NavEKF3_core::state_elements::velocity
Vector3f velocity
Definition: AP_NavEKF3_core.h:425
t36
t36
Definition: calcH_MAG.c:36
NavEKF3_core::lastRngBcnPassTime_ms
uint32_t lastRngBcnPassTime_ms
Definition: AP_NavEKF3_core.h:1094
NavEKF3_core::velTimeout
bool velTimeout
Definition: AP_NavEKF3_core.h:805
t21
t21
Definition: calcH_MAG.c:20
NavEKF3_core::posDownAtLastMagReset
float posDownAtLastMagReset
Definition: AP_NavEKF3_core.h:1162
NavEKF3_core::velOffsetNED
Vector3f velOffsetNED
Definition: AP_NavEKF3_core.h:964
NavEKF3_core::flowValidMeaTime_ms
uint32_t flowValidMeaTime_ms
Definition: AP_NavEKF3_core.h:1018
NavEKF3_core::storedRangeBeacon
obs_ring_buffer_t< rng_bcn_elements > storedRangeBeacon
Definition: AP_NavEKF3_core.h:1090
NavEKF3_core::framesSincePredict
uint32_t framesSincePredict
Definition: AP_NavEKF3_core.h:949
NavEKF3_core::timeAtLastAuxEKF_ms
uint32_t timeAtLastAuxEKF_ms
Definition: AP_NavEKF3_core.h:864
NavEKF3_core::readMagData
void readMagData()
Definition: AP_NavEKF3_Measurements.cpp:236
NavEKF3_core::delAngCorrected
Vector3f delAngCorrected
Definition: AP_NavEKF3_core.h:953
Location::lat
int32_t lat
param 3 - Latitude * 10**7
Definition: AP_Common.h:143
MIN
#define MIN(a, b)
Definition: usb_conf.h:215
NavEKF3_core::calcOutputStates
void calcOutputStates()
Definition: AP_NavEKF3_core.cpp:691
NavEKF3_core::lastYawReset_ms
uint32_t lastYawReset_ms
Definition: AP_NavEKF3_core.h:893
NavEKF3_core::delAngBiasLearned
bool delAngBiasLearned
Definition: AP_NavEKF3_core.h:958
AP_HAL
Definition: AP_HAL_Namespace.h:6
MAX
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
NavEKF3_core::accNavMagHoriz
ftype accNavMagHoriz
Definition: AP_NavEKF3_core.h:827
NavEKF3_core::delTimeOF
float delTimeOF
Definition: AP_NavEKF3_core.h:1053
NavEKF3_core::prevPosN
float prevPosN
Definition: AP_NavEKF3_core.h:1027
NavEKF3_core::velDotNEDfilt
Vector3f velDotNEDfilt
Definition: AP_NavEKF3_core.h:854
NavEKF3_core::_perf_UpdateFilter
AP_HAL::Util::perf_counter_t _perf_UpdateFilter
Definition: AP_NavEKF3_core.h:1229
NavEKF3_core::stateStruct
struct NavEKF3_core::state_elements & stateStruct
NavEKF3_core::inhibitDelAngBiasStates
bool inhibitDelAngBiasStates
Definition: AP_NavEKF3_core.h:881
t13
t13
Definition: calcH_MAG.c:12
NavEKF3_core::zeroRows
void zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
Definition: AP_NavEKF3_core.cpp:1343
NavEKF3_core::gpsInhibit
bool gpsInhibit
Definition: AP_NavEKF3_core.h:1050
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::innovBodyVel
Vector3 innovBodyVel
Definition: AP_NavEKF3_core.h:1075
t27
t27
Definition: calcH_MAG.c:28
NavEKF3::sensorIntervalMin_ms
const uint8_t sensorIntervalMin_ms
Definition: AP_NavEKF3.h:448
Compass::get_primary
uint8_t get_primary(void) const
Definition: AP_Compass.h:277
delta
static int8_t delta
Definition: RCOutput.cpp:21
AP_AHRS::get_compass
const Compass * get_compass() const
Definition: AP_AHRS.h:150
NavEKF3_core::state_elements::gyro_bias
Vector3f gyro_bias
Definition: AP_NavEKF3_core.h:427
ins
static AP_InertialSensor ins
Definition: AHRS_Test.cpp:18
NavEKF3_core::rngBcnDataToFuse
bool rngBcnDataToFuse
Definition: AP_NavEKF3_core.h:1101
NavEKF3_core::use_compass
bool use_compass(void) const
Definition: AP_NavEKF3_Control.cpp:454
NavEKF3_core::localFilterTimeStep_ms
uint8_t localFilterTimeStep_ms
Definition: AP_NavEKF3_core.h:951
NavEKF3_core::state_elements
Definition: AP_NavEKF3_core.h:423
AP_InertialSensor::get_accel
const Vector3f & get_accel(uint8_t i) const
Definition: AP_InertialSensor.h:124
NavEKF3_core::gpsPosAccuracy
float gpsPosAccuracy
Definition: AP_NavEKF3_core.h:886
NavEKF3_core::rngBcnPosSum
Vector3f rngBcnPosSum
Definition: AP_NavEKF3_core.h:1112
NavEKF3_core::controlFilterModes
void controlFilterModes()
Definition: AP_NavEKF3_Control.cpp:15
NavEKF3_core::imu_elements::time_ms
uint32_t time_ms
Definition: AP_NavEKF3_core.h:445
NavEKF3_core::ForceSymmetry
void ForceSymmetry()
Definition: AP_NavEKF3_core.cpp:1408
t39
t39
Definition: calcH_MAG.c:39
Vector2::y
T y
Definition: vector2.h:37
NavEKF3_core::posDownDerivative
float posDownDerivative
Definition: AP_NavEKF3_core.h:984
NavEKF3_core::lastDecayTime_ms
uint32_t lastDecayTime_ms
Definition: AP_NavEKF3_core.h:872
NavEKF3_core::recordMagReset
void recordMagReset()
Definition: AP_NavEKF3_MagFusion.cpp:1185
NavEKF3_core::manoeuvring
bool manoeuvring
Definition: AP_NavEKF3_core.h:837
NavEKF3_core::flowMeaTime_ms
uint32_t flowMeaTime_ms
Definition: AP_NavEKF3_core.h:1020
NavEKF3_core::numBcnMeas
uint8_t numBcnMeas
Definition: AP_NavEKF3_core.h:1113
NavEKF3_core::InitialiseFilterBootstrap
bool InitialiseFilterBootstrap(void)
Definition: AP_NavEKF3_core.cpp:397
NavEKF3_core::bcnPosOffsetNED
Vector3f bcnPosOffsetNED
Definition: AP_NavEKF3_core.h:1128
NavEKF3_core::rngMeasIndex
uint8_t rngMeasIndex[2]
Definition: AP_NavEKF3_core.h:1064
NavEKF3_core::SelectBetaFusion
void SelectBetaFusion()
Definition: AP_NavEKF3_AirDataFusion.cpp:231
NavEKF3_core::posOffsetNED
Vector3f posOffsetNED
Definition: AP_NavEKF3_core.h:965
NavEKF3_core::AID_NONE
Definition: AP_NavEKF3_core.h:1045
NavEKF3_core::rngBcnTestRatio
float rngBcnTestRatio
Definition: AP_NavEKF3_core.h:1095
NavEKF3_core::innovRngBcn
float innovRngBcn
Definition: AP_NavEKF3_core.h:1099
NavEKF3_core::beaconVehiclePosNED
Vector3f beaconVehiclePosNED
Definition: AP_NavEKF3_core.h:1102
NavEKF3_core::outputDataNew
output_elements outputDataNew
Definition: AP_NavEKF3_core.h:919
NavEKF3_core::lastInitFailReport_ms
uint32_t lastInitFailReport_ms
Definition: AP_NavEKF3_core.h:967
NavEKF3_core::calcRotVecVariances
Vector3f calcRotVecVariances(void)
Definition: AP_NavEKF3_core.cpp:1619
NavEKF3_core::StoreQuatRotate
void StoreQuatRotate(Quaternion deltaQuat)
Definition: AP_NavEKF3_core.cpp:1390
NavEKF3_core::bcnPosDownOffsetMax
float bcnPosDownOffsetMax
Definition: AP_NavEKF3_core.h:1120
NavEKF3_core::rngBcnStoreIndex
uint8_t rngBcnStoreIndex
Definition: AP_NavEKF3_core.h:1093
NavEKF3::_windVelProcessNoise
AP_Float _windVelProcessNoise
Definition: AP_NavEKF3.h:375
AP_HAL::HAL
Definition: HAL.h:26
NavEKF3_core::state_elements::position
Vector3f position
Definition: AP_NavEKF3_core.h:426
AP_InertialSensor
Definition: AP_InertialSensor.h:47
NavEKF3_core::output_elements::quat
Quaternion quat
Definition: AP_NavEKF3_core.h:435
wrap_PI
float wrap_PI(const T radian)
Definition: AP_Math.cpp:152
NavEKF3_core::minOffsetStateChangeFilt
float minOffsetStateChangeFilt
Definition: AP_NavEKF3_core.h:1126
t12
t12
Definition: calcH_MAG.c:11
NavEKF3_core::lastKnownPositionNE
Vector2f lastKnownPositionNE
Definition: AP_NavEKF3_core.h:871
NavEKF3_core::readIMUData
void readIMUData()
Definition: AP_NavEKF3_Measurements.cpp:330
NavEKF3_core::ConstrainStates
void ConstrainStates()
Definition: AP_NavEKF3_core.cpp:1488
NavEKF3_core::PV_AidingModePrev
AidingMode PV_AidingModePrev
Definition: AP_NavEKF3_core.h:1049
NavEKF3_core::lastHgtPassTime_ms
uint32_t lastHgtPassTime_ms
Definition: AP_NavEKF3_core.h:861
NavEKF3_core::expectGndEffectTakeoff
bool expectGndEffectTakeoff
Definition: AP_NavEKF3_core.h:1150
NavEKF3_core::delAngCorrection
Vector3f delAngCorrection
Definition: AP_NavEKF3_core.h:921
NavEKF3_core::ekfInnovationsPass
bool ekfInnovationsPass
Definition: AP_NavEKF3_core.h:996
GRAVITY_MSS
#define GRAVITY_MSS
Definition: definitions.h:47
NavEKF3_core::magYawResetRequest
bool magYawResetRequest
Definition: AP_NavEKF3_core.h:1160
NavEKF3_core::obs_buffer_length
uint8_t obs_buffer_length
Definition: AP_NavEKF3_core.h:363
NavEKF3_core::receiverPosCov
float receiverPosCov[3][3]
Definition: AP_NavEKF3_core.h:1108
NavEKF3_core::InitialiseVariables
void InitialiseVariables()
Definition: AP_NavEKF3_core.cpp:155
NavEKF3::_magBodyProcessNoise
AP_Float _magBodyProcessNoise
Definition: AP_NavEKF3.h:378
f
#define f(i)
NavEKF3_core::lastInnovation
float lastInnovation
Definition: AP_NavEKF3_core.h:1006
NavEKF3_core::yawInnovAtLastMagReset
float yawInnovAtLastMagReset
Definition: AP_NavEKF3_core.h:1163
NavEKF3_core::gpsSpdAccPass
bool gpsSpdAccPass
Definition: AP_NavEKF3_core.h:995
NavEKF3_core::terrainState
float terrainState
Definition: AP_NavEKF3_core.h:1026
NavEKF3_core::outputTrackError
Vector3f outputTrackError
Definition: AP_NavEKF3_core.h:963
NavEKF3_core::lastTimeGpsReceived_ms
uint32_t lastTimeGpsReceived_ms
Definition: AP_NavEKF3_core.h:863
Vector3::y
T y
Definition: vector3.h:67
NavEKF3_core::lastPreAlignGpsCheckTime_ms
uint32_t lastPreAlignGpsCheckTime_ms
Definition: AP_NavEKF3_core.h:989
NavEKF3_core::UpdateFilter
void UpdateFilter(bool predict)
Definition: AP_NavEKF3_core.cpp:534
NavEKF3_core::dtEkfAvg
ftype dtEkfAvg
Definition: AP_NavEKF3_core.h:830
NavEKF3_core::hgtRate
ftype hgtRate
Definition: AP_NavEKF3_core.h:832
NavEKF3_core::storedGPS
obs_ring_buffer_t< gps_elements > storedGPS
Definition: AP_NavEKF3_core.h:819
t7
t7
Definition: calcH_MAG.c:6
AP_HAL::millis
uint32_t millis()
Definition: system.cpp:157
NavEKF3_core::velResetSource
resetDataSource velResetSource
Definition: AP_NavEKF3_core.h:981
NavEKF3_core::q0
ftype q0
Definition: AP_NavEKF3_core.h:1208
NavEKF3_core::prevPosE
float prevPosE
Definition: AP_NavEKF3_core.h:1028
NavEKF3_core::ekfStartTime_ms
uint32_t ekfStartTime_ms
Definition: AP_NavEKF3_core.h:869
NavEKF3_core::state_elements::wind_vel
Vector2f wind_vel
Definition: AP_NavEKF3_core.h:431
NavEKF3_core::imu_index
uint8_t imu_index
Definition: AP_NavEKF3_core.h:360
NavEKF3_core::prevInFlight
bool prevInFlight
Definition: AP_NavEKF3_core.h:836
AP_HAL::Util::perf_end
virtual void perf_end(perf_counter_t h)
Definition: Util.h:104
NavEKF3_core::lastRngMeasTime_ms
uint32_t lastRngMeasTime_ms
Definition: AP_NavEKF3_core.h:1063
t40
t40
Definition: calcH_MAG.c:40
NavEKF3_core::output_elements::velocity
Vector3f velocity
Definition: AP_NavEKF3_core.h:436
NavEKF3_core::storedRange
obs_ring_buffer_t< range_elements > storedRange
Definition: AP_NavEKF3_core.h:823
Vector3::z
T z
Definition: vector3.h:67
NavEKF3_core::magFieldLearned
bool magFieldLearned
Definition: AP_NavEKF3_core.h:955
NavEKF3_core::rngBcnFuseDataReportIndex
uint8_t rngBcnFuseDataReportIndex
Definition: AP_NavEKF3_core.h:1132
NavEKF3_core::inFlight
bool inFlight
Definition: AP_NavEKF3_core.h:835
NavEKF3_core::velResetNE
Vector2f velResetNE
Definition: AP_NavEKF3_core.h:939
NavEKF3_core::stateIndexLim
uint8_t stateIndexLim
Definition: AP_NavEKF3_core.h:897
NavEKF3_core::bcnPosOffsetMaxVar
float bcnPosOffsetMaxVar
Definition: AP_NavEKF3_core.h:1121
NavEKF3_core::updateFilterStatus
void updateFilterStatus(void)
Definition: AP_NavEKF3_Control.cpp:534
NavEKF3_core::output_elements
Definition: AP_NavEKF3_core.h:434
ACCEL_BIAS_LIM_SCALER
#define ACCEL_BIAS_LIM_SCALER
Definition: AP_NavEKF3_core.h:58
NavEKF3_core::rngBcnTimeout
bool rngBcnTimeout
Definition: AP_NavEKF3_core.h:1097
NavEKF3_core::core_index
uint8_t core_index
Definition: AP_NavEKF3_core.h:361
NavEKF3_core::velDotNED
Vector3f velDotNED
Definition: AP_NavEKF3_core.h:853
t43
t43
Definition: calcH_MAG.c:42
NavEKF3_core::rngBcnGoodToAlign
bool rngBcnGoodToAlign
Definition: AP_NavEKF3_core.h:1105
NavEKF3_core::gpsHgtAccuracy
float gpsHgtAccuracy
Definition: AP_NavEKF3_core.h:887
NavEKF3_core::_perf_CovariancePrediction
AP_HAL::Util::perf_counter_t _perf_CovariancePrediction
Definition: AP_NavEKF3_core.h:1230
AP_NavEKF3_core.h
NavEKF3_core::magStateResetRequest
bool magStateResetRequest
Definition: AP_NavEKF3_core.h:1159
NavEKF3_core::bcnOriginEstInit
bool bcnOriginEstInit
Definition: AP_NavEKF3_core.h:1129
NavEKF3::imuSampleTime_us
uint64_t imuSampleTime_us
Definition: AP_NavEKF3.h:459
Quaternion::rotation_matrix
void rotation_matrix(Matrix3f &m) const
Definition: quaternion.cpp:24
t38
t38
Definition: calcH_MAG.c:38
NavEKF3_core::inhibitWindStates
bool inhibitWindStates
Definition: AP_NavEKF3_core.h:878
NavEKF3_core::allMagSensorsFailed
bool allMagSensorsFailed
Definition: AP_NavEKF3_core.h:867
NavEKF3_core::prevTnb
Matrix3f prevTnb
Definition: AP_NavEKF3_core.h:825
NavEKF3_core::nextP
Matrix24 nextP
Definition: AP_NavEKF3_core.h:870
NavEKF3_core::posResetSource
resetDataSource posResetSource
Definition: AP_NavEKF3_core.h:980
NavEKF3_core::gpsloc_prev
struct Location gpsloc_prev
Definition: AP_NavEKF3_core.h:988
GCS::send_text
void send_text(MAV_SEVERITY severity, const char *fmt,...)
Definition: GCS.cpp:8
NavEKF3_core::magYawResetTimer_ms
uint32_t magYawResetTimer_ms
Definition: AP_NavEKF3_core.h:927
Quaternion::from_axis_angle
void from_axis_angle(Vector3f v)
Definition: quaternion.cpp:154
NavEKF3_core::bodyOdmDataNew
vel_odm_elements bodyOdmDataNew
Definition: AP_NavEKF3_core.h:1070
NavEKF3_core::rngBcnFusionReport
struct NavEKF3_core::@152 rngBcnFusionReport[10]
AP_InertialSensor::get_sample_rate
uint16_t get_sample_rate(void) const
Definition: AP_InertialSensor.h:196
NavEKF3_core::magStoreIndex
uint8_t magStoreIndex
Definition: AP_NavEKF3_core.h:915
Vector3::normalize
void normalize()
Definition: vector3.h:176
NavEKF3_core::gpsGoodToAlign
bool gpsGoodToAlign
Definition: AP_NavEKF3_core.h:926
t14
t14
Definition: calcH_MAG.c:13
NavEKF3_core::badMagYaw
bool badMagYaw
Definition: AP_NavEKF3_core.h:810
NavEKF3_core::last_gps_idx
uint8_t last_gps_idx
Definition: AP_NavEKF3_core.h:918
NavEKF3_core::rangeDataToFuse
bool rangeDataToFuse
Definition: AP_NavEKF3_core.h:1039
AP_AHRS.h
AP_Vehicle.h
NavEKF3_core::Popt
float Popt
Definition: AP_NavEKF3_core.h:1025
NavEKF3_core::storedIMU
imu_ring_buffer_t< imu_elements > storedIMU
Definition: AP_NavEKF3_core.h:818
NavEKF3_core::terrainHgtStable
bool terrainHgtStable
Definition: AP_NavEKF3_core.h:1065
Vector3::length
float length(void) const
Definition: vector3.cpp:288
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
Matrix3::mul_transpose
Vector3< T > mul_transpose(const Vector3< T > &v) const
Definition: matrix3.cpp:165
NavEKF3_core::inhibitDelVelBiasStates
bool inhibitDelVelBiasStates
Definition: AP_NavEKF3_core.h:880
NavEKF3_core::outputDataDelayed
output_elements outputDataDelayed
Definition: AP_NavEKF3_core.h:920
NavEKF3_core::lastGpsCheckTime_ms
uint32_t lastGpsCheckTime_ms
Definition: AP_NavEKF3_core.h:999
Matrix3::zero
void zero(void)
Definition: matrix3.cpp:238
NavEKF3_core::gpsDriftNE
float gpsDriftNE
Definition: AP_NavEKF3_core.h:990
nav_filter_status::flags
struct nav_filter_status::@138 flags
NavEKF3_core::velErrintegral
Vector3f velErrintegral
Definition: AP_NavEKF3_core.h:922
NavEKF3_core::rngBcnDataNew
rng_bcn_elements rngBcnDataNew
Definition: AP_NavEKF3_core.h:1091
NavEKF3_core::magDataDelayed
mag_elements magDataDelayed
Definition: AP_NavEKF3_core.h:914
t23
t23
Definition: calcH_MAG.c:22
NavEKF3_core::imu_elements::delAngDT
float delAngDT
Definition: AP_NavEKF3_core.h:443
NavEKF3_core::storedOF
obs_ring_buffer_t< of_elements > storedOF
Definition: AP_NavEKF3_core.h:1009
NavEKF3_core::gpsCheckStatus
struct NavEKF3_core::@154 gpsCheckStatus
t15
t15
Definition: calcH_MAG.c:14
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
t3
t3
Definition: calcH_MAG.c:2
NavEKF3_core::rngSum
float rngSum
Definition: AP_NavEKF3_core.h:1114
NavEKF3::magDelay_ms
const uint16_t magDelay_ms
Definition: AP_NavEKF3.h:425
NavEKF3::_accBiasLim
AP_Float _accBiasLim
Definition: AP_NavEKF3.h:413
AP_NavEKF3.h
NavEKF3_core::lastMagUpdate_us
uint32_t lastMagUpdate_us
Definition: AP_NavEKF3_core.h:852
t8
t8
Definition: calcH_MAG.c:7
NavEKF3_core::touchdownExpectedSet_ms
uint32_t touchdownExpectedSet_ms
Definition: AP_NavEKF3_core.h:1153
AP_HAL::Util::perf_begin
virtual void perf_begin(perf_counter_t h)
Definition: Util.h:103
radians
static constexpr float radians(float deg)
Definition: AP_Math.h:158
EKF_TARGET_DT_MS
#define EKF_TARGET_DT_MS
Definition: AP_NavEKF3_core.h:61
t5
t5
Definition: calcH_MAG.c:4
NavEKF3_core::calcEarthRateNED
void calcEarthRateNED(Vector3f &omega, int32_t latitude) const
Definition: AP_NavEKF3_core.cpp:1515
Vector2::zero
void zero()
Definition: vector2.h:125
NavEKF3_core::sAccFilterState2
float sAccFilterState2
Definition: AP_NavEKF3_core.h:998
NavEKF3_core::CovariancePrediction
void CovariancePrediction()
Definition: AP_NavEKF3_core.cpp:835
NavEKF3_core::state_elements::body_magfield
Vector3f body_magfield
Definition: AP_NavEKF3_core.h:430
NavEKF3_core::mag_state
struct NavEKF3_core::@155 mag_state
NavEKF3_core::yawAlignComplete
bool yawAlignComplete
Definition: AP_NavEKF3_core.h:895
t4
t4
Definition: calcH_MAG.c:3
NavEKF3_core::prevBetaStep_ms
uint32_t prevBetaStep_ms
Definition: AP_NavEKF3_core.h:851
NavEKF3_core::sAccFilterState1
float sAccFilterState1
Definition: AP_NavEKF3_core.h:997
NavEKF3::_flowDelay_ms
AP_Int8 _flowDelay_ms
Definition: AP_NavEKF3.h:394
NavEKF3_core::storedWheelOdm
obs_ring_buffer_t< wheel_odm_elements > storedWheelOdm
Definition: AP_NavEKF3_core.h:1084
NavEKF3_core::Vector8
ftype Vector8[8]
Definition: AP_NavEKF3_core.h:398
NavEKF3_core::posResetNE
Vector2f posResetNE
Definition: AP_NavEKF3_core.h:937
NavEKF3_core::posDown
float posDown
Definition: AP_NavEKF3_core.h:985
NavEKF3_core::rngBcnHealth
bool rngBcnHealth
Definition: AP_NavEKF3_core.h:1096
earthRate
#define earthRate
Definition: AP_NavEKF2_core.cpp:14
t25
t25
Definition: calcH_MAG.c:25
NavEKF3_core::rngBcnAlignmentCompleted
bool rngBcnAlignmentCompleted
Definition: AP_NavEKF3_core.h:1110
NavEKF3_core::rngValidMeaTime_ms
uint32_t rngValidMeaTime_ms
Definition: AP_NavEKF3_core.h:1019
t29
t29
Definition: calcH_MAG.c:30
NavEKF3_core::takeoffExpectedSet_ms
uint32_t takeoffExpectedSet_ms
Definition: AP_NavEKF3_core.h:1151
t18
t18
Definition: calcH_MAG.c:16
NavEKF3_core::rngBcnLast3DmeasTime_ms
uint32_t rngBcnLast3DmeasTime_ms
Definition: AP_NavEKF3_core.h:1104
NavEKF3_core::flowGyroBias
Vector2f flowGyroBias
Definition: AP_NavEKF3_core.h:1038
NavEKF3_core::lastSynthYawTime_ms
uint32_t lastSynthYawTime_ms
Definition: AP_NavEKF3_core.h:868
NavEKF3_core::posErrintegral
Vector3f posErrintegral
Definition: AP_NavEKF3_core.h:923
sq
float sq(const T val)
Definition: AP_Math.h:170
t6
t6
Definition: calcH_MAG.c:5
NavEKF3_core::gpsVertVelFilt
float gpsVertVelFilt
Definition: AP_NavEKF3_core.h:991
NavEKF3_core::alignMagStateDeclination
void alignMagStateDeclination()
Definition: AP_NavEKF3_MagFusion.cpp:1152
NavEKF3_core::ResetPosition
void ResetPosition(void)
Definition: AP_NavEKF3_PosVelFusion.cpp:76
NavEKF3::_tauVelPosOutput
AP_Int8 _tauVelPosOutput
Definition: AP_NavEKF3.h:406
NavEKF3_core::baroStoreIndex
uint8_t baroStoreIndex
Definition: AP_NavEKF3_core.h:906
Quaternion::inverse
Quaternion inverse(void) const
Definition: quaternion.cpp:292
Vector3::is_zero
bool is_zero(void) const
Definition: vector3.h:159
t11
t11
Definition: calcH_MAG.c:10
NavEKF3_core::rngOnGnd
float rngOnGnd
Definition: AP_NavEKF3_core.h:1060
NavEKF3_core::hgtTimeout
bool hgtTimeout
Definition: AP_NavEKF3_core.h:807
NavEKF3_core::flowFusionActive
bool flowFusionActive
Definition: AP_NavEKF3_core.h:1054
t32
t32
Definition: calcH_MAG.c:32
NavEKF3_core::statesInitialised
bool statesInitialised
Definition: AP_NavEKF3_core.h:799
NavEKF3_core::validOrigin
bool validOrigin
Definition: AP_NavEKF3_core.h:884
NavEKF3_core::tiltAlignComplete
bool tiltAlignComplete
Definition: AP_NavEKF3_core.h:894
NavEKF3_core::SelectTasFusion
void SelectTasFusion()
Definition: AP_NavEKF3_AirDataFusion.cpp:200
NavEKF3_core::state_elements::accel_bias
Vector3f accel_bias
Definition: AP_NavEKF3_core.h:428
NavEKF3_core::lastVelReset_ms
uint32_t lastVelReset_ms
Definition: AP_NavEKF3_core.h:940
NavEKF3_core::innovationIncrement
float innovationIncrement
Definition: AP_NavEKF3_core.h:1005
t19
t19
Definition: calcH_MAG.c:18
AP::ins
AP_InertialSensor & ins()
Definition: AP_InertialSensor.cpp:1981
Quaternion::rotate
void rotate(const Vector3f &v)
Definition: quaternion.cpp:182
AP::gps
AP_GPS & gps()
Definition: AP_GPS.cpp:1550
t10
t10
Definition: calcH_MAG.c:9
NavEKF3_core::SelectFlowFusion
void SelectFlowFusion()
Definition: AP_NavEKF3_OptFlowFusion.cpp:22
t20
t20
Definition: calcH_MAG.c:19
t17
t17
Definition: calcH_MAG.c:17
t9
t9
Definition: calcH_MAG.c:8
NavEKF3_core::ConstrainVariances
void ConstrainVariances()
Definition: AP_NavEKF3_core.cpp:1423
AP_AHRS::airspeed_sensor_enabled
bool airspeed_sensor_enabled(void) const
Definition: AP_AHRS.h:299
NavEKF3_core::earthRateNED
Vector3f earthRateNED
Definition: AP_NavEKF3_core.h:828
NavEKF3_core::imuDataDownSampledNew
imu_elements imuDataDownSampledNew
Definition: AP_NavEKF3_core.h:900
NavEKF3_core::firstInitTime_ms
uint32_t firstInitTime_ms
Definition: AP_NavEKF3_core.h:966
NavEKF3_core::dtIMUavg
ftype dtIMUavg
Definition: AP_NavEKF3_core.h:829
NavEKF3::_rngBcnDelay_ms
AP_Int8 _rngBcnDelay_ms
Definition: AP_NavEKF3.h:411
NavEKF3::_accelBiasProcessNoise
AP_Float _accelBiasProcessNoise
Definition: AP_NavEKF3.h:382
AP_GPS::GPS_OK_FIX_3D
Receiving valid messages and 3D lock.
Definition: AP_GPS.h:100
NavEKF3::_gpsVertVelNoise
AP_Float _gpsVertVelNoise
Definition: AP_NavEKF3.h:370
NavEKF3_core::zeroAttCovOnly
void zeroAttCovOnly()
Definition: AP_NavEKF3_core.cpp:1605
NavEKF3_core::lastGpsAidBadTime_ms
uint32_t lastGpsAidBadTime_ms
Definition: AP_NavEKF3_core.h:889
NavEKF3_core::Vector11
ftype Vector11[11]
Definition: AP_NavEKF3_core.h:401
NavEKF3::_gyrNoise
AP_Float _gyrNoise
Definition: AP_NavEKF3.h:379
NavEKF3_core::lastBaroReceived_ms
uint32_t lastBaroReceived_ms
Definition: AP_NavEKF3_core.h:857
NavEKF3_core::hgtInnovFiltState
float hgtInnovFiltState
Definition: AP_NavEKF3_core.h:946
NavEKF3_core::finalInflightMagInit
bool finalInflightMagInit
Definition: AP_NavEKF3_core.h:1158
NavEKF3_core::q1
ftype q1
Definition: AP_NavEKF3_core.h:1209
t30
t30
Definition: calcH_MAG.c:31
NavEKF3_core::Matrix24
ftype Matrix24[24][24]
Definition: AP_NavEKF3_core.h:412
NavEKF3_core::DEFAULT
Definition: AP_NavEKF3_core.h:972
NavEKF3_core::bodyVelFusionDelayed
bool bodyVelFusionDelayed
Definition: AP_NavEKF3_core.h:1078
NavEKF3_core::gpsNoiseScaler
float gpsNoiseScaler
Definition: AP_NavEKF3_core.h:813
NavEKF3_core::imu_elements::delAng
Vector3f delAng
Definition: AP_NavEKF3_core.h:441
NavEKF3_core::bodyOdmMeasTime_ms
uint32_t bodyOdmMeasTime_ms
Definition: AP_NavEKF3_core.h:1077
NavEKF3_core::frontend
NavEKF3 * frontend
Definition: AP_NavEKF3_core.h:359
t44
t44
Definition: calcH_MAG.c:43
NavEKF3::_gyroBiasProcessNoise
AP_Float _gyroBiasProcessNoise
Definition: AP_NavEKF3.h:381
NavEKF3_core::lastMagOffsetsValid
bool lastMagOffsetsValid
Definition: AP_NavEKF3_core.h:936
AP_InertialSensor::get_loop_delta_t
float get_loop_delta_t(void) const
Definition: AP_InertialSensor.h:199
NavEKF3_core::flowDataValid
bool flowDataValid
Definition: AP_NavEKF3_core.h:1014
NavEKF3_core::CovarianceInit
void CovarianceInit()
Definition: AP_NavEKF3_core.cpp:484
NavEKF3_core::filterStatus
nav_filter_status filterStatus
Definition: AP_NavEKF3_core.h:959
NavEKF3_core::correctDeltaVelocity
void correctDeltaVelocity(Vector3f &delVel, float delVelDT)
Definition: AP_NavEKF3_core.cpp:606
NavEKF3_core::storedBaro
obs_ring_buffer_t< baro_elements > storedBaro
Definition: AP_NavEKF3_core.h:821
t22
t22
Definition: calcH_MAG.c:21
NavEKF3_core::rngBcnDataDelayed
rng_bcn_elements rngBcnDataDelayed
Definition: AP_NavEKF3_core.h:1092
NavEKF3_core::startPredictEnabled
bool startPredictEnabled
Definition: AP_NavEKF3_core.h:950
NavEKF3_core::imuSampleTime_ms
uint32_t imuSampleTime_ms
Definition: AP_NavEKF3_core.h:855
NavEKF3_core::bcnPosOffsetMinVar
float bcnPosOffsetMinVar
Definition: AP_NavEKF3_core.h:1125
NavEKF3_core::SelectRngBcnFusion
void SelectRngBcnFusion()
Definition: AP_NavEKF3_RngBcnFusion.cpp:18
NavEKF3_core::varInnovBodyVel
Vector3 varInnovBodyVel
Definition: AP_NavEKF3_core.h:1074
NavEKF3_core::ekfOriginHgtVar
float ekfOriginHgtVar
Definition: AP_NavEKF3_core.h:960
NavEKF3_core::runUpdates
bool runUpdates
Definition: AP_NavEKF3_core.h:948
NavEKF3_core::output_elements::position
Vector3f position
Definition: AP_NavEKF3_core.h:437
NavEKF3_core::NavEKF3_core
NavEKF3_core(void)
Definition: AP_NavEKF3_core.cpp:14
NavEKF3_core::prevBodyVelFuseTime_ms
uint32_t prevBodyVelFuseTime_ms
Definition: AP_NavEKF3_core.h:1076
NavEKF3_core::finalInflightYawInit
bool finalInflightYawInit
Definition: AP_NavEKF3_core.h:1157
NavEKF3_core::lastTimeRngBcn_ms
uint32_t lastTimeRngBcn_ms[10]
Definition: AP_NavEKF3_core.h:1100
NavEKF3_core::imu_elements::delVel
Vector3f delVel
Definition: AP_NavEKF3_core.h:442
Vector3::zero
void zero()
Definition: vector3.h:182
NavEKF3_core::lastGpsVelFail_ms
uint32_t lastGpsVelFail_ms
Definition: AP_NavEKF3_core.h:888
NavEKF3_core::lastHealthyMagTime_ms
uint32_t lastHealthyMagTime_ms
Definition: AP_NavEKF3_core.h:866
NavEKF3_core::delVelCorrected
Vector3f delVelCorrected
Definition: AP_NavEKF3_core.h:954
Vector3::x
T x
Definition: vector3.h:67
NavEKF3_core::bodyVelFusionActive
bool bodyVelFusionActive
Definition: AP_NavEKF3_core.h:1079
NavEKF3_core::initialiseQuatCovariances
void initialiseQuatCovariances(Vector3f &rotVarVec)
Definition: AP_NavEKF3_core.cpp:1662
NavEKF3_core::imu_buffer_length
uint8_t imu_buffer_length
Definition: AP_NavEKF3_core.h:362
NavEKF3::_gpsHorizVelNoise
AP_Float _gpsHorizVelNoise
Definition: AP_NavEKF3.h:369
NavEKF3_core::lastInnovFailTime_ms
uint32_t lastInnovFailTime_ms
Definition: AP_NavEKF3_core.h:1001
NavEKF3::_ahrs
const AP_AHRS * _ahrs
Definition: AP_NavEKF3.h:361
NavEKF3_core::bcnPosDownOffsetMin
float bcnPosDownOffsetMin
Definition: AP_NavEKF3_core.h:1124
NavEKF3_core::receiverPos
Vector3f receiverPos
Definition: AP_NavEKF3_core.h:1107
NavEKF3::_accNoise
AP_Float _accNoise
Definition: AP_NavEKF3.h:380
Generated on Sun Jun 17 2018 14:18:50 for APM:Libraries by   doxygen 1.8.13