Line data Source code
1 : #include "AP_NavEKF2_core.h"
2 :
3 : #include <AP_HAL/AP_HAL.h>
4 : #include <AP_DAL/AP_DAL.h>
5 : #include <GCS_MAVLink/GCS.h>
6 :
7 : #include "AP_NavEKF2.h"
8 :
9 : extern const AP_HAL::HAL& hal;
10 :
11 : #define earthRate 0.000072921f // earth rotation rate (rad/sec)
12 :
13 : // initial imu bias uncertainty (deg/sec)
14 : #define INIT_ACCEL_BIAS_UNCERTAINTY 0.5f
15 :
16 : // maximum allowed gyro bias (rad/sec)
17 : #define GYRO_BIAS_LIMIT 0.5f
18 :
19 : /*
20 : to run EK2 timing tests you need to set ENABLE_EKF_TIMING to 1, plus setup as follows:
21 : - copter at 400Hz
22 : - INS_FAST_SAMPLE=0
23 : - EKF2_MAG_CAL=4
24 : - GPS_TYPE=14
25 : - load fakegps in mavproxy
26 : - ensure a compass is enabled
27 : - wait till EK2 reports "using GPS" (this is important, ignore earlier results)
28 :
29 : DO NOT FLY WITH THIS ENABLED
30 : */
31 : #define ENABLE_EKF_TIMING 0
32 :
33 : NavEKF2_core::Matrix24 NavEKF2_core::KH;
34 : NavEKF2_core::Matrix24 NavEKF2_core::nextP;
35 :
36 : // constructor
37 1022 : NavEKF2_core::NavEKF2_core(NavEKF2 *_frontend) :
38 1022 : dal(AP::dal()),
39 14308 : frontend(_frontend)
40 : {
41 1022 : }
42 :
43 : // setup this core backend
44 1022 : bool NavEKF2_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
45 : {
46 1022 : imu_index = _imu_index;
47 1022 : gyro_index_active = _imu_index;
48 1022 : accel_index_active = _imu_index;
49 1022 : core_index = _core_index;
50 :
51 : /*
52 : the imu_buffer_length needs to cope with a 260ms delay at a
53 : maximum fusion rate of 100Hz. Non-imu data coming in at faster
54 : than 100Hz is downsampled. For 50Hz main loop rate we need a
55 : shorter buffer.
56 : */
57 1022 : if (dal.ins().get_loop_rate_hz() < 100) {
58 968 : imu_buffer_length = 13;
59 : } else {
60 : // maximum 260 msec delay at 100 Hz fusion rate
61 54 : imu_buffer_length = 26;
62 : }
63 1022 : if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
64 : return false;
65 : }
66 1022 : if(!storedMag.init(OBS_BUFFER_LENGTH)) {
67 : return false;
68 : }
69 1022 : if(!storedBaro.init(OBS_BUFFER_LENGTH)) {
70 : return false;
71 : }
72 1022 : if(!storedTAS.init(OBS_BUFFER_LENGTH)) {
73 : return false;
74 : }
75 1022 : if(!storedOF.init(FLOW_BUFFER_LENGTH)) {
76 : return false;
77 : }
78 : // Note: the use of dual range finders potentially doubles the amount of to be stored
79 1022 : if(!storedRange.init(2*OBS_BUFFER_LENGTH)) {
80 : return false;
81 : }
82 : // Note: range beacon data is read one beacon at a time and can arrive at a high rate
83 1022 : if(!storedRangeBeacon.init(imu_buffer_length)) {
84 : return false;
85 : }
86 1022 : if(!storedExtNav.init(EXTNAV_BUFFER_LENGTH)) {
87 : return false;
88 : }
89 1022 : if(!storedIMU.init(imu_buffer_length)) {
90 : return false;
91 : }
92 1022 : if(!storedOutput.init(imu_buffer_length)) {
93 : return false;
94 : }
95 1022 : if(!storedExtNavVel.init(EXTNAV_BUFFER_LENGTH)) {
96 : return false;
97 : }
98 :
99 1022 : if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
100 : // check if there is enough memory to create the EKF-GSF object
101 1022 : if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
102 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%u GSF: not enough memory",(unsigned)imu_index);
103 0 : return false;
104 : }
105 :
106 : // try to instantiate
107 1022 : yawEstimator = NEW_NOTHROW EKFGSF_yaw();
108 1022 : if (yawEstimator == nullptr) {
109 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF2 IMU%uGSF: allocation failed",(unsigned)imu_index);
110 0 : return false;
111 : }
112 : }
113 :
114 : return true;
115 : }
116 :
117 :
118 : /********************************************************
119 : * INIT FUNCTIONS *
120 : ********************************************************/
121 :
122 : // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
123 558 : void NavEKF2_core::InitialiseVariables()
124 : {
125 : // calculate the nominal filter update rate
126 558 : const auto &ins = dal.ins();
127 558 : localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
128 558 : localFilterTimeStep_ms = MAX(localFilterTimeStep_ms,10);
129 :
130 : // initialise time stamps
131 558 : imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
132 558 : prevTasStep_ms = imuSampleTime_ms;
133 558 : prevBetaStep_ms = imuSampleTime_ms;
134 558 : lastBaroReceived_ms = imuSampleTime_ms;
135 558 : lastVelPassTime_ms = 0;
136 558 : lastPosPassTime_ms = 0;
137 558 : lastHgtPassTime_ms = 0;
138 558 : lastTasPassTime_ms = 0;
139 558 : lastYawTime_ms = imuSampleTime_ms;
140 558 : lastTimeGpsReceived_ms = 0;
141 558 : timeAtLastAuxEKF_ms = imuSampleTime_ms;
142 558 : flowValidMeaTime_ms = imuSampleTime_ms;
143 558 : rngValidMeaTime_ms = imuSampleTime_ms;
144 558 : flowMeaTime_ms = 0;
145 558 : prevFlowFuseTime_ms = 0;
146 558 : gndHgtValidTime_ms = 0;
147 558 : ekfStartTime_ms = imuSampleTime_ms;
148 558 : lastGpsVelFail_ms = 0;
149 558 : lastGpsVelPass_ms = 0;
150 558 : lastGpsAidBadTime_ms = 0;
151 558 : timeTasReceived_ms = 0;
152 558 : lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;
153 558 : posNEResetCount = 0;
154 558 : posDResetCount = 0;
155 558 : lastRngMeasTime_ms = 0;
156 :
157 : // initialise other variables
158 558 : gpsNoiseScaler = 1.0f;
159 558 : hgtTimeout = true;
160 558 : tasTimeout = true;
161 558 : badIMUdata = false;
162 558 : dtIMUavg = 0.0025f;
163 558 : dtEkfAvg = EKF_TARGET_DT;
164 558 : dt = 0;
165 558 : velDotNEDfilt.zero();
166 558 : lastKnownPositionNE.zero();
167 558 : prevTnb.zero();
168 558 : memset(&P[0][0], 0, sizeof(P));
169 558 : memset(&KH[0][0], 0, sizeof(KH));
170 558 : memset(&KHP[0][0], 0, sizeof(KHP));
171 558 : memset(&nextP[0][0], 0, sizeof(nextP));
172 558 : flowDataValid = false;
173 558 : rangeDataToFuse = false;
174 558 : Popt = 0.0f;
175 558 : terrainState = 0.0f;
176 558 : prevPosN = stateStruct.position.x;
177 558 : prevPosE = stateStruct.position.y;
178 558 : inhibitGndState = false;
179 558 : flowGyroBias.x = 0;
180 558 : flowGyroBias.y = 0;
181 558 : PV_AidingMode = AID_NONE;
182 558 : PV_AidingModePrev = AID_NONE;
183 558 : posTimeout = true;
184 558 : velTimeout = true;
185 558 : memset(&faultStatus, 0, sizeof(faultStatus));
186 558 : hgtRate = 0.0f;
187 558 : mag_state.q0 = 1;
188 558 : mag_state.DCM.identity();
189 558 : onGround = true;
190 558 : prevOnGround = true;
191 558 : inFlight = false;
192 558 : prevInFlight = false;
193 558 : manoeuvring = false;
194 558 : inhibitWindStates = true;
195 558 : gndOffsetValid = false;
196 558 : validOrigin = false;
197 558 : gpsSpdAccuracy = 0.0f;
198 558 : gpsPosAccuracy = 0.0f;
199 558 : gpsHgtAccuracy = 0.0f;
200 558 : baroHgtOffset = 0.0f;
201 558 : rngOnGnd = 0.05f;
202 558 : yawResetCount = 0;
203 558 : tiltErrFilt = 1.0f;
204 558 : tiltAlignComplete = false;
205 558 : stateIndexLim = 23;
206 558 : delAngCorrection.zero();
207 558 : velErrintegral.zero();
208 558 : posErrintegral.zero();
209 558 : gpsGoodToAlign = false;
210 558 : gpsNotAvailable = true;
211 558 : motorsArmed = false;
212 558 : prevMotorsArmed = false;
213 558 : memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
214 558 : gpsSpdAccPass = false;
215 558 : ekfInnovationsPass = false;
216 558 : sAccFilterState1 = 0.0f;
217 558 : sAccFilterState2 = 0.0f;
218 558 : lastGpsCheckTime_ms = 0;
219 558 : lastInnovPassTime_ms = 0;
220 558 : lastInnovFailTime_ms = 0;
221 558 : gpsAccuracyGood = false;
222 558 : gpsloc_prev = {};
223 558 : gpsDriftNE = 0.0f;
224 558 : gpsVertVelFilt = 0.0f;
225 558 : gpsHorizVelFilt = 0.0f;
226 558 : ZERO_FARRAY(statesArray);
227 558 : memset(&vertCompFiltState, 0, sizeof(vertCompFiltState));
228 558 : posVelFusionDelayed = false;
229 558 : optFlowFusionDelayed = false;
230 558 : airSpdFusionDelayed = false;
231 558 : sideSlipFusionDelayed = false;
232 558 : posResetNE.zero();
233 558 : posResetD = 0.0f;
234 558 : hgtInnovFiltState = 0.0f;
235 :
236 558 : imuDataDownSampledNew.delAng.zero();
237 558 : imuDataDownSampledNew.delVel.zero();
238 558 : imuDataDownSampledNew.delAngDT = 0.0f;
239 558 : imuDataDownSampledNew.delVelDT = 0.0f;
240 558 : runUpdates = false;
241 558 : framesSincePredict = 0;
242 558 : gpsYawResetRequest = false;
243 558 : stateStruct.quat.initialise();
244 558 : quatAtLastMagReset = stateStruct.quat;
245 558 : delAngBiasLearned = false;
246 558 : memset(&filterStatus, 0, sizeof(filterStatus));
247 558 : activeHgtSource = 0;
248 : #if AP_RANGEFINDER_ENABLED
249 558 : memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));
250 558 : memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms));
251 558 : memset(&storedRngMeas, 0, sizeof(storedRngMeas));
252 : #endif
253 558 : terrainHgtStable = true;
254 558 : ekfOriginHgtVar = 0.0f;
255 558 : ekfGpsRefHgt = 0.0;
256 558 : velOffsetNED.zero();
257 558 : posOffsetNED.zero();
258 558 : ZERO_FARRAY(velPosObs);
259 :
260 : // range beacon fusion variables
261 558 : memset((void *)&rngBcnDataNew, 0, sizeof(rngBcnDataNew));
262 558 : memset((void *)&rngBcnDataDelayed, 0, sizeof(rngBcnDataDelayed));
263 558 : lastRngBcnPassTime_ms = 0;
264 558 : rngBcnTestRatio = 0.0f;
265 558 : rngBcnHealth = false;
266 558 : rngBcnTimeout = true;
267 558 : varInnovRngBcn = 0.0f;
268 558 : innovRngBcn = 0.0f;
269 558 : memset(&lastTimeRngBcn_ms, 0, sizeof(lastTimeRngBcn_ms));
270 : #if AP_BEACON_ENABLED
271 558 : rngBcnDataToFuse = false;
272 : #endif
273 558 : beaconVehiclePosNED.zero();
274 558 : beaconVehiclePosErr = 1.0f;
275 558 : rngBcnLast3DmeasTime_ms = 0;
276 558 : rngBcnGoodToAlign = false;
277 558 : lastRngBcnChecked = 0;
278 558 : receiverPos.zero();
279 558 : memset(&receiverPosCov, 0, sizeof(receiverPosCov));
280 558 : rngBcnAlignmentStarted = false;
281 558 : rngBcnAlignmentCompleted = false;
282 558 : lastBeaconIndex = 0;
283 558 : rngBcnPosSum.zero();
284 558 : numBcnMeas = 0;
285 558 : rngSum = 0.0f;
286 558 : N_beacons = 0;
287 558 : maxBcnPosD = 0.0f;
288 558 : minBcnPosD = 0.0f;
289 558 : bcnPosOffset = 0.0f;
290 558 : bcnPosOffsetMax = 0.0f;
291 558 : bcnPosOffsetMaxVar = 0.0f;
292 558 : OffsetMaxInnovFilt = 0.0f;
293 558 : bcnPosOffsetMin = 0.0f;
294 558 : bcnPosOffsetMinVar = 0.0f;
295 558 : OffsetMinInnovFilt = 0.0f;
296 558 : rngBcnFuseDataReportIndex = 0;
297 558 : memset(&rngBcnFusionReport, 0, sizeof(rngBcnFusionReport));
298 558 : last_gps_idx = 0;
299 :
300 : // external nav data fusion
301 558 : memset((void *)&extNavDataNew, 0, sizeof(extNavDataNew));
302 558 : memset((void *)&extNavDataDelayed, 0, sizeof(extNavDataDelayed));
303 558 : extNavDataToFuse = false;
304 558 : extNavMeasTime_ms = 0;
305 558 : extNavLastPosResetTime_ms = 0;
306 558 : extNavUsedForYaw = false;
307 558 : extNavUsedForPos = false;
308 558 : extNavYawResetRequest = false;
309 :
310 558 : extNavVelNew = {};
311 558 : extNavVelDelayed = {};
312 558 : extNavVelToFuse = false;
313 558 : extNavVelMeasTime_ms = 0;
314 558 : useExtNavVel = false;
315 :
316 : // zero data buffers
317 558 : storedIMU.reset();
318 558 : storedGPS.reset();
319 558 : storedBaro.reset();
320 558 : storedTAS.reset();
321 558 : storedRange.reset();
322 558 : storedOutput.reset();
323 558 : storedRangeBeacon.reset();
324 558 : storedExtNav.reset();
325 558 : storedExtNavVel.reset();
326 :
327 : // now init mag variables
328 558 : yawAlignComplete = false;
329 558 : have_table_earth_field = false;
330 :
331 : // initialise pre-arm message
332 558 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF2 still initialising");
333 :
334 558 : InitialiseVariablesMag();
335 :
336 : // emergency reset of yaw to EKFGSF estimate
337 558 : EKFGSF_yaw_reset_ms = 0;
338 558 : EKFGSF_yaw_reset_request_ms = 0;
339 558 : EKFGSF_yaw_reset_count = 0;
340 558 : EKFGSF_run_filterbank = false;
341 558 : }
342 :
343 :
344 : /*
345 : separate out the mag reset so it can be used when compass learning completes
346 : */
347 558 : void NavEKF2_core::InitialiseVariablesMag()
348 : {
349 558 : lastHealthyMagTime_ms = imuSampleTime_ms;
350 558 : lastMagUpdate_us = 0;
351 558 : magYawResetTimer_ms = imuSampleTime_ms;
352 558 : magTimeout = false;
353 558 : allMagSensorsFailed = false;
354 558 : finalInflightYawInit = false;
355 558 : finalInflightMagInit = false;
356 :
357 558 : inhibitMagStates = true;
358 :
359 558 : magSelectIndex = dal.compass().get_first_usable();
360 558 : lastMagOffsetsValid = false;
361 558 : magStateResetRequest = false;
362 558 : magStateInitComplete = false;
363 558 : magYawResetRequest = false;
364 :
365 558 : posDownAtLastMagReset = stateStruct.position.z;
366 558 : yawInnovAtLastMagReset = 0.0f;
367 558 : magFieldLearned = false;
368 :
369 558 : storedMag.reset();
370 558 : }
371 :
372 : // Initialise the states from accelerometer and magnetometer data (if present)
373 : // This method can only be used when the vehicle is static
374 207598 : bool NavEKF2_core::InitialiseFilterBootstrap(void)
375 : {
376 : // If we are a plane and don't have GPS lock then don't initialise
377 207598 : if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_GPS_FixType::FIX_3D) {
378 190082 : dal.snprintf(prearm_fail_string,
379 : sizeof(prearm_fail_string),
380 : "EKF2 init failure: No GPS lock");
381 190082 : statesInitialised = false;
382 190082 : return false;
383 : }
384 :
385 17516 : if (statesInitialised) {
386 : // we are initialised, but we don't return true until the IMU
387 : // buffer has been filled. This prevents a timing
388 : // vulnerability with a pause in IMU data during filter startup
389 16958 : readIMUData();
390 16958 : readMagData();
391 16958 : readGpsData();
392 16958 : readBaroData();
393 16958 : return storedIMU.is_filled();
394 : }
395 :
396 : // set re-used variables to zero
397 558 : InitialiseVariables();
398 :
399 558 : const auto &ins = dal.ins();
400 :
401 : // Initialise IMU data
402 558 : dtIMUavg = ins.get_loop_delta_t();
403 558 : readIMUData();
404 558 : storedIMU.reset_history(imuDataNew);
405 558 : imuDataDelayed = imuDataNew;
406 :
407 : // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
408 558 : Vector3F initAccVec;
409 :
410 : // TODO we should average accel readings over several cycles
411 558 : initAccVec = ins.get_accel(accel_index_active).toftype();
412 :
413 : // read the magnetometer data
414 558 : readMagData();
415 :
416 : // normalise the acceleration vector
417 : ftype pitch=0, roll=0;
418 558 : if (initAccVec.length() > 0.001f) {
419 558 : initAccVec.normalize();
420 :
421 : // calculate initial pitch angle
422 558 : pitch = asinF(initAccVec.x);
423 :
424 : // calculate initial roll angle
425 558 : roll = atan2F(-initAccVec.y , -initAccVec.z);
426 : }
427 :
428 : // calculate initial roll and pitch orientation
429 558 : stateStruct.quat.from_euler(roll, pitch, 0.0f);
430 :
431 : // initialise dynamic states
432 558 : stateStruct.velocity.zero();
433 558 : stateStruct.position.zero();
434 558 : stateStruct.angErr.zero();
435 :
436 : // initialise static process model states
437 558 : stateStruct.gyro_bias.zero();
438 558 : stateStruct.gyro_scale.x = 1.0f;
439 558 : stateStruct.gyro_scale.y = 1.0f;
440 558 : stateStruct.gyro_scale.z = 1.0f;
441 558 : stateStruct.accel_zbias = 0.0f;
442 558 : stateStruct.wind_vel.zero();
443 558 : stateStruct.earth_magfield.zero();
444 558 : stateStruct.body_magfield.zero();
445 :
446 : // read the GPS and set the position and velocity states
447 558 : readGpsData();
448 558 : ResetVelocity();
449 558 : ResetPosition();
450 :
451 : // read the barometer and set the height state
452 558 : readBaroData();
453 558 : ResetHeight();
454 :
455 : // define Earth rotation vector in the NED navigation frame
456 558 : calcEarthRateNED(earthRateNED, dal.get_home().lat);
457 :
458 : // initialise the covariance matrix
459 558 : CovarianceInit();
460 :
461 : // reset output states
462 558 : StoreOutputReset();
463 :
464 : // set to true now that states have be initialised
465 558 : statesInitialised = true;
466 :
467 : // reset inactive biases
468 2232 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
469 1674 : inactiveBias[i].gyro_bias.zero();
470 1674 : inactiveBias[i].accel_zbias = 0;
471 1674 : inactiveBias[i].gyro_scale.x = 1;
472 1674 : inactiveBias[i].gyro_scale.y = 1;
473 1674 : inactiveBias[i].gyro_scale.z = 1;
474 : }
475 :
476 : // we initially return false to wait for the IMU buffer to fill
477 : return false;
478 : }
479 :
480 : // initialise the covariance matrix
481 558 : void NavEKF2_core::CovarianceInit()
482 : {
483 : // zero the matrix
484 558 : memset(&P[0][0], 0, sizeof(P));
485 :
486 : // attitude error
487 558 : P[0][0] = 0.1f;
488 558 : P[1][1] = 0.1f;
489 558 : P[2][2] = 0.1f;
490 : // velocities
491 558 : P[3][3] = sq(frontend->_gpsHorizVelNoise);
492 558 : P[4][4] = P[3][3];
493 558 : P[5][5] = sq(frontend->_gpsVertVelNoise);
494 : // positions
495 558 : P[6][6] = sq(frontend->_gpsHorizPosNoise);
496 558 : P[7][7] = P[6][6];
497 558 : P[8][8] = sq(frontend->_baroAltNoise);
498 : // gyro delta angle biases
499 558 : P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
500 558 : P[10][10] = P[9][9];
501 558 : P[11][11] = P[9][9];
502 : // gyro scale factor biases
503 558 : P[12][12] = sq(1e-3);
504 558 : P[13][13] = P[12][12];
505 558 : P[14][14] = P[12][12];
506 : // Z delta velocity bias
507 558 : P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
508 : // earth magnetic field
509 558 : P[16][16] = 0.0f;
510 558 : P[17][17] = P[16][16];
511 558 : P[18][18] = P[16][16];
512 : // body magnetic field
513 558 : P[19][19] = 0.0f;
514 558 : P[20][20] = P[19][19];
515 558 : P[21][21] = P[19][19];
516 : // wind velocities
517 558 : P[22][22] = 0.0f;
518 558 : P[23][23] = P[22][22];
519 :
520 : // optical flow ground height covariance
521 558 : Popt = 0.25f;
522 558 : }
523 :
524 : /********************************************************
525 : * UPDATE FUNCTIONS *
526 : ********************************************************/
527 : // Update Filter States - this should be called whenever new IMU data is available
528 4213784 : void NavEKF2_core::UpdateFilter(bool predict)
529 : {
530 : // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started
531 4213784 : startPredictEnabled = predict;
532 :
533 : // don't run filter updates if states have not been initialised
534 4213784 : if (!statesInitialised) {
535 : return;
536 : }
537 :
538 : // start the timer used for load measurement
539 : #if ENABLE_EKF_TIMING
540 : void *istate = hal.scheduler->disable_interrupts_save();
541 : static uint32_t timing_start_us;
542 : timing_start_us = dal.micros();
543 : #endif
544 :
545 4213784 : fill_scratch_variables();
546 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
547 4213784 : fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype)); // see fill_scratch_variables()
548 4213784 : fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype));
549 : #endif
550 :
551 : // TODO - in-flight restart method
552 :
553 : //get starting time for update step
554 4213784 : imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
555 :
556 : // Check arm status and perform required checks and mode changes
557 4213784 : controlFilterModes();
558 :
559 : // read IMU data as delta angles and velocities
560 4213784 : readIMUData();
561 :
562 : // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
563 4213784 : if (runUpdates) {
564 : // Predict states using IMU data from the delayed time horizon
565 3248540 : UpdateStrapdownEquationsNED();
566 :
567 : // Predict the covariance growth
568 3248540 : CovariancePrediction();
569 :
570 : // Run the IMU prediction step for the GSF yaw estimator algorithm
571 : // using IMU and optionally true airspeed data.
572 : // Must be run before SelectMagFusion() to provide an up to date yaw estimate
573 3248540 : runYawEstimatorPrediction();
574 :
575 : // Update states using magnetometer data
576 3248540 : SelectMagFusion();
577 :
578 : // Update states using GPS and altimeter data
579 3248540 : SelectVelPosFusion();
580 :
581 : // Run the GPS velocity correction step for the GSF yaw estimator algorithm
582 : // and use the yaw estimate to reset the main EKF yaw if requested
583 : // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
584 3248540 : runYawEstimatorCorrection();
585 :
586 : #if AP_BEACON_ENABLED
587 : // Update states using range beacon data
588 3248540 : SelectRngBcnFusion();
589 : #endif
590 :
591 : // Update states using optical flow data
592 3248540 : SelectFlowFusion();
593 :
594 : // Update states using airspeed data
595 3248540 : SelectTasFusion();
596 :
597 : // Update states using sideslip constraint assumption for fly-forward vehicles
598 3248540 : SelectBetaFusion();
599 :
600 : // Update the filter status
601 3248540 : updateFilterStatus();
602 : }
603 :
604 : // Wind output forward from the fusion to output time horizon
605 4213784 : calcOutputStates();
606 :
607 : // stop the timer used for load measurement
608 : #if ENABLE_EKF_TIMING
609 : static uint32_t total_us;
610 : static uint32_t timing_counter;
611 : total_us += dal.micros() - timing_start_us;
612 : if (timing_counter++ == 4000) {
613 : DEV_PRINTF("ekf2 avg %.2f us\n", total_us / float(timing_counter));
614 : total_us = 0;
615 : timing_counter = 0;
616 : }
617 : hal.scheduler->restore_interrupts(istate);
618 : #endif
619 :
620 : /*
621 : this is a check to cope with a vehicle sitting idle on the
622 : ground and getting over-confident of the state. The symptoms
623 : would be "gyros still settling" when the user tries to arm. In
624 : that state the EKF can't recover, so we do a hard reset and let
625 : it try again.
626 : */
627 4213784 : if (filterStatus.value != 0) {
628 4184630 : last_filter_ok_ms = dal.millis();
629 : }
630 29154 : if (filterStatus.value == 0 &&
631 29154 : last_filter_ok_ms != 0 &&
632 4213784 : dal.millis() - last_filter_ok_ms > 5000 &&
633 0 : !dal.get_armed()) {
634 : // we've been unhealthy for 5 seconds after being healthy, reset the filter
635 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
636 0 : last_filter_ok_ms = 0;
637 0 : statesInitialised = false;
638 0 : InitialiseFilterBootstrap();
639 : }
640 :
641 : }
642 :
643 7470324 : void NavEKF2_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index)
644 : {
645 7470324 : delAng.x = delAng.x * stateStruct.gyro_scale.x;
646 7470324 : delAng.y = delAng.y * stateStruct.gyro_scale.y;
647 7470324 : delAng.z = delAng.z * stateStruct.gyro_scale.z;
648 7470324 : delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg);
649 7470324 : }
650 :
651 7470324 : void NavEKF2_core::correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index)
652 : {
653 7470324 : delVel.z -= inactiveBias[accel_index].accel_zbias * (delVelDT / dtEkfAvg);
654 7470324 : }
655 :
656 : /*
657 : * Update the quaternion, velocity and position states using delayed IMU measurements
658 : * because the EKF is running on a delayed time horizon. Note that the quaternion is
659 : * not used by the EKF equations, which instead estimate the error in the attitude of
660 : * the vehicle when each observation is fused. This attitude error is then used to correct
661 : * the quaternion.
662 : */
663 3248540 : void NavEKF2_core::UpdateStrapdownEquationsNED()
664 : {
665 : // update the quaternion states by rotating from the previous attitude through
666 : // the delta angle rotation quaternion and normalise
667 : // apply correction for earth's rotation rate
668 : // % * - and + operators have been overloaded
669 3248540 : stateStruct.quat.rotate(delAngCorrected - prevTnb * earthRateNED*imuDataDelayed.delAngDT);
670 3248540 : stateStruct.quat.normalize();
671 :
672 : // transform body delta velocities to delta velocities in the nav frame
673 : // use the nav frame from previous time step as the delta velocities
674 : // have been rotated into that frame
675 : // * and + operators have been overloaded
676 3248540 : Vector3F delVelNav; // delta velocity vector in earth axes
677 3248540 : delVelNav = prevTnb.mul_transpose(delVelCorrected);
678 3248540 : delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
679 :
680 : // calculate the nav to body cosine matrix
681 3248540 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
682 :
683 : // calculate the rate of change of velocity (used for launch detect and other functions)
684 3248540 : velDotNED = delVelNav / imuDataDelayed.delVelDT;
685 :
686 : // apply a first order lowpass filter
687 3248540 : velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
688 :
689 : // calculate a magnitude of the filtered nav acceleration (required for GPS
690 : // variance estimation)
691 3248540 : accNavMag = velDotNEDfilt.length();
692 3248540 : accNavMagHoriz = velDotNEDfilt.xy().length();
693 :
694 : // if we are not aiding, then limit the horizontal magnitude of acceleration
695 : // to prevent large manoeuvre transients disturbing the attitude
696 3248540 : if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {
697 62662 : ftype gain = 5.0f/accNavMagHoriz;
698 62662 : delVelNav.x *= gain;
699 62662 : delVelNav.y *= gain;
700 : }
701 :
702 : // save velocity for use in trapezoidal integration for position calcuation
703 3248540 : Vector3F lastVelocity = stateStruct.velocity;
704 :
705 : // sum delta velocities to get velocity
706 3248540 : stateStruct.velocity += delVelNav;
707 :
708 : // apply a trapezoidal integration to velocities to calculate position
709 3248540 : stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
710 :
711 : // accumulate the bias delta angle and time since last reset by an OF measurement arrival
712 3248540 : delAngBodyOF += delAngCorrected;
713 3248540 : delTimeOF += imuDataDelayed.delAngDT;
714 :
715 : // limit states to protect against divergence
716 3248540 : ConstrainStates();
717 3248540 : }
718 :
719 : /*
720 : * Propagate PVA solution forward from the fusion time horizon to the current time horizon
721 : * using simple observer which performs two functions:
722 : * 1) Corrects for the delayed time horizon used by the EKF.
723 : * 2) Applies a LPF to state corrections to prevent 'stepping' in states due to measurement
724 : * fusion introducing unwanted noise into the control loops.
725 : * The inspiration for using a complementary filter to correct for time delays in the EKF
726 : * is based on the work by A Khosravian.
727 : *
728 : * "Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements"
729 : * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
730 : */
731 4213784 : void NavEKF2_core::calcOutputStates()
732 : {
733 : // apply corrections to the IMU data
734 4213784 : Vector3F delAngNewCorrected = imuDataNew.delAng;
735 4213784 : Vector3F delVelNewCorrected = imuDataNew.delVel;
736 4213784 : correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index);
737 4213784 : correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index);
738 :
739 : // apply corections to track EKF solution
740 4213784 : Vector3F delAng = delAngNewCorrected + delAngCorrection;
741 :
742 : // convert the rotation vector to its equivalent quaternion
743 4213784 : QuaternionF deltaQuat;
744 4213784 : deltaQuat.from_axis_angle(delAng);
745 :
746 : // update the quaternion states by rotating from the previous attitude through
747 : // the delta angle rotation quaternion and normalise
748 4213784 : outputDataNew.quat *= deltaQuat;
749 4213784 : outputDataNew.quat.normalize();
750 :
751 : // calculate the body to nav cosine matrix
752 4213784 : Matrix3F Tbn_temp;
753 4213784 : outputDataNew.quat.rotation_matrix(Tbn_temp);
754 :
755 : // transform body delta velocities to delta velocities in the nav frame
756 4213784 : Vector3F delVelNav = Tbn_temp*delVelNewCorrected;
757 4213784 : delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
758 :
759 : // save velocity for use in trapezoidal integration for position calcuation
760 4213784 : Vector3F lastVelocity = outputDataNew.velocity;
761 :
762 : // sum delta velocities to get velocity
763 4213784 : outputDataNew.velocity += delVelNav;
764 :
765 : // Implement third order complementary filter for height and height rate
766 : // Reference Paper :
767 : // Optimizing the Gains of the Baro-Inertial Vertical Channel
768 : // Widnall W.S, Sinha P.K,
769 : // AIAA Journal of Guidance and Control, 78-1307R
770 :
771 : // Perform filter calculation using backwards Euler integration
772 : // Coefficients selected to place all three filter poles at omega
773 4213784 : const ftype CompFiltOmega = M_2PI * constrain_ftype(frontend->_hrt_filt_freq, 0.1f, 30.0f);
774 4213784 : ftype omega2 = CompFiltOmega * CompFiltOmega;
775 4213784 : ftype pos_err = constrain_ftype(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5);
776 4213784 : ftype integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;
777 4213784 : vertCompFiltState.acc += integ1_input;
778 4213784 : ftype integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT;
779 4213784 : vertCompFiltState.vel += integ2_input;
780 4213784 : ftype integ3_input = (vertCompFiltState.vel + pos_err * CompFiltOmega * 3.0f) * imuDataNew.delVelDT;
781 4213784 : vertCompFiltState.pos += integ3_input;
782 :
783 : // apply a trapezoidal integration to velocities to calculate position
784 4213784 : outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
785 :
786 : // If the IMU accelerometer is offset from the body frame origin, then calculate corrections
787 : // that can be added to the EKF velocity and position outputs so that they represent the velocity
788 : // and position of the body frame origin.
789 : // Note the * operator has been overloaded to operate as a dot product
790 4213784 : if (!accelPosOffset.is_zero()) {
791 : // calculate the average angular rate across the last IMU update
792 : // note delAngDT is prevented from being zero in readIMUData()
793 128217 : Vector3F angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
794 :
795 : // Calculate the velocity of the body frame origin relative to the IMU in body frame
796 : // and rotate into earth frame. Note % operator has been overloaded to perform a cross product
797 128217 : Vector3F velBodyRelIMU = angRate % (- accelPosOffset);
798 128217 : velOffsetNED = Tbn_temp * velBodyRelIMU;
799 :
800 : // calculate the earth frame position of the body frame origin relative to the IMU
801 128217 : posOffsetNED = Tbn_temp * (- accelPosOffset);
802 : } else {
803 4085567 : velOffsetNED.zero();
804 4085567 : posOffsetNED.zero();
805 : }
806 :
807 : // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
808 4213784 : if (runUpdates) {
809 : // store the states at the output time horizon
810 3248540 : storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
811 :
812 : // recall the states from the fusion time horizon
813 3248540 : outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
814 :
815 : // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
816 :
817 : // divide the demanded quaternion by the estimated to get the error
818 3248540 : QuaternionF quatErr = stateStruct.quat / outputDataDelayed.quat;
819 :
820 : // Convert to a delta rotation using a small angle approximation
821 3248540 : quatErr.normalize();
822 3248540 : Vector3F deltaAngErr;
823 : ftype scaler;
824 3248540 : if (quatErr[0] >= 0.0f) {
825 : scaler = 2.0f;
826 : } else {
827 : scaler = -2.0f;
828 : }
829 3248540 : deltaAngErr.x = scaler * quatErr[1];
830 3248540 : deltaAngErr.y = scaler * quatErr[2];
831 3248540 : deltaAngErr.z = scaler * quatErr[3];
832 :
833 : // calculate a gain that provides tight tracking of the estimator states and
834 : // adjust for changes in time delay to maintain consistent damping ratio of ~0.7
835 3248540 : ftype timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
836 3248540 : timeDelay = fmaxF(timeDelay, dtIMUavg);
837 3248540 : ftype errorGain = 0.5f / timeDelay;
838 :
839 : // calculate a corrrection to the delta angle
840 : // that will cause the INS to track the EKF quaternions
841 3248540 : delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
842 :
843 : // calculate velocity and position tracking errors
844 3248540 : Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity);
845 3248540 : Vector3F posErr = (stateStruct.position - outputDataDelayed.position);
846 :
847 : // collect magnitude tracking error for diagnostics
848 3248540 : outputTrackError.x = deltaAngErr.length();
849 3248540 : outputTrackError.y = velErr.length();
850 3248540 : outputTrackError.z = posErr.length();
851 :
852 : // convert user specified time constant from centi-seconds to seconds
853 3248540 : ftype tauPosVel = constrain_ftype(0.01f*(ftype)frontend->_tauVelPosOutput, 0.1f, 0.5f);
854 :
855 : // calculate a gain to track the EKF position states with the specified time constant
856 3248540 : ftype velPosGain = dtEkfAvg / constrain_ftype(tauPosVel, dtEkfAvg, 10.0f);
857 :
858 : // use a PI feedback to calculate a correction that will be applied to the output state history
859 3248540 : posErrintegral += posErr;
860 3248540 : velErrintegral += velErr;
861 3248540 : Vector3F velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f;
862 3248540 : Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f;
863 :
864 : // loop through the output filter state history and apply the corrections to the velocity and position states
865 : // this method is too expensive to use for the attitude states due to the quaternion operations required
866 : // but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
867 : // to be used
868 3248540 : output_elements outputStates;
869 50038452 : for (unsigned index=0; index < imu_buffer_length; index++) {
870 46789912 : outputStates = storedOutput[index];
871 :
872 : // a constant velocity correction is applied
873 46789912 : outputStates.velocity += velCorrection;
874 :
875 : // a constant position correction is applied
876 46789912 : outputStates.position += posCorrection;
877 :
878 : // push the updated data to the buffer
879 46789912 : storedOutput[index] = outputStates;
880 : }
881 :
882 : // update output state to corrected values
883 3248540 : outputDataNew = storedOutput[storedIMU.get_youngest_index()];
884 :
885 : }
886 4213784 : }
887 :
888 : /*
889 : * Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox.
890 : * The script file used to generate these and other equations in this filter can be found here:
891 : * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
892 : */
893 3248540 : void NavEKF2_core::CovariancePrediction()
894 : {
895 : ftype windVelSigma; // wind velocity 1-sigma process noise - m/s
896 : ftype dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/s
897 : ftype dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/s
898 : ftype dAngScaleSigma;// delta angle scale factor 1-Sigma process noise
899 : ftype magEarthSigma;// earth magnetic field 1-sigma process noise
900 : ftype magBodySigma; // body magnetic field 1-sigma process noise
901 : ftype daxNoise; // X axis delta angle noise variance rad^2
902 : ftype dayNoise; // Y axis delta angle noise variance rad^2
903 : ftype dazNoise; // Z axis delta angle noise variance rad^2
904 : ftype dvxNoise; // X axis delta velocity variance noise (m/s)^2
905 : ftype dvyNoise; // Y axis delta velocity variance noise (m/s)^2
906 : ftype dvzNoise; // Z axis delta velocity variance noise (m/s)^2
907 : ftype dvx; // X axis delta velocity (m/s)
908 : ftype dvy; // Y axis delta velocity (m/s)
909 : ftype dvz; // Z axis delta velocity (m/s)
910 : ftype dax; // X axis delta angle (rad)
911 : ftype day; // Y axis delta angle (rad)
912 : ftype daz; // Z axis delta angle (rad)
913 : ftype q0; // attitude quaternion
914 : ftype q1; // attitude quaternion
915 : ftype q2; // attitude quaternion
916 : ftype q3; // attitude quaternion
917 : ftype dax_b; // X axis delta angle measurement bias (rad)
918 : ftype day_b; // Y axis delta angle measurement bias (rad)
919 : ftype daz_b; // Z axis delta angle measurement bias (rad)
920 : ftype dax_s; // X axis delta angle measurement scale factor
921 : ftype day_s; // Y axis delta angle measurement scale factor
922 : ftype daz_s; // Z axis delta angle measurement scale factor
923 : ftype dvz_b; // Z axis delta velocity measurement bias (rad)
924 : Vector25 SF;
925 : Vector5 SG;
926 : Vector8 SQ;
927 : Vector24 processNoise;
928 :
929 : // calculate covariance prediction process noise
930 : // use filtered height rate to increase wind process noise when climbing or descending
931 : // this allows for wind gradient effects.
932 : // filter height rate using a 10 second time constant filter
933 3248540 : dt = imuDataDelayed.delAngDT;
934 3248540 : ftype alpha = 0.1f * dt;
935 3248540 : hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
936 :
937 : // use filtered height rate to increase wind process noise when climbing or descending
938 : // this allows for wind gradient effects.
939 3248540 : windVelSigma = dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate));
940 3248540 : dAngBiasSigma = sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
941 3248540 : dVelBiasSigma = sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);
942 3248540 : dAngScaleSigma = dt * constrain_ftype(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f);
943 3248540 : magEarthSigma = dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f);
944 3248540 : magBodySigma = dt * constrain_ftype(frontend->_magBodyProcessNoise, 0.0f, 1.0f);
945 32485400 : for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
946 12994160 : for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
947 12994160 : for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;
948 3248540 : if (dal.get_takeoff_expected()) {
949 29980 : processNoise[15] = 0.0f;
950 : } else {
951 3218560 : processNoise[15] = dVelBiasSigma;
952 : }
953 12994160 : for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;
954 12994160 : for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma;
955 9745620 : for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma;
956 :
957 73250190 : for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);
958 :
959 : // set variables used to calculate covariance growth
960 3248540 : dvx = imuDataDelayed.delVel.x;
961 3248540 : dvy = imuDataDelayed.delVel.y;
962 3248540 : dvz = imuDataDelayed.delVel.z;
963 3248540 : dax = imuDataDelayed.delAng.x;
964 3248540 : day = imuDataDelayed.delAng.y;
965 3248540 : daz = imuDataDelayed.delAng.z;
966 3248540 : q0 = stateStruct.quat[0];
967 3248540 : q1 = stateStruct.quat[1];
968 3248540 : q2 = stateStruct.quat[2];
969 3248540 : q3 = stateStruct.quat[3];
970 3248540 : dax_b = stateStruct.gyro_bias.x;
971 3248540 : day_b = stateStruct.gyro_bias.y;
972 3248540 : daz_b = stateStruct.gyro_bias.z;
973 3248540 : dax_s = stateStruct.gyro_scale.x;
974 3248540 : day_s = stateStruct.gyro_scale.y;
975 3248540 : daz_s = stateStruct.gyro_scale.z;
976 3248540 : dvz_b = stateStruct.accel_zbias;
977 3248540 : ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
978 3248540 : daxNoise = dayNoise = dazNoise = sq(dt*_gyrNoise);
979 3248540 : ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);
980 3248540 : dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise);
981 :
982 : // calculate the predicted covariance due to inertial sensor error propagation
983 : // we calculate the upper diagonal and copy to take advantage of symmetry
984 3248540 : SF[0] = daz_b/2 - (daz*daz_s)/2;
985 3248540 : SF[1] = day_b/2 - (day*day_s)/2;
986 3248540 : SF[2] = dax_b/2 - (dax*dax_s)/2;
987 3248540 : SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2;
988 3248540 : SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2;
989 3248540 : SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2;
990 3248540 : SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2;
991 3248540 : SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2;
992 3248540 : SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2;
993 3248540 : SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2;
994 3248540 : SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2;
995 3248540 : SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2;
996 3248540 : SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2;
997 3248540 : SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2;
998 3248540 : SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2;
999 3248540 : SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
1000 3248540 : SF[16] = dvz_b - dvz;
1001 : SF[17] = dvx;
1002 : SF[18] = dvy;
1003 3248540 : SF[19] = sq(q2);
1004 3248540 : SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3);
1005 3248540 : SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3);
1006 3248540 : SF[22] = 2*q0*q1 - 2*q2*q3;
1007 3248540 : SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3);
1008 3248540 : SF[24] = 2*q1*q2;
1009 :
1010 3248540 : SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);
1011 3248540 : SG[1] = sq(q3);
1012 3248540 : SG[2] = sq(q2);
1013 3248540 : SG[3] = sq(q1);
1014 3248540 : SG[4] = sq(q0);
1015 :
1016 3248540 : SQ[0] = - dvyNoise*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
1017 3248540 : SQ[1] = dvxNoise*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
1018 3248540 : SQ[2] = dvyNoise*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
1019 3248540 : SQ[3] = sq(SG[0]);
1020 : SQ[4] = 2*q2*q3;
1021 : SQ[5] = 2*q1*q3;
1022 : SQ[6] = 2*q1*q2;
1023 : SQ[7] = SG[4];
1024 :
1025 : Vector23 SPP;
1026 3248540 : SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);
1027 3248540 : SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);
1028 3248540 : SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];
1029 3248540 : SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10];
1030 3248540 : SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12];
1031 3248540 : SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14];
1032 3248540 : SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12];
1033 3248540 : SPP[7] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3);
1034 3248540 : SPP[8] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9];
1035 3248540 : SPP[9] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9];
1036 3248540 : SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3);
1037 3248540 : SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3);
1038 3248540 : SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3);
1039 3248540 : SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9];
1040 3248540 : SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13];
1041 3248540 : SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3);
1042 3248540 : SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3);
1043 3248540 : SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3);
1044 3248540 : SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3);
1045 3248540 : SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3);
1046 3248540 : SPP[20] = SF[16]*SF[21] - SF[18]*SF[22];
1047 : SPP[21] = 2*q0*q2 + 2*q1*q3;
1048 : SPP[22] = SF[15];
1049 :
1050 3248540 : if (inhibitMagStates) {
1051 952942 : zeroRows(P,16,21);
1052 952942 : zeroCols(P,16,21);
1053 2295598 : } else if (inhibitWindStates) {
1054 261255 : zeroRows(P,22,23);
1055 261255 : zeroCols(P,22,23);
1056 : }
1057 :
1058 3248540 : if (!inhibitMagStates && lastInhibitMagStates) {
1059 : // when starting 3D fusion we want to reset body mag variances
1060 308 : needMagBodyVarReset = true;
1061 : }
1062 :
1063 3248540 : if (needMagBodyVarReset) {
1064 : // reset body mag variances
1065 326 : needMagBodyVarReset = false;
1066 326 : zeroCols(P,19,21);
1067 326 : zeroRows(P,19,21);
1068 326 : P[19][19] = sq(frontend->_magNoise);
1069 326 : P[20][20] = P[19][19];
1070 326 : P[21][21] = P[19][19];
1071 : }
1072 :
1073 3248540 : lastInhibitMagStates = inhibitMagStates;
1074 :
1075 3248540 : nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[9][0]*SPP[22] + P[12][0]*SPP[18] + P[2][0]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[9][1]*SPP[22] + P[12][1]*SPP[18] + P[2][1]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[8]*(P[0][2]*SPP[5] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18] - P[1][2]*(2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12])) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[9][9]*SPP[22] + P[12][9]*SPP[18] + P[2][9]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[9][12]*SPP[22] + P[12][12]*SPP[18] + P[2][12]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]));
1076 3248540 : nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18]) - (2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9])*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
1077 3248540 : nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[9]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);
1078 3248540 : nextP[0][2] = SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]);
1079 3248540 : nextP[1][2] = SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]);
1080 3248540 : nextP[2][2] = dazNoise*SQ[3] - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]);
1081 3248540 : nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
1082 3248540 : nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
1083 3248540 : nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
1084 3248540 : nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + dvyNoise*sq(SQ[6] - 2*q0*q3) + dvzNoise*sq(SQ[5] + 2*q0*q2) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[2][15]*SPP[15] - P[15][15]*SPP[21] + P[1][15]*(SF[16]*SF[23] - SF[17]*SPP[21])) + dvxNoise*sq(SG[1] + SG[2] - SG[3] - SQ[7]);
1085 3248540 : nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);
1086 3248540 : nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]);
1087 3248540 : nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]);
1088 3248540 : nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]);
1089 3248540 : nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + dvxNoise*sq(SQ[6] + 2*q0*q3) + dvzNoise*sq(SQ[4] - 2*q0*q1) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + dvyNoise*sq(SG[1] - SG[2] + SG[3] - SQ[7]);
1090 3248540 : nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[7]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);
1091 3248540 : nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[7]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);
1092 3248540 : nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[7]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);
1093 3248540 : nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[7]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]);
1094 3248540 : nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[7]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]);
1095 3248540 : nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + dvxNoise*sq(SQ[5] - 2*q0*q2) + dvyNoise*sq(SQ[4] + 2*q0*q1) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[7]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[7] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[7] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[7] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + dvzNoise*sq(SG[1] - SG[2] - SG[3] + SQ[7]);
1096 3248540 : nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[8] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18]);
1097 3248540 : nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[9] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17]);
1098 3248540 : nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]);
1099 3248540 : nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]);
1100 3248540 : nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]);
1101 3248540 : nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[7] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[7] + P[1][3]*SPP[10] + P[2][3]*SPP[0]);
1102 3248540 : nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt);
1103 3248540 : nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[8] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18]);
1104 3248540 : nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[9] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17]);
1105 3248540 : nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]);
1106 3248540 : nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]);
1107 3248540 : nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]);
1108 3248540 : nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[7] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[7] + P[1][4]*SPP[10] + P[2][4]*SPP[0]);
1109 3248540 : nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt);
1110 3248540 : nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
1111 3248540 : nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[8] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18]);
1112 3248540 : nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[9] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17]);
1113 3248540 : nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]);
1114 3248540 : nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]);
1115 3248540 : nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]);
1116 3248540 : nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[7] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0]);
1117 3248540 : nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt);
1118 3248540 : nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
1119 3248540 : nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
1120 3248540 : nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[8] + P[9][9]*SPP[22] + P[12][9]*SPP[18];
1121 3248540 : nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[9] + P[10][9]*SPP[22] + P[13][9]*SPP[17];
1122 3248540 : nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16];
1123 3248540 : nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21];
1124 3248540 : nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11];
1125 3248540 : nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[7] + P[1][9]*SPP[10] + P[2][9]*SPP[0];
1126 3248540 : nextP[6][9] = P[6][9] + P[3][9]*dt;
1127 3248540 : nextP[7][9] = P[7][9] + P[4][9]*dt;
1128 3248540 : nextP[8][9] = P[8][9] + P[5][9]*dt;
1129 3248540 : nextP[9][9] = P[9][9];
1130 3248540 : nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18];
1131 3248540 : nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17];
1132 3248540 : nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16];
1133 3248540 : nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21];
1134 3248540 : nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11];
1135 3248540 : nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[7] + P[1][10]*SPP[10] + P[2][10]*SPP[0];
1136 3248540 : nextP[6][10] = P[6][10] + P[3][10]*dt;
1137 3248540 : nextP[7][10] = P[7][10] + P[4][10]*dt;
1138 3248540 : nextP[8][10] = P[8][10] + P[5][10]*dt;
1139 3248540 : nextP[9][10] = P[9][10];
1140 3248540 : nextP[10][10] = P[10][10];
1141 3248540 : nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18];
1142 3248540 : nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17];
1143 3248540 : nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16];
1144 3248540 : nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21];
1145 3248540 : nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11];
1146 3248540 : nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[7] + P[1][11]*SPP[10] + P[2][11]*SPP[0];
1147 3248540 : nextP[6][11] = P[6][11] + P[3][11]*dt;
1148 3248540 : nextP[7][11] = P[7][11] + P[4][11]*dt;
1149 3248540 : nextP[8][11] = P[8][11] + P[5][11]*dt;
1150 3248540 : nextP[9][11] = P[9][11];
1151 3248540 : nextP[10][11] = P[10][11];
1152 3248540 : nextP[11][11] = P[11][11];
1153 3248540 : nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[8] + P[9][12]*SPP[22] + P[12][12]*SPP[18];
1154 3248540 : nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[9] + P[10][12]*SPP[22] + P[13][12]*SPP[17];
1155 3248540 : nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16];
1156 3248540 : nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21];
1157 3248540 : nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11];
1158 3248540 : nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[7] + P[1][12]*SPP[10] + P[2][12]*SPP[0];
1159 3248540 : nextP[6][12] = P[6][12] + P[3][12]*dt;
1160 3248540 : nextP[7][12] = P[7][12] + P[4][12]*dt;
1161 3248540 : nextP[8][12] = P[8][12] + P[5][12]*dt;
1162 3248540 : nextP[9][12] = P[9][12];
1163 3248540 : nextP[10][12] = P[10][12];
1164 3248540 : nextP[11][12] = P[11][12];
1165 3248540 : nextP[12][12] = P[12][12];
1166 3248540 : nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18];
1167 3248540 : nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17];
1168 3248540 : nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16];
1169 3248540 : nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21];
1170 3248540 : nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11];
1171 3248540 : nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[7] + P[1][13]*SPP[10] + P[2][13]*SPP[0];
1172 3248540 : nextP[6][13] = P[6][13] + P[3][13]*dt;
1173 3248540 : nextP[7][13] = P[7][13] + P[4][13]*dt;
1174 3248540 : nextP[8][13] = P[8][13] + P[5][13]*dt;
1175 3248540 : nextP[9][13] = P[9][13];
1176 3248540 : nextP[10][13] = P[10][13];
1177 3248540 : nextP[11][13] = P[11][13];
1178 3248540 : nextP[12][13] = P[12][13];
1179 3248540 : nextP[13][13] = P[13][13];
1180 3248540 : nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18];
1181 3248540 : nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17];
1182 3248540 : nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16];
1183 3248540 : nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21];
1184 3248540 : nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11];
1185 3248540 : nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[7] + P[1][14]*SPP[10] + P[2][14]*SPP[0];
1186 3248540 : nextP[6][14] = P[6][14] + P[3][14]*dt;
1187 3248540 : nextP[7][14] = P[7][14] + P[4][14]*dt;
1188 3248540 : nextP[8][14] = P[8][14] + P[5][14]*dt;
1189 3248540 : nextP[9][14] = P[9][14];
1190 3248540 : nextP[10][14] = P[10][14];
1191 3248540 : nextP[11][14] = P[11][14];
1192 3248540 : nextP[12][14] = P[12][14];
1193 3248540 : nextP[13][14] = P[13][14];
1194 3248540 : nextP[14][14] = P[14][14];
1195 3248540 : nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18];
1196 3248540 : nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17];
1197 3248540 : nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16];
1198 3248540 : nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21];
1199 3248540 : nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11];
1200 3248540 : nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0];
1201 3248540 : nextP[6][15] = P[6][15] + P[3][15]*dt;
1202 3248540 : nextP[7][15] = P[7][15] + P[4][15]*dt;
1203 3248540 : nextP[8][15] = P[8][15] + P[5][15]*dt;
1204 3248540 : nextP[9][15] = P[9][15];
1205 3248540 : nextP[10][15] = P[10][15];
1206 3248540 : nextP[11][15] = P[11][15];
1207 3248540 : nextP[12][15] = P[12][15];
1208 3248540 : nextP[13][15] = P[13][15];
1209 3248540 : nextP[14][15] = P[14][15];
1210 3248540 : nextP[15][15] = P[15][15];
1211 :
1212 3248540 : if (stateIndexLim > 15) {
1213 2318440 : nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[8] + P[9][16]*SPP[22] + P[12][16]*SPP[18];
1214 2318440 : nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[9] + P[10][16]*SPP[22] + P[13][16]*SPP[17];
1215 2318440 : nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16];
1216 2318440 : nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21];
1217 2318440 : nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11];
1218 2318440 : nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[7] + P[1][16]*SPP[10] + P[2][16]*SPP[0];
1219 2318440 : nextP[6][16] = P[6][16] + P[3][16]*dt;
1220 2318440 : nextP[7][16] = P[7][16] + P[4][16]*dt;
1221 2318440 : nextP[8][16] = P[8][16] + P[5][16]*dt;
1222 2318440 : nextP[9][16] = P[9][16];
1223 2318440 : nextP[10][16] = P[10][16];
1224 2318440 : nextP[11][16] = P[11][16];
1225 2318440 : nextP[12][16] = P[12][16];
1226 2318440 : nextP[13][16] = P[13][16];
1227 2318440 : nextP[14][16] = P[14][16];
1228 2318440 : nextP[15][16] = P[15][16];
1229 2318440 : nextP[16][16] = P[16][16];
1230 2318440 : nextP[0][17] = P[0][17]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[8] + P[9][17]*SPP[22] + P[12][17]*SPP[18];
1231 2318440 : nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[9] + P[10][17]*SPP[22] + P[13][17]*SPP[17];
1232 2318440 : nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16];
1233 2318440 : nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21];
1234 2318440 : nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11];
1235 2318440 : nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[7] + P[1][17]*SPP[10] + P[2][17]*SPP[0];
1236 2318440 : nextP[6][17] = P[6][17] + P[3][17]*dt;
1237 2318440 : nextP[7][17] = P[7][17] + P[4][17]*dt;
1238 2318440 : nextP[8][17] = P[8][17] + P[5][17]*dt;
1239 2318440 : nextP[9][17] = P[9][17];
1240 2318440 : nextP[10][17] = P[10][17];
1241 2318440 : nextP[11][17] = P[11][17];
1242 2318440 : nextP[12][17] = P[12][17];
1243 2318440 : nextP[13][17] = P[13][17];
1244 2318440 : nextP[14][17] = P[14][17];
1245 2318440 : nextP[15][17] = P[15][17];
1246 2318440 : nextP[16][17] = P[16][17];
1247 2318440 : nextP[17][17] = P[17][17];
1248 2318440 : nextP[0][18] = P[0][18]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[8] + P[9][18]*SPP[22] + P[12][18]*SPP[18];
1249 2318440 : nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[9] + P[10][18]*SPP[22] + P[13][18]*SPP[17];
1250 2318440 : nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16];
1251 2318440 : nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21];
1252 2318440 : nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11];
1253 2318440 : nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[7] + P[1][18]*SPP[10] + P[2][18]*SPP[0];
1254 2318440 : nextP[6][18] = P[6][18] + P[3][18]*dt;
1255 2318440 : nextP[7][18] = P[7][18] + P[4][18]*dt;
1256 2318440 : nextP[8][18] = P[8][18] + P[5][18]*dt;
1257 2318440 : nextP[9][18] = P[9][18];
1258 2318440 : nextP[10][18] = P[10][18];
1259 2318440 : nextP[11][18] = P[11][18];
1260 2318440 : nextP[12][18] = P[12][18];
1261 2318440 : nextP[13][18] = P[13][18];
1262 2318440 : nextP[14][18] = P[14][18];
1263 2318440 : nextP[15][18] = P[15][18];
1264 2318440 : nextP[16][18] = P[16][18];
1265 2318440 : nextP[17][18] = P[17][18];
1266 2318440 : nextP[18][18] = P[18][18];
1267 2318440 : nextP[0][19] = P[0][19]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[8] + P[9][19]*SPP[22] + P[12][19]*SPP[18];
1268 2318440 : nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[9] + P[10][19]*SPP[22] + P[13][19]*SPP[17];
1269 2318440 : nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16];
1270 2318440 : nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21];
1271 2318440 : nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11];
1272 2318440 : nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[7] + P[1][19]*SPP[10] + P[2][19]*SPP[0];
1273 2318440 : nextP[6][19] = P[6][19] + P[3][19]*dt;
1274 2318440 : nextP[7][19] = P[7][19] + P[4][19]*dt;
1275 2318440 : nextP[8][19] = P[8][19] + P[5][19]*dt;
1276 2318440 : nextP[9][19] = P[9][19];
1277 2318440 : nextP[10][19] = P[10][19];
1278 2318440 : nextP[11][19] = P[11][19];
1279 2318440 : nextP[12][19] = P[12][19];
1280 2318440 : nextP[13][19] = P[13][19];
1281 2318440 : nextP[14][19] = P[14][19];
1282 2318440 : nextP[15][19] = P[15][19];
1283 2318440 : nextP[16][19] = P[16][19];
1284 2318440 : nextP[17][19] = P[17][19];
1285 2318440 : nextP[18][19] = P[18][19];
1286 2318440 : nextP[19][19] = P[19][19];
1287 2318440 : nextP[0][20] = P[0][20]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[8] + P[9][20]*SPP[22] + P[12][20]*SPP[18];
1288 2318440 : nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[9] + P[10][20]*SPP[22] + P[13][20]*SPP[17];
1289 2318440 : nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16];
1290 2318440 : nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21];
1291 2318440 : nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11];
1292 2318440 : nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[7] + P[1][20]*SPP[10] + P[2][20]*SPP[0];
1293 2318440 : nextP[6][20] = P[6][20] + P[3][20]*dt;
1294 2318440 : nextP[7][20] = P[7][20] + P[4][20]*dt;
1295 2318440 : nextP[8][20] = P[8][20] + P[5][20]*dt;
1296 2318440 : nextP[9][20] = P[9][20];
1297 2318440 : nextP[10][20] = P[10][20];
1298 2318440 : nextP[11][20] = P[11][20];
1299 2318440 : nextP[12][20] = P[12][20];
1300 2318440 : nextP[13][20] = P[13][20];
1301 2318440 : nextP[14][20] = P[14][20];
1302 2318440 : nextP[15][20] = P[15][20];
1303 2318440 : nextP[16][20] = P[16][20];
1304 2318440 : nextP[17][20] = P[17][20];
1305 2318440 : nextP[18][20] = P[18][20];
1306 2318440 : nextP[19][20] = P[19][20];
1307 2318440 : nextP[20][20] = P[20][20];
1308 2318440 : nextP[0][21] = P[0][21]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[8] + P[9][21]*SPP[22] + P[12][21]*SPP[18];
1309 2318440 : nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[9] + P[10][21]*SPP[22] + P[13][21]*SPP[17];
1310 2318440 : nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16];
1311 2318440 : nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21];
1312 2318440 : nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11];
1313 2318440 : nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[7] + P[1][21]*SPP[10] + P[2][21]*SPP[0];
1314 2318440 : nextP[6][21] = P[6][21] + P[3][21]*dt;
1315 2318440 : nextP[7][21] = P[7][21] + P[4][21]*dt;
1316 2318440 : nextP[8][21] = P[8][21] + P[5][21]*dt;
1317 2318440 : nextP[9][21] = P[9][21];
1318 2318440 : nextP[10][21] = P[10][21];
1319 2318440 : nextP[11][21] = P[11][21];
1320 2318440 : nextP[12][21] = P[12][21];
1321 2318440 : nextP[13][21] = P[13][21];
1322 2318440 : nextP[14][21] = P[14][21];
1323 2318440 : nextP[15][21] = P[15][21];
1324 2318440 : nextP[16][21] = P[16][21];
1325 2318440 : nextP[17][21] = P[17][21];
1326 2318440 : nextP[18][21] = P[18][21];
1327 2318440 : nextP[19][21] = P[19][21];
1328 2318440 : nextP[20][21] = P[20][21];
1329 2318440 : nextP[21][21] = P[21][21];
1330 :
1331 2318440 : if (stateIndexLim > 21) {
1332 2057185 : nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[8] + P[9][22]*SPP[22] + P[12][22]*SPP[18];
1333 2057185 : nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[9] + P[10][22]*SPP[22] + P[13][22]*SPP[17];
1334 2057185 : nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16];
1335 2057185 : nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21];
1336 2057185 : nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11];
1337 2057185 : nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[7] + P[1][22]*SPP[10] + P[2][22]*SPP[0];
1338 2057185 : nextP[6][22] = P[6][22] + P[3][22]*dt;
1339 2057185 : nextP[7][22] = P[7][22] + P[4][22]*dt;
1340 2057185 : nextP[8][22] = P[8][22] + P[5][22]*dt;
1341 2057185 : nextP[9][22] = P[9][22];
1342 2057185 : nextP[10][22] = P[10][22];
1343 2057185 : nextP[11][22] = P[11][22];
1344 2057185 : nextP[12][22] = P[12][22];
1345 2057185 : nextP[13][22] = P[13][22];
1346 2057185 : nextP[14][22] = P[14][22];
1347 2057185 : nextP[15][22] = P[15][22];
1348 2057185 : nextP[16][22] = P[16][22];
1349 2057185 : nextP[17][22] = P[17][22];
1350 2057185 : nextP[18][22] = P[18][22];
1351 2057185 : nextP[19][22] = P[19][22];
1352 2057185 : nextP[20][22] = P[20][22];
1353 2057185 : nextP[21][22] = P[21][22];
1354 2057185 : nextP[22][22] = P[22][22];
1355 2057185 : nextP[0][23] = P[0][23]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[8] + P[9][23]*SPP[22] + P[12][23]*SPP[18];
1356 2057185 : nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[9] + P[10][23]*SPP[22] + P[13][23]*SPP[17];
1357 2057185 : nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16];
1358 2057185 : nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21];
1359 2057185 : nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11];
1360 2057185 : nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[7] + P[1][23]*SPP[10] + P[2][23]*SPP[0];
1361 2057185 : nextP[6][23] = P[6][23] + P[3][23]*dt;
1362 2057185 : nextP[7][23] = P[7][23] + P[4][23]*dt;
1363 2057185 : nextP[8][23] = P[8][23] + P[5][23]*dt;
1364 2057185 : nextP[9][23] = P[9][23];
1365 2057185 : nextP[10][23] = P[10][23];
1366 2057185 : nextP[11][23] = P[11][23];
1367 2057185 : nextP[12][23] = P[12][23];
1368 2057185 : nextP[13][23] = P[13][23];
1369 2057185 : nextP[14][23] = P[14][23];
1370 2057185 : nextP[15][23] = P[15][23];
1371 2057185 : nextP[16][23] = P[16][23];
1372 2057185 : nextP[17][23] = P[17][23];
1373 2057185 : nextP[18][23] = P[18][23];
1374 2057185 : nextP[19][23] = P[19][23];
1375 2057185 : nextP[20][23] = P[20][23];
1376 2057185 : nextP[21][23] = P[21][23];
1377 2057185 : nextP[22][23] = P[22][23];
1378 2057185 : nextP[23][23] = P[23][23];
1379 : }
1380 : }
1381 :
1382 : // Copy upper diagonal to lower diagonal taking advantage of symmetry
1383 73250190 : for (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++)
1384 : {
1385 809746615 : for (uint8_t rowIndex=0; rowIndex<colIndex; rowIndex++)
1386 : {
1387 739744965 : nextP[colIndex][rowIndex] = nextP[rowIndex][colIndex];
1388 : }
1389 : }
1390 :
1391 : // add the general state process noise variances
1392 73250190 : for (uint8_t i=0; i<=stateIndexLim; i++)
1393 : {
1394 70001650 : nextP[i][i] = nextP[i][i] + processNoise[i];
1395 : }
1396 :
1397 : // if the total position variance exceeds 1e4 (100m), then stop covariance
1398 : // growth by setting the predicted to the previous values
1399 : // This prevent an ill conditioned matrix from occurring for long periods
1400 : // without GPS
1401 3248540 : if ((P[6][6] + P[7][7]) > 1e4f)
1402 : {
1403 54 : for (uint8_t i=6; i<=7; i++)
1404 : {
1405 900 : for (uint8_t j=0; j<=stateIndexLim; j++)
1406 : {
1407 864 : nextP[i][j] = P[i][j];
1408 864 : nextP[j][i] = P[j][i];
1409 : }
1410 : }
1411 : }
1412 :
1413 : // copy covariances to output
1414 3248540 : CopyCovariances();
1415 :
1416 : // constrain diagonals to prevent ill-conditioning
1417 3248540 : ConstrainVariances();
1418 3248540 : }
1419 :
1420 : // zero specified range of rows in the state covariance matrix
1421 2313418 : void NavEKF2_core::zeroRows(Matrix24 &covMat, uint8_t first, uint8_t last)
1422 : {
1423 : uint8_t row;
1424 11847739 : for (row=first; row<=last; row++)
1425 : {
1426 9534321 : zero_range(&covMat[row][0], 0, 23);
1427 : }
1428 2313418 : }
1429 :
1430 : // zero specified range of columns in the state covariance matrix
1431 2313418 : void NavEKF2_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
1432 : {
1433 : uint8_t row;
1434 57835450 : for (row=0; row<=23; row++)
1435 : {
1436 55522032 : zero_range(&covMat[row][0], first, last);
1437 : }
1438 2313418 : }
1439 :
1440 : // reset the output data to the current EKF state
1441 558 : void NavEKF2_core::StoreOutputReset()
1442 : {
1443 558 : outputDataNew.quat = stateStruct.quat;
1444 558 : outputDataNew.velocity = stateStruct.velocity;
1445 558 : outputDataNew.position = stateStruct.position;
1446 : // write current measurement to entire table
1447 8514 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1448 7956 : storedOutput[i] = outputDataNew;
1449 : }
1450 558 : outputDataDelayed = outputDataNew;
1451 : // reset the states for the complementary filter used to provide a vertical position dervative output
1452 558 : vertCompFiltState.pos = stateStruct.position.z;
1453 558 : vertCompFiltState.vel = stateStruct.velocity.z;
1454 558 : }
1455 :
1456 : // Reset the stored output quaternion history to current EKF state
1457 0 : void NavEKF2_core::StoreQuatReset()
1458 : {
1459 0 : outputDataNew.quat = stateStruct.quat;
1460 : // write current measurement to entire table
1461 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1462 0 : storedOutput[i].quat = outputDataNew.quat;
1463 : }
1464 0 : outputDataDelayed.quat = outputDataNew.quat;
1465 0 : }
1466 :
1467 : // Rotate the stored output quaternion history through a quaternion rotation
1468 836 : void NavEKF2_core::StoreQuatRotate(const QuaternionF &deltaQuat)
1469 : {
1470 836 : outputDataNew.quat = outputDataNew.quat*deltaQuat;
1471 : // write current measurement to entire table
1472 12770 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1473 11934 : storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
1474 : }
1475 836 : outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
1476 836 : }
1477 :
1478 : // force symmetry on the covariance matrix to prevent ill-conditioning
1479 4426288 : void NavEKF2_core::ForceSymmetry()
1480 : {
1481 99920494 : for (uint8_t i=1; i<=stateIndexLim; i++)
1482 : {
1483 1192443705 : for (uint8_t j=0; j<=i-1; j++)
1484 : {
1485 1096949499 : ftype temp = 0.5f*(P[i][j] + P[j][i]);
1486 1096949499 : P[i][j] = temp;
1487 1096949499 : P[j][i] = temp;
1488 : }
1489 : }
1490 4426288 : }
1491 :
1492 : // copy covariances across from covariance prediction calculation
1493 3248540 : void NavEKF2_core::CopyCovariances()
1494 : {
1495 : // copy predicted covariances
1496 73250190 : for (uint8_t i=0; i<=stateIndexLim; i++) {
1497 1619493230 : for (uint8_t j=0; j<=stateIndexLim; j++)
1498 : {
1499 1549491580 : P[i][j] = nextP[i][j];
1500 : }
1501 : }
1502 3248540 : }
1503 :
1504 : // constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
1505 7674828 : void NavEKF2_core::ConstrainVariances()
1506 : {
1507 30699312 : for (uint8_t i=0; i<=2; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0f); // attitude error
1508 30699312 : for (uint8_t i=3; i<=5; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); // velocities
1509 23024484 : for (uint8_t i=6; i<=7; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,EK2_POSXY_STATE_LIMIT);
1510 7674828 : P[8][8] = constrain_ftype(P[8][8],0.0f,1.0e6f); // vertical position
1511 30699312 : for (uint8_t i=9; i<=11; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,sq(0.175f * dtEkfAvg)); // delta angle biases
1512 7674828 : if (PV_AidingMode != AID_NONE) {
1513 26323508 : for (uint8_t i=12; i<=14; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // delta angle scale factors
1514 : } else {
1515 : // we can't reliably estimate scale factors when there is no aiding data due to transient manoeuvre induced innovations
1516 : // so inhibit estimation by keeping covariance elements at zero
1517 1093951 : zeroRows(P,12,14);
1518 1093951 : zeroCols(P,12,14);
1519 : }
1520 7674828 : P[15][15] = constrain_ftype(P[15][15],0.0f,sq(10.0f * dtEkfAvg)); // delta velocity bias
1521 30699312 : for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // earth magnetic field
1522 30699312 : for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // body magnetic field
1523 23024484 : for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,1.0e3f); // wind velocity
1524 7674828 : }
1525 :
1526 : // constrain states using WMM tables and specified limit
1527 4304607 : void NavEKF2_core::MagTableConstrain(void)
1528 : {
1529 : // constrain to error from table earth field
1530 4304607 : ftype limit_ga = frontend->_mag_ef_limit * 0.001f;
1531 4304607 : stateStruct.earth_magfield.x = constrain_ftype(stateStruct.earth_magfield.x,
1532 : table_earth_field_ga.x-limit_ga,
1533 : table_earth_field_ga.x+limit_ga);
1534 4304607 : stateStruct.earth_magfield.y = constrain_ftype(stateStruct.earth_magfield.y,
1535 : table_earth_field_ga.y-limit_ga,
1536 : table_earth_field_ga.y+limit_ga);
1537 4304607 : stateStruct.earth_magfield.z = constrain_ftype(stateStruct.earth_magfield.z,
1538 : table_earth_field_ga.z-limit_ga,
1539 : table_earth_field_ga.z+limit_ga);
1540 4304607 : }
1541 :
1542 : // constrain states to prevent ill-conditioning
1543 3248540 : void NavEKF2_core::ConstrainStates()
1544 : {
1545 : // attitude errors are limited between +-1
1546 12994160 : for (uint8_t i=0; i<=2; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f);
1547 : // velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
1548 12994160 : for (uint8_t i=3; i<=5; i++) statesArray[i] = constrain_ftype(statesArray[i],-5.0e2f,5.0e2f);
1549 : // position limit - TODO apply circular limit
1550 9745620 : for (uint8_t i=6; i<=7; i++) statesArray[i] = constrain_ftype(statesArray[i],-EK2_POSXY_STATE_LIMIT,EK2_POSXY_STATE_LIMIT);
1551 : // height limit covers home alt on everest through to home alt at SL and ballon drop
1552 3248540 : stateStruct.position.z = constrain_ftype(stateStruct.position.z,-4.0e4f,1.0e4f);
1553 : // gyro bias limit (this needs to be set based on manufacturers specs)
1554 12994160 : for (uint8_t i=9; i<=11; i++) statesArray[i] = constrain_ftype(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg);
1555 : // gyro scale factor limit of +-5% (this needs to be set based on manufacturers specs)
1556 12994160 : for (uint8_t i=12; i<=14; i++) statesArray[i] = constrain_ftype(statesArray[i],0.95f,1.05f);
1557 : // Z accel bias limit 1.0 m/s^2 (this needs to be finalised from test data)
1558 3248540 : stateStruct.accel_zbias = constrain_ftype(stateStruct.accel_zbias,-1.0f*dtEkfAvg,1.0f*dtEkfAvg);
1559 :
1560 : // earth magnetic field limit
1561 3248540 : if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) {
1562 : // constrain to +/-1Ga
1563 1023560 : for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f);
1564 : } else {
1565 : // use table constrain
1566 2992650 : MagTableConstrain();
1567 : }
1568 :
1569 : // body magnetic field limit
1570 12994160 : for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_ftype(statesArray[i],-0.5f,0.5f);
1571 : // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
1572 9745620 : for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_ftype(statesArray[i],-100.0f,100.0f);
1573 : // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum
1574 3248540 : if (!inhibitGndState) {
1575 3248540 : terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
1576 : }
1577 3248540 : }
1578 :
1579 : // calculate the NED earth spin vector in rad/sec
1580 952 : void NavEKF2_core::calcEarthRateNED(Vector3F &omega, int32_t latitude) const
1581 : {
1582 952 : ftype lat_rad = radians(latitude*1.0e-7f);
1583 952 : omega.x = earthRate*cosF(lat_rad);
1584 952 : omega.y = 0;
1585 952 : omega.z = -earthRate*sinF(lat_rad);
1586 952 : }
1587 :
1588 : // initialise the earth magnetic field states using declination, suppled roll/pitch
1589 : // and magnetometer measurements and return initial attitude quaternion
1590 850 : QuaternionF NavEKF2_core::calcQuatAndFieldStates(ftype roll, ftype pitch)
1591 : {
1592 : // declare local variables required to calculate initial orientation and magnetic field
1593 : ftype yaw;
1594 850 : Matrix3F Tbn;
1595 : Vector3F initMagNED;
1596 850 : QuaternionF initQuat;
1597 :
1598 850 : if (use_compass()) {
1599 : // calculate rotation matrix from body to NED frame
1600 850 : Tbn.from_euler(roll, pitch, 0.0f);
1601 :
1602 : // read the magnetometer data
1603 850 : readMagData();
1604 :
1605 : // rotate the magnetic field into NED axes
1606 850 : initMagNED = Tbn * magDataDelayed.mag;
1607 :
1608 : // calculate heading of mag field rel to body heading
1609 850 : ftype magHeading = atan2F(initMagNED.y, initMagNED.x);
1610 :
1611 : // get the magnetic declination
1612 850 : ftype magDecAng = MagDeclination();
1613 :
1614 : // calculate yaw angle rel to true north
1615 850 : yaw = magDecAng - magHeading;
1616 :
1617 : // calculate initial filter quaternion states using yaw from magnetometer
1618 : // record the reset so that it can be detected externally for use by the control loops to prevent yaw disturbances following a reset
1619 850 : yawResetCount++;
1620 : // calculate an initial quaternion using the new yaw value
1621 850 : initQuat.from_euler(roll, pitch, yaw);
1622 : // zero the attitude covariances because the corelations will now be invalid
1623 850 : zeroAttCovOnly();
1624 :
1625 : // calculate initial Tbn matrix and rotate Mag measurements into NED
1626 : // to set initial NED magnetic field states
1627 : // don't do this if the earth field has already been learned
1628 850 : if (!magFieldLearned) {
1629 848 : initQuat.rotation_matrix(Tbn);
1630 848 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
1631 292 : stateStruct.earth_magfield = table_earth_field_ga;
1632 : } else {
1633 556 : stateStruct.earth_magfield = Tbn * magDataDelayed.mag;
1634 : }
1635 :
1636 : // set the NE earth magnetic field states using the published declination
1637 : // and set the corresponding variances and covariances
1638 848 : alignMagStateDeclination();
1639 :
1640 : // set the remaining variances and covariances
1641 848 : zeroRows(P,18,21);
1642 848 : zeroCols(P,18,21);
1643 848 : P[18][18] = sq(frontend->_magNoise);
1644 848 : P[19][19] = P[18][18];
1645 848 : P[20][20] = P[18][18];
1646 848 : P[21][21] = P[18][18];
1647 :
1648 : }
1649 :
1650 : // record the fact we have initialised the magnetic field states
1651 850 : recordMagReset();
1652 :
1653 : // clear mag state reset request
1654 850 : magStateResetRequest = false;
1655 :
1656 : } else {
1657 : // this function should not be called if there is no compass data but if is is, return the
1658 : // current attitude
1659 0 : initQuat = stateStruct.quat;
1660 : }
1661 :
1662 : // return attitude quaternion
1663 850 : return initQuat;
1664 : }
1665 :
1666 : // zero the attitude covariances, but preserve the variances
1667 860 : void NavEKF2_core::zeroAttCovOnly()
1668 : {
1669 : ftype varTemp[3];
1670 3440 : for (uint8_t index=0; index<=2; index++) {
1671 2580 : varTemp[index] = P[index][index];
1672 : }
1673 860 : zeroCols(P,0,2);
1674 860 : zeroRows(P,0,2);
1675 3440 : for (uint8_t index=0; index<=2; index++) {
1676 2580 : P[index][index] = varTemp[index];
1677 : }
1678 860 : }
1679 :
|
