Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_core.h"
5 : #include <GCS_MAVLink/GCS.h>
6 : #include <AP_VisualOdom/AP_VisualOdom.h>
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <AP_DAL/AP_DAL.h>
9 :
10 : // constructor
11 3989 : NavEKF3_core::NavEKF3_core(NavEKF3 *_frontend, AP_DAL &_dal) :
12 3989 : dal(_dal),
13 3989 : frontend(_frontend),
14 3989 : public_origin(frontend->common_EKF_origin)
15 : {
16 3989 : firstInitTime_ms = 0;
17 3989 : lastInitFailReport_ms = 0;
18 3989 : }
19 :
20 : // setup this core backend
21 48273 : bool NavEKF3_core::setup_core(uint8_t _imu_index, uint8_t _core_index)
22 : {
23 48273 : imu_index = _imu_index;
24 48273 : gyro_index_active = imu_index;
25 48273 : accel_index_active = imu_index;
26 48273 : core_index = _core_index;
27 :
28 : /*
29 : The imu_buffer_length needs to cope with the worst case sensor delay at the
30 : target EKF state prediction rate. Non-IMU data coming in faster is downsampled.
31 : */
32 :
33 : // Calculate the expected EKF time step
34 48273 : if (dal.ins().get_loop_rate_hz() > 0) {
35 48273 : dtEkfAvg = 1.0f / dal.ins().get_loop_rate_hz();
36 48273 : dtEkfAvg = MAX(dtEkfAvg,EKF_TARGET_DT);
37 : } else {
38 : return false;
39 : }
40 :
41 : // find the maximum time delay for all potential sensors
42 48273 : uint16_t maxTimeDelay_ms = MAX(frontend->_hgtDelay_ms ,
43 48273 : MAX(frontend->_flowDelay_ms ,
44 48273 : MAX(frontend->_rngBcnDelay_ms ,
45 48273 : MAX(frontend->magDelay_ms ,
46 48273 : (uint16_t)(EKF_TARGET_DT_MS)
47 48273 : ))));
48 :
49 : // GPS sensing can have large delays and should not be included if disabled
50 48273 : if (frontend->sources.usingGPS(core_index)) {
51 : // Wait for the configuration of all GPS units to be confirmed. Until this has occurred the GPS driver cannot provide a correct time delay
52 48231 : float gps_delay_sec = 0;
53 48231 : if (!dal.gps().get_lag(selected_gps, gps_delay_sec)) {
54 : #if HAL_GCS_ENABLED
55 44318 : const uint32_t now = dal.millis();
56 44318 : if (now - lastInitFailReport_ms > 10000) {
57 16 : lastInitFailReport_ms = now;
58 : // provide an escalating series of messages
59 : MAV_SEVERITY severity = MAV_SEVERITY_INFO;
60 16 : if (now > 30000) {
61 : severity = MAV_SEVERITY_ERROR;
62 16 : } else if (now > 15000) {
63 : severity = MAV_SEVERITY_WARNING;
64 : }
65 16 : GCS_SEND_TEXT(severity, "EKF3 waiting for GPS config data");
66 : }
67 : #endif
68 44318 : return false;
69 : }
70 : // limit the time delay value from the GPS library to a max of 250 msec which is the max value the EKF has been tested for.
71 3913 : maxTimeDelay_ms = MAX(maxTimeDelay_ms , MIN((uint16_t)(gps_delay_sec * 1000.0f),250));
72 : }
73 :
74 : // airspeed sensing can have large delays and should not be included if disabled
75 3955 : if (dal.airspeed_sensor_enabled()) {
76 912 : maxTimeDelay_ms = MAX(maxTimeDelay_ms , frontend->tasDelay_ms);
77 : }
78 :
79 : #if HAL_VISUALODOM_ENABLED
80 : // include delay from visual odometry if enabled
81 3955 : const auto *visual_odom = dal.visualodom();
82 3955 : if ((visual_odom != nullptr) && visual_odom->enabled()) {
83 20 : maxTimeDelay_ms = MAX(maxTimeDelay_ms, MIN(visual_odom->get_delay_ms(), 250));
84 : }
85 : #endif
86 :
87 : // calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter
88 3955 : imu_buffer_length = (maxTimeDelay_ms / (uint16_t)(EKF_TARGET_DT_MS)) + 1;
89 :
90 : // set the observation buffer length to handle the minimum time of arrival between observations in combination
91 : // with the worst case delay from current time to ekf fusion time
92 : // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
93 3955 : uint16_t ekf_delay_ms = maxTimeDelay_ms + (int)(ceilF((ftype)maxTimeDelay_ms * 0.5f));
94 3955 : obs_buffer_length = (ekf_delay_ms / frontend->sensorIntervalMin_ms) + 1;
95 :
96 : // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
97 3955 : obs_buffer_length = MIN(obs_buffer_length,imu_buffer_length);
98 :
99 : // calculate buffer size for optical flow data
100 3955 : const uint8_t flow_buffer_length = MIN((ekf_delay_ms / frontend->flowIntervalMin_ms) + 1, imu_buffer_length);
101 :
102 : #if EK3_FEATURE_EXTERNAL_NAV
103 : // calculate buffer size for external nav data
104 3955 : const uint8_t extnav_buffer_length = MIN((ekf_delay_ms / frontend->extNavIntervalMin_ms) + 1, imu_buffer_length);
105 : #endif // EK3_FEATURE_EXTERNAL_NAV
106 :
107 3955 : if(!storedGPS.init(obs_buffer_length)) {
108 : return false;
109 : }
110 3955 : if(!storedMag.init(obs_buffer_length)) {
111 : return false;
112 : }
113 3955 : if(!storedBaro.init(obs_buffer_length)) {
114 : return false;
115 : }
116 3955 : if(dal.airspeed() && !storedTAS.init(obs_buffer_length)) {
117 : return false;
118 : }
119 3955 : if(dal.opticalflow_enabled() && !storedOF.init(flow_buffer_length)) {
120 : return false;
121 : }
122 : #if EK3_FEATURE_BODY_ODOM
123 3955 : if(frontend->sources.ext_nav_enabled() && !storedBodyOdm.init(obs_buffer_length)) {
124 : return false;
125 : }
126 3955 : if(frontend->sources.wheel_encoder_enabled() && !storedWheelOdm.init(imu_buffer_length)) {
127 : // initialise to same length of IMU to allow for multiple wheel sensors
128 : return false;
129 : }
130 : #endif // EK3_FEATURE_BODY_ODOM
131 3955 : if(frontend->sources.gps_yaw_enabled() && !storedYawAng.init(obs_buffer_length)) {
132 : return false;
133 : }
134 : #if AP_RANGEFINDER_ENABLED
135 : // Note: the use of dual range finders potentially doubles the amount of data to be stored
136 3955 : if(dal.rangefinder() && !storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
137 0 : return false;
138 : }
139 : #endif
140 : // Note: range beacon data is read one beacon at a time and can arrive at a high rate
141 : #if EK3_FEATURE_BEACON_FUSION
142 3955 : if(dal.beacon() && !rngBcn.storedRange.init(imu_buffer_length+1)) {
143 : return false;
144 : }
145 : #endif
146 : #if EK3_FEATURE_EXTERNAL_NAV
147 3955 : if (frontend->sources.ext_nav_enabled() && !storedExtNav.init(extnav_buffer_length)) {
148 : return false;
149 : }
150 3955 : if (frontend->sources.ext_nav_enabled() && !storedExtNavVel.init(extnav_buffer_length)) {
151 : return false;
152 : }
153 3955 : if(frontend->sources.ext_nav_enabled() && !storedExtNavYawAng.init(extnav_buffer_length)) {
154 : return false;
155 : }
156 : #endif // EK3_FEATURE_EXTERNAL_NAV
157 3955 : if(!storedIMU.init(imu_buffer_length)) {
158 : return false;
159 : }
160 3955 : if(!storedOutput.init(imu_buffer_length)) {
161 : return false;
162 : }
163 : #if EK3_FEATURE_DRAG_FUSION
164 3955 : if (!storedDrag.init(obs_buffer_length)) {
165 : return false;
166 : }
167 : #endif
168 :
169 3955 : if ((yawEstimator == nullptr) && (frontend->_gsfRunMask & (1U<<core_index))) {
170 : // check if there is enough memory to create the EKF-GSF object
171 3954 : if (dal.available_memory() < sizeof(EKFGSF_yaw) + 1024) {
172 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 IMU%u GSF: not enough memory",(unsigned)imu_index);
173 0 : return false;
174 : }
175 :
176 : // try to instantiate
177 3954 : yawEstimator = NEW_NOTHROW EKFGSF_yaw();
178 3954 : if (yawEstimator == nullptr) {
179 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "EKF3 IMU%uGSF: allocation failed",(unsigned)imu_index);
180 0 : return false;
181 : }
182 : }
183 :
184 : return true;
185 : }
186 :
187 :
188 : /********************************************************
189 : * INIT FUNCTIONS *
190 : ********************************************************/
191 :
192 : // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
193 3144 : void NavEKF3_core::InitialiseVariables()
194 : {
195 : // calculate the nominal filter update rate
196 3144 : const auto &ins = dal.ins();
197 3144 : localFilterTimeStep_ms = (uint8_t)(1000*ins.get_loop_delta_t());
198 3144 : localFilterTimeStep_ms = MAX(localFilterTimeStep_ms, (uint8_t)EKF_TARGET_DT_MS);
199 :
200 : // initialise time stamps
201 3144 : imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
202 3144 : prevTasStep_ms = imuSampleTime_ms;
203 3144 : prevBetaDragStep_ms = imuSampleTime_ms;
204 3144 : lastBaroReceived_ms = imuSampleTime_ms;
205 3144 : lastVelPassTime_ms = 0;
206 3144 : lastGpsPosPassTime_ms = 0;
207 3144 : lastHgtPassTime_ms = 0;
208 3144 : lastTasPassTime_ms = 0;
209 3144 : lastSynthYawTime_ms = 0;
210 3144 : lastTimeGpsReceived_ms = 0;
211 3144 : timeAtLastAuxEKF_ms = imuSampleTime_ms;
212 3144 : flowValidMeaTime_ms = imuSampleTime_ms;
213 3144 : rngValidMeaTime_ms = imuSampleTime_ms;
214 3144 : flowMeaTime_ms = 0;
215 3144 : prevFlowFuseTime_ms = 0;
216 3144 : gndHgtValidTime_ms = 0;
217 3144 : ekfStartTime_ms = imuSampleTime_ms;
218 3144 : lastGpsVelFail_ms = 0;
219 3144 : lastGpsAidBadTime_ms = 0;
220 3144 : timeTasReceived_ms = 0;
221 3144 : lastPreAlignGpsCheckTime_ms = imuSampleTime_ms;
222 3144 : posNEResetCount = 0;
223 3144 : posDResetCount = 0;
224 3144 : lastRngMeasTime_ms = 0;
225 :
226 : // initialise other variables
227 3144 : memset(&dvelBiasAxisInhibit, 0, sizeof(dvelBiasAxisInhibit));
228 3144 : dvelBiasAxisVarPrev.zero();
229 3144 : gpsNoiseScaler = 1.0f;
230 3144 : hgtTimeout = true;
231 3144 : tasTimeout = true;
232 3144 : dragTimeout = true;
233 3144 : badIMUdata = false;
234 3144 : badIMUdata_ms = 0;
235 3144 : goodIMUdata_ms = 0;
236 3144 : vertVelVarClipCounter = 0;
237 3144 : finalInflightYawInit = false;
238 3144 : dtIMUavg = ins.get_loop_delta_t();
239 3144 : dtEkfAvg = EKF_TARGET_DT;
240 3144 : dt = 0;
241 3144 : velDotNEDfilt.zero();
242 3144 : lastKnownPositionNE.zero();
243 3144 : lastKnownPositionD = 0;
244 3144 : prevTnb.zero();
245 3144 : memset(&P[0][0], 0, sizeof(P));
246 3144 : memset(&KHP[0][0], 0, sizeof(KHP));
247 3144 : flowDataValid = false;
248 3144 : rangeDataToFuse = false;
249 : #if EK3_FEATURE_OPTFLOW_FUSION
250 3144 : Popt = 0.0f;
251 : #endif
252 3144 : terrainState = 0.0f;
253 3144 : prevPosN = stateStruct.position.x;
254 3144 : prevPosE = stateStruct.position.y;
255 3144 : inhibitGndState = false;
256 3144 : flowGyroBias.x = 0;
257 3144 : flowGyroBias.y = 0;
258 3144 : PV_AidingMode = AID_NONE;
259 3144 : PV_AidingModePrev = AID_NONE;
260 3144 : posTimeout = true;
261 3144 : velTimeout = true;
262 3144 : velAiding = false;
263 3144 : waitingForGpsChecks = false;
264 3144 : memset(&faultStatus, 0, sizeof(faultStatus));
265 3144 : hgtRate = 0.0f;
266 3144 : onGround = true;
267 3144 : prevOnGround = true;
268 3144 : inFlight = false;
269 3144 : prevInFlight = false;
270 3144 : manoeuvring = false;
271 3144 : fusingStationaryZeroVel = false;
272 3144 : inhibitWindStates = true;
273 3144 : windStateIsObservable = false;
274 3144 : treatWindStatesAsTruth = false;
275 3144 : lastAspdEstIsValid = false;
276 3144 : windStatesAligned = false;
277 3144 : inhibitDelVelBiasStates = true;
278 3144 : inhibitDelAngBiasStates = true;
279 3144 : gndOffsetValid = false;
280 3144 : validOrigin = false;
281 3144 : gpsSpdAccuracy = 0.0f;
282 3144 : gpsPosAccuracy = 0.0f;
283 3144 : gpsHgtAccuracy = 0.0f;
284 3144 : baroHgtOffset = 0.0f;
285 3144 : rngOnGnd = 0.05f;
286 : #if EK3_FEATURE_OPTFLOW_AGL_KF
287 : // 2-state AGL KF initialisation
288 : // Start with generous uncertainty; the first valid RF measurement will hard-reset the state
289 3144 : aglKfH = rngOnGnd; // assume sitting on ground at minimum range
290 3144 : aglKfV = 0.0f;
291 3144 : aglKfP[0][0] = 25.0f; // 5 m initial std-dev in height
292 3144 : aglKfP[0][1] = 0.0f;
293 3144 : aglKfP[1][0] = 0.0f;
294 3144 : aglKfP[1][1] = 1.0f; // 1 m/s initial std-dev in velocity
295 3144 : aglKfValid = false;
296 3144 : lastAglRngFuseTime_ms = 0;
297 : #endif
298 3144 : yawResetCount = 0;
299 3144 : tiltErrorVariance = sq(M_2PI);
300 3144 : tiltAlignComplete = false;
301 3144 : yawAlignComplete = false;
302 3144 : yawAlignGpsValidCount = 0;
303 3144 : have_table_earth_field = false;
304 3144 : stateIndexLim = 23;
305 3144 : last_gps_idx = 0;
306 3144 : delAngCorrection.zero();
307 3144 : velErrintegral.zero();
308 3144 : posErrintegral.zero();
309 3144 : gpsGoodToAlign = false;
310 3144 : gpsIsInUse = false;
311 3144 : motorsArmed = false;
312 3144 : prevMotorsArmed = false;
313 3144 : memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus));
314 3144 : gpsSpdAccPass = false;
315 3144 : ekfInnovationsPass = false;
316 3144 : sAccFilterState1 = 0.0f;
317 3144 : sAccFilterState2 = 0.0f;
318 3144 : lastGpsCheckTime_ms = 0;
319 3144 : lastGpsInnovPassTime_ms = 0;
320 3144 : lastGpsInnovFailTime_ms = 0;
321 3144 : lastGpsVertAccPassTime_ms = 0;
322 3144 : lastGpsVertAccFailTime_ms = 0;
323 3144 : gpsAccuracyGood = false;
324 3144 : gpsAccuracyGoodForAltitude = false;
325 3144 : gpsloc_prev = {};
326 3144 : gpsDriftNE = 0.0f;
327 3144 : gpsVertVelFilt = 0.0f;
328 3144 : gpsHorizVelFilt = 0.0f;
329 3144 : ZERO_FARRAY(statesArray);
330 3144 : memset(&vertCompFiltState, 0, sizeof(vertCompFiltState));
331 3144 : posVelFusionDelayed = false;
332 : #if EK3_FEATURE_OPTFLOW_FUSION
333 3144 : optFlowFusionDelayed = false;
334 : #endif
335 3144 : flowFusionActive = false;
336 3144 : airSpdFusionDelayed = false;
337 3144 : sideSlipFusionDelayed = false;
338 3144 : airDataFusionWindOnly = false;
339 3144 : posResetNE.zero();
340 3144 : posResetD = 0.0f;
341 3144 : hgtInnovFiltState = 0.0f;
342 3144 : imuDataDownSampledNew.delAng.zero();
343 3144 : imuDataDownSampledNew.delVel.zero();
344 3144 : imuDataDownSampledNew.delAngDT = 0.0f;
345 3144 : imuDataDownSampledNew.delVelDT = 0.0f;
346 3144 : imuDataDownSampledNew.gyro_index = gyro_index_active;
347 3144 : imuDataDownSampledNew.accel_index = accel_index_active;
348 3144 : runUpdates = false;
349 3144 : framesSincePredict = 0;
350 3144 : gpsYawResetRequest = false;
351 3144 : delAngBiasLearned = false;
352 3144 : memset(&filterStatus, 0, sizeof(filterStatus));
353 3144 : activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
354 3144 : prevHgtSource = activeHgtSource;
355 : #if EK3_FEATURE_RANGEFINDER_MEASUREMENTS
356 3144 : memset(&rngMeasIndex, 0, sizeof(rngMeasIndex));
357 3144 : memset(&storedRngMeasTime_ms, 0, sizeof(storedRngMeasTime_ms));
358 3144 : memset(&storedRngMeas, 0, sizeof(storedRngMeas));
359 : #endif
360 3144 : terrainHgtStable = true;
361 3144 : ekfOriginHgtVar = 0.0f;
362 3144 : ekfGpsRefHgt = 0.0;
363 3144 : velOffsetNED.zero();
364 3144 : posOffsetNED.zero();
365 3144 : ZERO_FARRAY(velPosObs);
366 :
367 : // range beacon fusion variables
368 : #if EK3_FEATURE_BEACON_FUSION
369 3144 : rngBcn.InitialiseVariables();
370 : #endif // EK3_FEATURE_BEACON_FUSION
371 :
372 : #if EK3_FEATURE_BODY_ODOM
373 : // body frame displacement fusion
374 3144 : memset((void *)&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
375 3144 : memset((void *)&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
376 : #endif
377 3144 : lastbodyVelPassTime_ms = 0;
378 3144 : ZERO_FARRAY(bodyVelTestRatio);
379 3144 : ZERO_FARRAY(varInnovBodyVel);
380 3144 : ZERO_FARRAY(innovBodyVel);
381 3144 : prevBodyVelFuseTime_ms = 0;
382 3144 : bodyOdmMeasTime_ms = 0;
383 3144 : bodyVelFusionDelayed = false;
384 3144 : bodyVelFusionActive = false;
385 :
386 : // yaw sensor fusion
387 3144 : yawMeasTime_ms = 0;
388 3144 : memset((void *)&yawAngDataNew, 0, sizeof(yawAngDataNew));
389 3144 : memset((void *)&yawAngDataDelayed, 0, sizeof(yawAngDataDelayed));
390 :
391 : #if EK3_FEATURE_EXTERNAL_NAV
392 : // external nav data fusion
393 3144 : extNavDataDelayed = {};
394 3144 : extNavMeasTime_ms = 0;
395 3144 : extNavLastPosResetTime_ms = 0;
396 3144 : extNavDataToFuse = false;
397 3144 : extNavUsedForPos = false;
398 3144 : extNavVelDelayed = {};
399 3144 : extNavVelToFuse = false;
400 3144 : useExtNavVel = false;
401 3144 : extNavVelMeasTime_ms = 0;
402 : #endif
403 :
404 : // zero data buffers
405 3144 : storedIMU.reset();
406 3144 : storedGPS.reset();
407 3144 : storedBaro.reset();
408 3144 : storedTAS.reset();
409 : #if EK3_FEATURE_RANGEFINDER_MEASUREMENTS
410 3144 : storedRange.reset();
411 : #endif
412 3144 : storedOutput.reset();
413 : #if EK3_FEATURE_BEACON_FUSION
414 3144 : rngBcn.storedRange.reset();
415 : #endif
416 : #if EK3_FEATURE_BODY_ODOM
417 3144 : storedBodyOdm.reset();
418 3144 : storedWheelOdm.reset();
419 : #endif
420 : #if EK3_FEATURE_EXTERNAL_NAV
421 3144 : storedExtNav.reset();
422 3144 : storedExtNavVel.reset();
423 : #endif
424 :
425 : // initialise pre-arm message
426 3144 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF3 still initialising");
427 :
428 3144 : InitialiseVariablesMag();
429 :
430 : // emergency reset of yaw to EKFGSF estimate
431 3144 : EKFGSF_yaw_reset_ms = 0;
432 3144 : EKFGSF_yaw_reset_request_ms = 0;
433 3144 : EKFGSF_yaw_reset_count = 0;
434 3144 : EKFGSF_run_filterbank = false;
435 3144 : EKFGSF_yaw_valid_count = 0;
436 :
437 3144 : effectiveMagCal = effective_magCal();
438 3144 : }
439 :
440 : // Use a function call rather than a constructor to initialise variables because it enables the filter to be re-started in flight if necessary.
441 3144 : void NavEKF3_core::InitialiseVariablesMag()
442 : {
443 3144 : lastHealthyMagTime_ms = imuSampleTime_ms;
444 3144 : lastMagUpdate_us = 0;
445 3144 : magYawResetTimer_ms = imuSampleTime_ms;
446 3144 : magTimeout = false;
447 3144 : allMagSensorsFailed = false;
448 3144 : finalInflightMagInit = false;
449 3144 : inhibitMagStates = true;
450 3144 : magSelectIndex = dal.compass().get_first_usable();
451 3144 : lastMagOffsetsValid = false;
452 3144 : magStateResetRequest = false;
453 3144 : magStateInitComplete = false;
454 3144 : magYawResetRequest = false;
455 3144 : posDownAtLastMagReset = stateStruct.position.z;
456 3144 : yawInnovAtLastMagReset = 0.0f;
457 3144 : stateStruct.quat.initialise();
458 3144 : quatAtLastMagReset = stateStruct.quat;
459 3144 : magFieldLearned = false;
460 3144 : storedMag.reset();
461 3144 : storedYawAng.reset();
462 : #if EK3_FEATURE_EXTERNAL_NAV
463 3144 : storedExtNavYawAng.reset();
464 : #endif
465 3144 : needMagBodyVarReset = false;
466 3144 : needEarthBodyVarReset = false;
467 3144 : magFusionSel = MagFuseSel::NOT_FUSING;
468 3144 : }
469 :
470 : /*
471 : Initialise the states from accelerometer data. This assumes measured acceleration
472 : is dominated by gravity. If this assumption is not true then the EKF will require
473 : timee to reduce the resulting tilt error. Yaw alignment is not performed by this
474 : function, but is perfomred later and initiated the SelectMagFusion() function
475 : after the tilt has stabilised.
476 : */
477 :
478 1466953 : bool NavEKF3_core::InitialiseFilterBootstrap(void)
479 : {
480 : // update sensor selection (for affinity)
481 1466953 : update_sensor_selection();
482 :
483 : // If we are a plane and don't have GPS lock then don't initialise
484 1466953 : if (assume_zero_sideslip() && dal.gps().status(preferred_gps) < AP_GPS_FixType::FIX_3D) {
485 561320 : dal.snprintf(prearm_fail_string,
486 : sizeof(prearm_fail_string),
487 : "EKF3 init failure: No GPS lock");
488 561320 : statesInitialised = false;
489 561320 : return false;
490 : }
491 :
492 : // read all the sensors required to start the EKF the states
493 905633 : readIMUData(false); // don't allow prediction
494 905633 : readMagData();
495 905633 : readGpsData();
496 905633 : readGpsYawData();
497 905633 : readBaroData();
498 :
499 905633 : if (statesInitialised) {
500 : // we are initialised, but we don't return true until the IMU
501 : // buffer has been filled. This prevents a timing
502 : // vulnerability with a pause in IMU data during filter startup
503 3191 : return storedIMU.is_filled();
504 : }
505 :
506 : // accumulate enough sensor data to fill the buffers
507 902442 : if (firstInitTime_ms == 0) {
508 3329 : firstInitTime_ms = imuSampleTime_ms;
509 3329 : return false;
510 899113 : } else if (imuSampleTime_ms - firstInitTime_ms < 1000) {
511 : return false;
512 : }
513 :
514 : // set re-used variables to zero
515 3144 : InitialiseVariables();
516 :
517 : // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
518 3144 : Vector3F initAccVec;
519 :
520 : // TODO we should average accel readings over several cycles
521 3144 : initAccVec = dal.ins().get_accel(accel_index_active).toftype();
522 :
523 : // normalise the acceleration vector
524 : ftype pitch=0, roll=0;
525 3144 : if (initAccVec.length() > 0.001f) {
526 3144 : initAccVec.normalize();
527 :
528 : // calculate initial pitch angle
529 3144 : pitch = asinF(initAccVec.x);
530 :
531 : // calculate initial roll angle
532 3144 : roll = atan2F(-initAccVec.y , -initAccVec.z);
533 : }
534 :
535 : // calculate initial roll and pitch orientation
536 3144 : stateStruct.quat.from_euler(roll, pitch, 0.0f);
537 :
538 : // initialise dynamic states
539 3144 : stateStruct.velocity.zero();
540 3144 : stateStruct.position.zero();
541 :
542 : // initialise static process model states
543 3144 : stateStruct.gyro_bias.zero();
544 3144 : stateStruct.accel_bias.zero();
545 3144 : stateStruct.wind_vel.zero();
546 3144 : stateStruct.earth_magfield.zero();
547 3144 : stateStruct.body_magfield.zero();
548 :
549 : // set the position, velocity and height
550 3144 : ResetVelocity(resetDataSource::DEFAULT);
551 3144 : ResetPosition(resetDataSource::DEFAULT);
552 3144 : ResetHeight();
553 :
554 : // initialise sources
555 3144 : posxy_source_last = frontend->sources.getPosXYSource(core_index);
556 3144 : yaw_source_last = frontend->sources.getYawSource(core_index);
557 :
558 : // define Earth rotation vector in the NED navigation frame
559 3144 : calcEarthRateNED(earthRateNED, dal.get_home().lat);
560 :
561 : // initialise the covariance matrix
562 3144 : CovarianceInit();
563 :
564 : // reset the output predictor states
565 3144 : StoreOutputReset();
566 :
567 : // set to true now that states have be initialised
568 3144 : statesInitialised = true;
569 :
570 : // reset inactive biases
571 12576 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
572 9432 : inactiveBias[i].gyro_bias.zero();
573 9432 : inactiveBias[i].accel_bias.zero();
574 : }
575 :
576 : // restore the navigation origin from the public origin if possible:
577 3144 : if (public_origin.initialised()) {
578 9 : setOriginLLH(public_origin);
579 : }
580 :
581 3144 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initialised",(unsigned)imu_index);
582 :
583 : // we initially return false to wait for the IMU buffer to fill
584 3144 : return false;
585 : }
586 :
587 : // initialise the covariance matrix
588 3144 : void NavEKF3_core::CovarianceInit()
589 : {
590 : // zero the matrix
591 3144 : memset(&P[0][0], 0, sizeof(P));
592 :
593 : // define the initial angle uncertainty as variances for a rotation vector
594 : Vector3F rot_vec_var;
595 3144 : rot_vec_var.x = rot_vec_var.y = rot_vec_var.z = sq(0.1f);
596 :
597 : // reset the quaternion state covariances
598 3144 : CovariancePrediction(&rot_vec_var);
599 :
600 : // velocities
601 3144 : P[4][4] = sq(frontend->_gpsHorizVelNoise);
602 3144 : P[5][5] = P[4][4];
603 3144 : P[6][6] = sq(frontend->_gpsVertVelNoise);
604 : // positions
605 3144 : P[7][7] = sq(frontend->_gpsHorizPosNoise);
606 3144 : P[8][8] = P[7][7];
607 3144 : P[9][9] = sq(frontend->_baroAltNoise);
608 : // gyro delta angle biases
609 3144 : P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
610 3144 : P[11][11] = P[10][10];
611 3144 : P[12][12] = P[10][10];
612 : // delta velocity biases
613 3144 : P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
614 3144 : P[14][14] = P[13][13];
615 3144 : P[15][15] = P[13][13];
616 : // earth magnetic field
617 3144 : P[16][16] = sq(frontend->_magNoise);
618 3144 : P[17][17] = P[16][16];
619 3144 : P[18][18] = P[16][16];
620 : // body magnetic field
621 3144 : P[19][19] = sq(frontend->_magNoise);
622 3144 : P[20][20] = P[19][19];
623 3144 : P[21][21] = P[19][19];
624 : // wind velocities
625 3144 : P[22][22] = 0.0f;
626 3144 : P[23][23] = P[22][22];
627 :
628 :
629 : #if EK3_FEATURE_OPTFLOW_FUSION
630 : // optical flow ground height covariance
631 3144 : Popt = 0.25f;
632 : #endif
633 :
634 3144 : }
635 :
636 : /********************************************************
637 : * UPDATE FUNCTIONS *
638 : ********************************************************/
639 : // Update Filter States - this should be called whenever new IMU data is available
640 78317950 : void NavEKF3_core::UpdateFilter(bool predict)
641 : {
642 : // don't run filter updates if states have not been initialised
643 78317950 : if (!statesInitialised) {
644 : return;
645 : }
646 :
647 78317950 : fill_scratch_variables();
648 :
649 : // update sensor selection (for affinity)
650 78317950 : update_sensor_selection();
651 :
652 : // TODO - in-flight restart method
653 :
654 : // Check arm status and perform required checks and mode changes
655 78317950 : controlFilterModes();
656 :
657 : // read IMU data as delta angles and velocities
658 78317950 : readIMUData(predict);
659 :
660 : // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
661 78317950 : if (runUpdates) {
662 : // Predict states using IMU data from the delayed time horizon
663 19965274 : UpdateStrapdownEquationsNED();
664 :
665 : // Predict the covariance growth
666 19965274 : CovariancePrediction(nullptr);
667 :
668 : // Run the IMU prediction step for the GSF yaw estimator algorithm
669 : // using IMU and optionally true airspeed data.
670 : // Must be run before SelectMagFusion() to provide an up to date yaw estimate
671 19965274 : runYawEstimatorPrediction();
672 :
673 : // Update states using magnetometer or external yaw sensor data
674 19965274 : SelectMagFusion();
675 :
676 : // Update states using GPS and altimeter data
677 19965274 : SelectVelPosFusion();
678 :
679 : // Run the GPS velocity correction step for the GSF yaw estimator algorithm
680 : // and use the yaw estimate to reset the main EKF yaw if requested
681 : // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
682 19965274 : runYawEstimatorCorrection();
683 :
684 : #if EK3_FEATURE_BEACON_FUSION
685 : // Update states using range beacon data
686 19965274 : SelectRngBcnFusion();
687 : #endif
688 :
689 : #if EK3_FEATURE_OPTFLOW_FUSION
690 : // Update states using optical flow data
691 19965274 : SelectFlowFusion();
692 : #endif
693 :
694 : #if EK3_FEATURE_BODY_ODOM
695 : // Update states using body frame odometry data
696 19965274 : SelectBodyOdomFusion();
697 : #endif
698 :
699 : // Update states using airspeed data
700 19965274 : SelectTasFusion();
701 :
702 : // Update states using sideslip constraint assumption for fly-forward vehicles or body drag for multicopters
703 19965274 : SelectBetaDragFusion();
704 :
705 : // Update the filter status
706 19965274 : updateFilterStatus();
707 :
708 19965274 : if (imuSampleTime_ms - last_oneHz_ms >= 1000) {
709 : // 1Hz tasks
710 280584 : last_oneHz_ms = imuSampleTime_ms;
711 280584 : moveEKFOrigin();
712 280584 : checkUpdateEarthField();
713 : }
714 : }
715 :
716 : // Wind output forward from the fusion to output time horizon
717 78317950 : calcOutputStates();
718 :
719 : /*
720 : this is a check to cope with a vehicle sitting idle on the
721 : ground and getting over-confident of the state. The symptoms
722 : would be "gyros still settling" when the user tries to arm. In
723 : that state the EKF can't recover, so we do a hard reset and let
724 : it try again.
725 : */
726 78317950 : if (filterStatus.value != 0) {
727 77470025 : last_filter_ok_ms = dal.millis();
728 : }
729 847925 : if (filterStatus.value == 0 &&
730 847925 : last_filter_ok_ms != 0 &&
731 78319427 : dal.millis() - last_filter_ok_ms > 5000 &&
732 1 : !dal.get_armed()) {
733 : // we've been unhealthy for 5 seconds after being healthy, reset the filter
734 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u forced reset",(unsigned)imu_index);
735 1 : last_filter_ok_ms = 0;
736 1 : statesInitialised = false;
737 1 : InitialiseFilterBootstrap();
738 : }
739 : }
740 :
741 98396519 : void NavEKF3_core::correctDeltaAngle(Vector3F &delAng, ftype delAngDT, uint8_t gyro_index)
742 : {
743 98396519 : delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg);
744 98396519 : }
745 :
746 98396519 : void NavEKF3_core::correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index)
747 : {
748 98396519 : delVel -= inactiveBias[accel_index].accel_bias * (delVelDT / dtEkfAvg);
749 98396519 : }
750 :
751 : /*
752 : * Update the quaternion, velocity and position states using delayed IMU measurements
753 : * because the EKF is running on a delayed time horizon. Note that the quaternion is
754 : * not used by the EKF equations, which instead estimate the error in the attitude of
755 : * the vehicle when each observation is fused. This attitude error is then used to correct
756 : * the quaternion.
757 : */
758 19965274 : void NavEKF3_core::UpdateStrapdownEquationsNED()
759 : {
760 : // update the quaternion states by rotating from the previous attitude through
761 : // the delta angle rotation quaternion and normalise
762 : // apply correction for earth's rotation rate
763 : // % * - and + operators have been overloaded
764 19965274 : stateStruct.quat.rotate(delAngCorrected - prevTnb * earthRateNED*imuDataDelayed.delAngDT);
765 :
766 19965274 : stateStruct.quat.normalize();
767 :
768 : // transform body delta velocities to delta velocities in the nav frame
769 : // use the nav frame from previous time step as the delta velocities
770 : // have been rotated into that frame
771 : // * and + operators have been overloaded
772 19965274 : Vector3F delVelNav; // delta velocity vector in earth axes
773 19965274 : delVelNav = prevTnb.mul_transpose(delVelCorrected);
774 19965274 : delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;
775 :
776 : // calculate the nav to body cosine matrix
777 19965274 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
778 :
779 : // calculate the rate of change of velocity (used for launch detect and other functions)
780 19965274 : velDotNED = delVelNav / imuDataDelayed.delVelDT;
781 :
782 : // apply a first order lowpass filter
783 19965274 : velDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;
784 :
785 : // calculate a magnitude of the filtered nav acceleration (required for GPS
786 : // variance estimation)
787 19965274 : accNavMag = velDotNEDfilt.length();
788 19965274 : accNavMagHoriz = velDotNEDfilt.xy().length();
789 :
790 : // if we are not aiding, then limit the horizontal magnitude of acceleration
791 : // to prevent large manoeuvre transients disturbing the attitude
792 19965274 : if ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {
793 2063 : ftype gain = 5.0f/accNavMagHoriz;
794 2063 : delVelNav.x *= gain;
795 2063 : delVelNav.y *= gain;
796 : }
797 :
798 : // save velocity for use in trapezoidal integration for position calcuation
799 19965274 : Vector3F lastVelocity = stateStruct.velocity;
800 :
801 : // sum delta velocities to get velocity
802 19965274 : stateStruct.velocity += delVelNav;
803 :
804 : // apply a trapezoidal integration to velocities to calculate position
805 19965274 : stateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
806 :
807 : // accumulate the bias delta angle and time since last reset by an OF measurement arrival
808 19965274 : delAngBodyOF += delAngCorrected;
809 19965274 : delTimeOF += imuDataDelayed.delAngDT;
810 :
811 : // limit states to protect against divergence
812 19965274 : ConstrainStates();
813 :
814 : #if EK3_FEATURE_BEACON_FUSION
815 : // If main filter velocity states are valid, update the range beacon receiver position states
816 19965274 : if (filterStatus.flags.horiz_vel) {
817 19624288 : rngBcn.receiverPos += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);
818 : }
819 : #endif
820 19965274 : }
821 :
822 : /*
823 : * Propagate PVA solution forward from the fusion time horizon to the current time horizon
824 : * using simple observer which performs two functions:
825 : * 1) Corrects for the delayed time horizon used by the EKF.
826 : * 2) Applies a LPF to state corrections to prevent 'stepping' in states due to measurement
827 : * fusion introducing unwanted noise into the control loops.
828 : * The inspiration for using a complementary filter to correct for time delays in the EKF
829 : * is based on the work by A Khosravian.
830 : *
831 : * "Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements"
832 : * A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
833 : */
834 78317950 : void NavEKF3_core::calcOutputStates()
835 : {
836 : // apply corrections to the IMU data
837 78317950 : Vector3F delAngNewCorrected = imuDataNew.delAng;
838 78317950 : Vector3F delVelNewCorrected = imuDataNew.delVel;
839 78317950 : correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index);
840 78317950 : correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index);
841 :
842 : // apply corrections to track EKF solution
843 78317950 : Vector3F delAng = delAngNewCorrected + delAngCorrection;
844 :
845 : // convert the rotation vector to its equivalent quaternion
846 78317950 : QuaternionF deltaQuat;
847 78317950 : deltaQuat.from_axis_angle(delAng);
848 :
849 : // update the quaternion states by rotating from the previous attitude through
850 : // the delta angle rotation quaternion and normalise
851 78317950 : outputDataNew.quat *= deltaQuat;
852 78317950 : outputDataNew.quat.normalize();
853 :
854 : // calculate the body to nav cosine matrix
855 78317950 : Matrix3F Tbn_temp;
856 78317950 : outputDataNew.quat.rotation_matrix(Tbn_temp);
857 :
858 : // transform body delta velocities to delta velocities in the nav frame
859 78317950 : Vector3F delVelNav = Tbn_temp*delVelNewCorrected;
860 78317950 : delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
861 :
862 : // save velocity for use in trapezoidal integration for position calcuation
863 78317950 : Vector3F lastVelocity = outputDataNew.velocity;
864 :
865 : // sum delta velocities to get velocity
866 78317950 : outputDataNew.velocity += delVelNav;
867 :
868 : // Implement third order complementary filter for height and height rate
869 : // Reference Paper :
870 : // Optimizing the Gains of the Baro-Inertial Vertical Channel
871 : // Widnall W.S, Sinha P.K,
872 : // AIAA Journal of Guidance and Control, 78-1307R
873 :
874 : // Perform filter calculation using backwards Euler integration
875 : // Coefficients selected to place all three filter poles at omega
876 78317950 : const ftype CompFiltOmega = M_2PI * constrain_ftype(frontend->_hrt_filt_freq, 0.1f, 30.0f);
877 78317950 : ftype omega2 = CompFiltOmega * CompFiltOmega;
878 78317950 : ftype pos_err = constrain_ftype(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5);
879 78317950 : ftype integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;
880 78317950 : vertCompFiltState.acc += integ1_input;
881 78317950 : ftype integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT;
882 78317950 : vertCompFiltState.vel += integ2_input;
883 78317950 : ftype integ3_input = (vertCompFiltState.vel + pos_err * CompFiltOmega * 3.0f) * imuDataNew.delVelDT;
884 78317950 : vertCompFiltState.pos += integ3_input;
885 :
886 : // apply a trapezoidal integration to velocities to calculate position
887 78317950 : outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
888 :
889 : // If the IMU accelerometer is offset from the body frame origin, then calculate corrections
890 : // that can be added to the EKF velocity and position outputs so that they represent the velocity
891 : // and position of the body frame origin.
892 : // Note the * operator has been overloaded to operate as a dot product
893 78317950 : if (!accelPosOffset.is_zero()) {
894 : // calculate the average angular rate across the last IMU update
895 : // note delAngDT is prevented from being zero in readIMUData()
896 127686 : Vector3F angRate = dal.ins().get_gyro(gyro_index_active).toftype();
897 :
898 : // Calculate the velocity of the body frame origin relative to the IMU in body frame
899 : // and rotate into earth frame. Note % operator has been overloaded to perform a cross product
900 127686 : Vector3F velBodyRelIMU = angRate % (- accelPosOffset);
901 127686 : velOffsetNED = Tbn_temp * velBodyRelIMU;
902 :
903 : // calculate the earth frame position of the body frame origin relative to the IMU
904 127686 : posOffsetNED = Tbn_temp * (- accelPosOffset);
905 : } else {
906 78190264 : velOffsetNED.zero();
907 78190264 : posOffsetNED.zero();
908 : }
909 :
910 : // Detect fixed wing launch acceleration using latest data from IMU to enable early startup of filter functions
911 : // that use launch acceleration to detect start of flight
912 78317950 : if (!inFlight && !dal.get_takeoff_expected() && assume_zero_sideslip()) {
913 2082027 : const ftype launchDelVel = imuDataNew.delVel.x + GRAVITY_MSS * imuDataNew.delVelDT * Tbn_temp.c.x;
914 2082027 : if (launchDelVel > GRAVITY_MSS * imuDataNew.delVelDT) {
915 19 : dal.set_takeoff_expected();
916 : }
917 : }
918 :
919 : // store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
920 78317950 : if (runUpdates) {
921 : // store the states at the output time horizon
922 19965274 : storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
923 :
924 : // recall the states from the fusion time horizon
925 19965274 : outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
926 :
927 : // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
928 :
929 : // divide the demanded quaternion by the estimated to get the error
930 19965274 : QuaternionF quatErr = stateStruct.quat / outputDataDelayed.quat;
931 :
932 : // Convert to a delta rotation using a small angle approximation
933 19965274 : quatErr.normalize();
934 19965274 : Vector3F deltaAngErr;
935 : ftype scaler;
936 19965274 : if (quatErr[0] >= 0.0f) {
937 : scaler = 2.0f;
938 : } else {
939 : scaler = -2.0f;
940 : }
941 19965274 : deltaAngErr.x = scaler * quatErr[1];
942 19965274 : deltaAngErr.y = scaler * quatErr[2];
943 19965274 : deltaAngErr.z = scaler * quatErr[3];
944 :
945 : // calculate a gain that provides tight tracking of the estimator states and
946 : // adjust for changes in time delay to maintain consistent damping ratio of ~0.7
947 19965274 : ftype timeDelay = 1e-3f * (ftype)(imuDataNew.time_ms - imuDataDelayed.time_ms);
948 19965274 : timeDelay = MAX(timeDelay, dtIMUavg);
949 19965274 : ftype errorGain = 0.5f / timeDelay;
950 :
951 : // calculate a correction to the delta angle
952 : // that will cause the INS to track the EKF quaternions
953 19965274 : delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
954 :
955 : // calculate velocity and position tracking errors
956 19965274 : Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity);
957 19965274 : Vector3F posErr = (stateStruct.position - outputDataDelayed.position);
958 :
959 19965274 : if (badIMUdata) {
960 : // When IMU accel is bad, calculate an integral that will be used to drive the difference
961 : // between the output state and internal EKF state at the delayed time horizon to zero.
962 17546 : badImuVelErrIntegral += (stateStruct.velocity.z - outputDataNew.velocity.z);
963 : } else {
964 19947728 : badImuVelErrIntegral = velErrintegral.z;
965 : }
966 :
967 : // collect magnitude tracking error for diagnostics
968 19965274 : outputTrackError.x = deltaAngErr.length();
969 19965274 : outputTrackError.y = velErr.length();
970 19965274 : outputTrackError.z = posErr.length();
971 :
972 : // convert user specified time constant from centi-seconds to seconds
973 19965274 : ftype tauPosVel = constrain_ftype(0.01f*(ftype)frontend->_tauVelPosOutput, 0.1f, 0.5f);
974 :
975 : // calculate a gain to track the EKF position states with the specified time constant
976 19965274 : ftype velPosGain = dtEkfAvg / constrain_ftype(tauPosVel, dtEkfAvg, 10.0f);
977 :
978 : // use a PI feedback to calculate a correction that will be applied to the output state history
979 19965274 : posErrintegral += posErr;
980 19965274 : velErrintegral += velErr;
981 19965274 : Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1F;
982 19965274 : Vector3F velCorrection;
983 19965274 : velCorrection.x = velErr.x * velPosGain + velErrintegral.x * sq(velPosGain) * 0.1F;
984 19965274 : velCorrection.y = velErr.y * velPosGain + velErrintegral.y * sq(velPosGain) * 0.1F;
985 19965274 : if (badIMUdata) {
986 17546 : velCorrection.z = velErr.z * velPosGain + badImuVelErrIntegral * sq(velPosGain) * 0.07F;
987 17546 : velErrintegral.z = badImuVelErrIntegral;
988 : } else {
989 19947728 : velCorrection.z = velErr.z * velPosGain + velErrintegral.z * sq(velPosGain) * 0.1F;
990 : }
991 :
992 : // loop through the output filter state history and apply the corrections to the velocity and position states
993 : // this method is too expensive to use for the attitude states due to the quaternion operations required
994 : // but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
995 : // to be used
996 19965274 : output_elements outputStates;
997 200243343 : for (unsigned index=0; index < imu_buffer_length; index++) {
998 180278069 : outputStates = storedOutput[index];
999 :
1000 : // a constant velocity correction is applied
1001 180278069 : outputStates.velocity += velCorrection;
1002 :
1003 : // a constant position correction is applied
1004 180278069 : outputStates.position += posCorrection;
1005 :
1006 : // push the updated data to the buffer
1007 180278069 : storedOutput[index] = outputStates;
1008 : }
1009 :
1010 : // update output state to corrected values
1011 19965274 : outputDataNew = storedOutput[storedIMU.get_youngest_index()];
1012 :
1013 : }
1014 78317950 : }
1015 :
1016 : /*
1017 : * Calculate the predicted state covariance matrix using algebraic equations generated using SymPy
1018 : * See derivation/generate_1.py for derivation
1019 : * Output for change reference: derivation/generated/covariance_generated.cpp
1020 : * Argument rotVarVecPtr is pointer to a vector defining the earth frame uncertainty variance of the quaternion states
1021 : * used to perform a reset of the quaternion state covariances only. Set to null for normal operation.
1022 : */
1023 19972655 : void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr)
1024 : {
1025 : ftype daxVar; // X axis delta angle noise variance rad^2
1026 : ftype dayVar; // Y axis delta angle noise variance rad^2
1027 : ftype dazVar; // Z axis delta angle noise variance rad^2
1028 : ftype dvxVar; // X axis delta velocity variance noise (m/s)^2
1029 : ftype dvyVar; // Y axis delta velocity variance noise (m/s)^2
1030 : ftype dvzVar; // Z axis delta velocity variance noise (m/s)^2
1031 : ftype dvx; // X axis delta velocity (m/s)
1032 : ftype dvy; // Y axis delta velocity (m/s)
1033 : ftype dvz; // Z axis delta velocity (m/s)
1034 : ftype dax; // X axis delta angle (rad)
1035 : ftype day; // Y axis delta angle (rad)
1036 : ftype daz; // Z axis delta angle (rad)
1037 : ftype q0; // attitude quaternion
1038 : ftype q1; // attitude quaternion
1039 : ftype q2; // attitude quaternion
1040 : ftype q3; // attitude quaternion
1041 : ftype dax_b; // X axis delta angle measurement bias (rad)
1042 : ftype day_b; // Y axis delta angle measurement bias (rad)
1043 : ftype daz_b; // Z axis delta angle measurement bias (rad)
1044 : ftype dvx_b; // X axis delta velocity measurement bias (rad)
1045 : ftype dvy_b; // Y axis delta velocity measurement bias (rad)
1046 : ftype dvz_b; // Z axis delta velocity measurement bias (rad)
1047 :
1048 : // Calculate the time step used by the covariance prediction as an average of the gyro and accel integration period
1049 : // Constrain to prevent bad timing jitter causing numerical conditioning problems with the covariance prediction
1050 19972655 : dt = constrain_ftype(0.5f*(imuDataDelayed.delAngDT+imuDataDelayed.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
1051 :
1052 : // use filtered height rate to increase wind process noise when climbing or descending
1053 : // this allows for wind gradient effects.Filter height rate using a 10 second time constant filter
1054 19972655 : ftype alpha = 0.1f * dt;
1055 19972655 : hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;
1056 :
1057 : // calculate covariance prediction process noise added to diagonals of predicted covariance matrix
1058 : // error growth of first 10 kinematic states is built into auto-code for covariance prediction and driven by IMU noise parameters
1059 19972655 : Vector14 processNoiseVariance = {};
1060 :
1061 19972655 : if (!inhibitDelAngBiasStates) {
1062 19661807 : ftype dAngBiasVar = sq(sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0, 1.0));
1063 78647228 : for (uint8_t i=0; i<=2; i++) processNoiseVariance[i] = dAngBiasVar;
1064 : }
1065 :
1066 19972655 : if (!inhibitDelVelBiasStates) {
1067 : // default process noise (m/s)^2
1068 19661807 : ftype dVelBiasVar = sq(sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0, 1.0));
1069 78647228 : for (uint8_t i=3; i<=5; i++) {
1070 58985421 : processNoiseVariance[i] = dVelBiasVar;
1071 : }
1072 : }
1073 :
1074 19972655 : if (!inhibitMagStates && lastInhibitMagStates) {
1075 : // when starting 3D fusion we want to reset mag variances
1076 1363 : needMagBodyVarReset = true;
1077 1363 : needEarthBodyVarReset = true;
1078 : }
1079 :
1080 19972655 : if (needMagBodyVarReset) {
1081 : // reset body mag variances
1082 1451 : needMagBodyVarReset = false;
1083 1451 : zeroStatesVarCov(19, 21);
1084 1451 : P[19][19] = sq(frontend->_magNoise);
1085 1451 : P[20][20] = P[19][19];
1086 1451 : P[21][21] = P[19][19];
1087 : }
1088 :
1089 19972655 : if (needEarthBodyVarReset) {
1090 : // reset mag earth field variances
1091 1363 : needEarthBodyVarReset = false;
1092 1363 : zeroStatesVarCov(16, 18);
1093 1363 : P[16][16] = sq(frontend->_magNoise);
1094 1363 : P[17][17] = P[16][16];
1095 1363 : P[18][18] = P[16][16];
1096 : // Fusing the declinaton angle as an observaton with a 20 deg uncertainty helps
1097 : // to stabilise the earth field.
1098 1363 : FuseDeclination(radians(20.0f));
1099 : }
1100 :
1101 19972655 : if (!inhibitMagStates) {
1102 12067284 : ftype magEarthVar = sq(dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f));
1103 12067284 : ftype magBodyVar = sq(dt * constrain_ftype(frontend->_magBodyProcessNoise, 0.0f, 1.0f));
1104 48269136 : for (uint8_t i=6; i<=8; i++) processNoiseVariance[i] = magEarthVar;
1105 48269136 : for (uint8_t i=9; i<=11; i++) processNoiseVariance[i] = magBodyVar;
1106 : }
1107 19972655 : lastInhibitMagStates = inhibitMagStates;
1108 :
1109 19972655 : if (!inhibitWindStates) {
1110 3552707 : const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout;
1111 3539606 : const bool newTreatWindStatesAsTruth = isDragFusionDeadReckoning || !windStateIsObservable;
1112 : if (newTreatWindStatesAsTruth) {
1113 37289 : treatWindStatesAsTruth = true;
1114 37289 : zeroStatesVarCov(22, 23);
1115 : } else {
1116 3515418 : if (treatWindStatesAsTruth) {
1117 19 : treatWindStatesAsTruth = false;
1118 : if (windStateIsObservable) {
1119 : // allow EKF to relearn wind states rapidly
1120 19 : P[23][23] = P[22][22] = sq(WIND_VEL_VARIANCE_MAX);
1121 : }
1122 : }
1123 3515418 : ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate)));
1124 3515418 : if (!tasDataDelayed.allowFusion) {
1125 : // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor
1126 119803 : windVelVar *= 10.0f;
1127 : }
1128 10546254 : for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar;
1129 : }
1130 : }
1131 :
1132 : // set variables used to calculate covariance growth
1133 19972655 : dvx = imuDataDelayed.delVel.x;
1134 19972655 : dvy = imuDataDelayed.delVel.y;
1135 19972655 : dvz = imuDataDelayed.delVel.z;
1136 19972655 : dax = imuDataDelayed.delAng.x;
1137 19972655 : day = imuDataDelayed.delAng.y;
1138 19972655 : daz = imuDataDelayed.delAng.z;
1139 19972655 : q0 = stateStruct.quat[0];
1140 19972655 : q1 = stateStruct.quat[1];
1141 19972655 : q2 = stateStruct.quat[2];
1142 19972655 : q3 = stateStruct.quat[3];
1143 19972655 : dax_b = stateStruct.gyro_bias.x;
1144 19972655 : day_b = stateStruct.gyro_bias.y;
1145 19972655 : daz_b = stateStruct.gyro_bias.z;
1146 19972655 : dvx_b = stateStruct.accel_bias.x;
1147 19972655 : dvy_b = stateStruct.accel_bias.y;
1148 19972655 : dvz_b = stateStruct.accel_bias.z;
1149 :
1150 : bool quatCovResetOnly = false;
1151 19972655 : if (rotVarVecPtr != nullptr) {
1152 : // Handle special case where we are initialising the quaternion covariances using an earth frame
1153 : // vector defining the variance of the angular alignment uncertainty. Convert he varaince vector
1154 : // to a matrix and rotate into body frame. Use the exisiting gyro error propagation mechanism to
1155 : // propagate the body frame angular uncertainty variances.
1156 : const Vector3F &rotVarVec = *rotVarVecPtr;
1157 : Matrix3F R_ef = Matrix3F (
1158 7381 : rotVarVec.x, 0.0f, 0.0f,
1159 7381 : 0.0f, rotVarVec.y, 0.0f,
1160 7381 : 0.0f, 0.0f, rotVarVec.z);
1161 7381 : Matrix3F Tnb;
1162 7381 : stateStruct.quat.inverse().rotation_matrix(Tnb);
1163 7381 : Matrix3F R_bf = Tnb * R_ef * Tnb.transposed();
1164 7381 : daxVar = R_bf.a.x;
1165 7381 : dayVar = R_bf.b.y;
1166 7381 : dazVar = R_bf.c.z;
1167 : quatCovResetOnly = true;
1168 7381 : zeroStatesVarCov(0, 3);
1169 : } else {
1170 19965274 : ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);
1171 19965274 : daxVar = dayVar = dazVar = sq(dt*_gyrNoise);
1172 : }
1173 19972655 : ftype _accNoise = badIMUdata ? BAD_IMU_DATA_ACC_P_NSE : constrain_ftype(frontend->_accNoise, 0.0f, BAD_IMU_DATA_ACC_P_NSE);
1174 19972655 : dvxVar = dvyVar = dvzVar = sq(dt*_accNoise);
1175 :
1176 19972655 : if (!inhibitDelVelBiasStates) {
1177 78647228 : for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
1178 58985421 : const uint8_t index = stateIndex - 13;
1179 :
1180 : // Don't attempt learning of IMU delta velocity bias if on ground.
1181 : // In flight: all axes are observable from velocity/position aiding.
1182 : // On ground and stationary: only the gravity-aligned axis (Z for a level
1183 : // vehicle) is observable. XY biases remain unobservable until the vehicle
1184 : // accelerates horizontally in flight.
1185 : // On ground and moving (e.g. carried or on a boat): inhibit all axes
1186 : // to prevent learning biases from external motion accelerations.
1187 58985421 : const bool is_bias_observable = (fabsF(prevTnb[index][2]) > 0.8f && onGroundNotMoving) || !onGround;
1188 :
1189 12081819 : if (!is_bias_observable && !dvelBiasAxisInhibit[index]) {
1190 : // store variances to be reinstated wben learning can commence later
1191 9995 : dvelBiasAxisVarPrev[index] = P[stateIndex][stateIndex];
1192 9995 : dvelBiasAxisInhibit[index] = true;
1193 58975426 : } else if (is_bias_observable && dvelBiasAxisInhibit[index]) {
1194 4468 : P[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index];
1195 4468 : dvelBiasAxisInhibit[index] = false;
1196 : }
1197 : }
1198 : }
1199 :
1200 : // nextP is a temporary only used in this function, and KHP is a temporary
1201 : // the same size only used outside of it. save memory by using KHP as nextP.
1202 : auto& nextP = KHP;
1203 :
1204 : // calculate the predicted covariance due to inertial sensor error propagation
1205 : // we calculate the lower diagonal and copy to take advantage of symmetry
1206 :
1207 : // intermediate calculations
1208 19972655 : const ftype PS0 = sq(q1);
1209 19972655 : const ftype PS1 = 0.25F*daxVar;
1210 19972655 : const ftype PS2 = sq(q2);
1211 19972655 : const ftype PS3 = 0.25F*dayVar;
1212 19972655 : const ftype PS4 = sq(q3);
1213 19972655 : const ftype PS5 = 0.25F*dazVar;
1214 19972655 : const ftype PS6 = 0.5F*q1;
1215 19972655 : const ftype PS7 = 0.5F*q2;
1216 19972655 : const ftype PS8 = PS7*P[10][11];
1217 19972655 : const ftype PS9 = 0.5F*q3;
1218 19972655 : const ftype PS10 = PS9*P[10][12];
1219 19972655 : const ftype PS11 = 0.5F*dax - 0.5F*dax_b;
1220 19972655 : const ftype PS12 = 0.5F*day - 0.5F*day_b;
1221 19972655 : const ftype PS13 = 0.5F*daz - 0.5F*daz_b;
1222 19972655 : const ftype PS14 = PS10 - PS11*P[1][10] - PS12*P[2][10] - PS13*P[3][10] + PS6*P[10][10] + PS8 + P[0][10];
1223 19972655 : const ftype PS15 = PS6*P[10][11];
1224 19972655 : const ftype PS16 = PS9*P[11][12];
1225 19972655 : const ftype PS17 = -PS11*P[1][11] - PS12*P[2][11] - PS13*P[3][11] + PS15 + PS16 + PS7*P[11][11] + P[0][11];
1226 19972655 : const ftype PS18 = PS6*P[10][12];
1227 19972655 : const ftype PS19 = PS7*P[11][12];
1228 19972655 : const ftype PS20 = -PS11*P[1][12] - PS12*P[2][12] - PS13*P[3][12] + PS18 + PS19 + PS9*P[12][12] + P[0][12];
1229 19972655 : const ftype PS21 = PS12*P[1][2];
1230 19972655 : const ftype PS22 = -PS13*P[1][3];
1231 19972655 : const ftype PS23 = -PS11*P[1][1] - PS21 + PS22 + PS6*P[1][10] + PS7*P[1][11] + PS9*P[1][12] + P[0][1];
1232 19972655 : const ftype PS24 = -PS11*P[1][2];
1233 19972655 : const ftype PS25 = PS13*P[2][3];
1234 19972655 : const ftype PS26 = -PS12*P[2][2] + PS24 - PS25 + PS6*P[2][10] + PS7*P[2][11] + PS9*P[2][12] + P[0][2];
1235 19972655 : const ftype PS27 = PS11*P[1][3];
1236 19972655 : const ftype PS28 = -PS12*P[2][3];
1237 19972655 : const ftype PS29 = -PS13*P[3][3] - PS27 + PS28 + PS6*P[3][10] + PS7*P[3][11] + PS9*P[3][12] + P[0][3];
1238 19972655 : const ftype PS30 = PS11*P[0][1];
1239 19972655 : const ftype PS31 = PS12*P[0][2];
1240 19972655 : const ftype PS32 = PS13*P[0][3];
1241 19972655 : const ftype PS33 = -PS30 - PS31 - PS32 + PS6*P[0][10] + PS7*P[0][11] + PS9*P[0][12] + P[0][0];
1242 19972655 : const ftype PS34 = 0.5F*q0;
1243 19972655 : const ftype PS35 = q2*q3;
1244 19972655 : const ftype PS36 = q0*q1;
1245 19972655 : const ftype PS37 = q1*q3;
1246 19972655 : const ftype PS38 = q0*q2;
1247 19972655 : const ftype PS39 = q1*q2;
1248 19972655 : const ftype PS40 = q0*q3;
1249 19972655 : const ftype PS41 = 2*PS2;
1250 19972655 : const ftype PS42 = 2*PS4 - 1;
1251 19972655 : const ftype PS43 = PS41 + PS42;
1252 19972655 : const ftype PS44 = -PS11*P[1][13] - PS12*P[2][13] - PS13*P[3][13] + PS6*P[10][13] + PS7*P[11][13] + PS9*P[12][13] + P[0][13];
1253 19972655 : const ftype PS45 = PS37 + PS38;
1254 19972655 : const ftype PS46 = -PS11*P[1][15] - PS12*P[2][15] - PS13*P[3][15] + PS6*P[10][15] + PS7*P[11][15] + PS9*P[12][15] + P[0][15];
1255 19972655 : const ftype PS47 = 2*PS46;
1256 19972655 : const ftype PS48 = dvy - dvy_b;
1257 19972655 : const ftype PS49 = PS48*q0;
1258 19972655 : const ftype PS50 = dvz - dvz_b;
1259 19972655 : const ftype PS51 = PS50*q1;
1260 19972655 : const ftype PS52 = dvx - dvx_b;
1261 19972655 : const ftype PS53 = PS52*q3;
1262 19972655 : const ftype PS54 = PS49 - PS51 + 2*PS53;
1263 19972655 : const ftype PS55 = 2*PS29;
1264 19972655 : const ftype PS56 = -PS39 + PS40;
1265 19972655 : const ftype PS57 = -PS11*P[1][14] - PS12*P[2][14] - PS13*P[3][14] + PS6*P[10][14] + PS7*P[11][14] + PS9*P[12][14] + P[0][14];
1266 19972655 : const ftype PS58 = 2*PS57;
1267 19972655 : const ftype PS59 = PS48*q2;
1268 19972655 : const ftype PS60 = PS50*q3;
1269 19972655 : const ftype PS61 = PS59 + PS60;
1270 19972655 : const ftype PS62 = 2*PS23;
1271 19972655 : const ftype PS63 = PS50*q2;
1272 19972655 : const ftype PS64 = PS48*q3;
1273 : const ftype PS65 = -PS64;
1274 : const ftype PS66 = PS63 + PS65;
1275 19972655 : const ftype PS67 = 2*PS33;
1276 19972655 : const ftype PS68 = PS50*q0;
1277 19972655 : const ftype PS69 = PS48*q1;
1278 19972655 : const ftype PS70 = PS52*q2;
1279 19972655 : const ftype PS71 = PS68 + PS69 - 2*PS70;
1280 19972655 : const ftype PS72 = 2*PS26;
1281 19972655 : const ftype PS73 = -PS11*P[1][4] - PS12*P[2][4] - PS13*P[3][4] + PS6*P[4][10] + PS7*P[4][11] + PS9*P[4][12] + P[0][4];
1282 19972655 : const ftype PS74 = 2*PS0;
1283 19972655 : const ftype PS75 = PS42 + PS74;
1284 19972655 : const ftype PS76 = PS39 + PS40;
1285 19972655 : const ftype PS77 = 2*PS44;
1286 19972655 : const ftype PS78 = PS51 - PS53;
1287 : const ftype PS79 = -PS70;
1288 19972655 : const ftype PS80 = PS68 + 2*PS69 + PS79;
1289 19972655 : const ftype PS81 = -PS35 + PS36;
1290 19972655 : const ftype PS82 = PS52*q1;
1291 19972655 : const ftype PS83 = PS60 + PS82;
1292 19972655 : const ftype PS84 = PS52*q0;
1293 19972655 : const ftype PS85 = PS63 - 2*PS64 + PS84;
1294 19972655 : const ftype PS86 = -PS11*P[1][5] - PS12*P[2][5] - PS13*P[3][5] + PS6*P[5][10] + PS7*P[5][11] + PS9*P[5][12] + P[0][5];
1295 19972655 : const ftype PS87 = PS41 + PS74 - 1;
1296 19972655 : const ftype PS88 = PS35 + PS36;
1297 19972655 : const ftype PS89 = 2*PS63 + PS65 + PS84;
1298 19972655 : const ftype PS90 = -PS37 + PS38;
1299 19972655 : const ftype PS91 = PS59 + PS82;
1300 : const ftype PS92 = PS69 + PS79;
1301 19972655 : const ftype PS93 = PS49 - 2*PS51 + PS53;
1302 19972655 : const ftype PS94 = -PS11*P[1][6] - PS12*P[2][6] - PS13*P[3][6] + PS6*P[6][10] + PS7*P[6][11] + PS9*P[6][12] + P[0][6];
1303 19972655 : const ftype PS95 = sq(q0);
1304 19972655 : const ftype PS96 = -PS34*P[10][11];
1305 19972655 : const ftype PS97 = PS11*P[0][11] - PS12*P[3][11] + PS13*P[2][11] - PS19 + PS9*P[11][11] + PS96 + P[1][11];
1306 19972655 : const ftype PS98 = PS13*P[0][2];
1307 19972655 : const ftype PS99 = PS12*P[0][3];
1308 19972655 : const ftype PS100 = PS11*P[0][0] - PS34*P[0][10] - PS7*P[0][12] + PS9*P[0][11] + PS98 - PS99 + P[0][1];
1309 19972655 : const ftype PS101 = PS11*P[0][2];
1310 19972655 : const ftype PS102 = PS101 + PS13*P[2][2] + PS28 - PS34*P[2][10] - PS7*P[2][12] + PS9*P[2][11] + P[1][2];
1311 19972655 : const ftype PS103 = PS9*P[10][11];
1312 19972655 : const ftype PS104 = PS7*P[10][12];
1313 19972655 : const ftype PS105 = PS103 - PS104 + PS11*P[0][10] - PS12*P[3][10] + PS13*P[2][10] - PS34*P[10][10] + P[1][10];
1314 19972655 : const ftype PS106 = -PS34*P[10][12];
1315 19972655 : const ftype PS107 = PS106 + PS11*P[0][12] - PS12*P[3][12] + PS13*P[2][12] + PS16 - PS7*P[12][12] + P[1][12];
1316 19972655 : const ftype PS108 = PS11*P[0][3];
1317 19972655 : const ftype PS109 = PS108 - PS12*P[3][3] + PS25 - PS34*P[3][10] - PS7*P[3][12] + PS9*P[3][11] + P[1][3];
1318 19972655 : const ftype PS110 = PS13*P[1][2];
1319 19972655 : const ftype PS111 = PS12*P[1][3];
1320 19972655 : const ftype PS112 = PS110 - PS111 + PS30 - PS34*P[1][10] - PS7*P[1][12] + PS9*P[1][11] + P[1][1];
1321 19972655 : const ftype PS113 = PS11*P[0][13] - PS12*P[3][13] + PS13*P[2][13] - PS34*P[10][13] - PS7*P[12][13] + PS9*P[11][13] + P[1][13];
1322 19972655 : const ftype PS114 = PS11*P[0][15] - PS12*P[3][15] + PS13*P[2][15] - PS34*P[10][15] - PS7*P[12][15] + PS9*P[11][15] + P[1][15];
1323 19972655 : const ftype PS115 = 2*PS114;
1324 19972655 : const ftype PS116 = 2*PS109;
1325 19972655 : const ftype PS117 = PS11*P[0][14] - PS12*P[3][14] + PS13*P[2][14] - PS34*P[10][14] - PS7*P[12][14] + PS9*P[11][14] + P[1][14];
1326 19972655 : const ftype PS118 = 2*PS117;
1327 19972655 : const ftype PS119 = 2*PS112;
1328 19972655 : const ftype PS120 = 2*PS100;
1329 19972655 : const ftype PS121 = 2*PS102;
1330 19972655 : const ftype PS122 = PS11*P[0][4] - PS12*P[3][4] + PS13*P[2][4] - PS34*P[4][10] - PS7*P[4][12] + PS9*P[4][11] + P[1][4];
1331 19972655 : const ftype PS123 = 2*PS113;
1332 19972655 : const ftype PS124 = PS11*P[0][5] - PS12*P[3][5] + PS13*P[2][5] - PS34*P[5][10] - PS7*P[5][12] + PS9*P[5][11] + P[1][5];
1333 19972655 : const ftype PS125 = PS11*P[0][6] - PS12*P[3][6] + PS13*P[2][6] - PS34*P[6][10] - PS7*P[6][12] + PS9*P[6][11] + P[1][6];
1334 19972655 : const ftype PS126 = -PS34*P[11][12];
1335 19972655 : const ftype PS127 = -PS10 + PS11*P[3][12] + PS12*P[0][12] + PS126 - PS13*P[1][12] + PS6*P[12][12] + P[2][12];
1336 19972655 : const ftype PS128 = PS11*P[3][3] + PS22 - PS34*P[3][11] + PS6*P[3][12] - PS9*P[3][10] + PS99 + P[2][3];
1337 19972655 : const ftype PS129 = PS13*P[0][1];
1338 19972655 : const ftype PS130 = PS108 + PS12*P[0][0] - PS129 - PS34*P[0][11] + PS6*P[0][12] - PS9*P[0][10] + P[0][2];
1339 19972655 : const ftype PS131 = PS6*P[11][12];
1340 19972655 : const ftype PS132 = -PS103 + PS11*P[3][11] + PS12*P[0][11] - PS13*P[1][11] + PS131 - PS34*P[11][11] + P[2][11];
1341 19972655 : const ftype PS133 = PS11*P[3][10] + PS12*P[0][10] - PS13*P[1][10] + PS18 - PS9*P[10][10] + PS96 + P[2][10];
1342 19972655 : const ftype PS134 = PS12*P[0][1];
1343 19972655 : const ftype PS135 = -PS13*P[1][1] + PS134 + PS27 - PS34*P[1][11] + PS6*P[1][12] - PS9*P[1][10] + P[1][2];
1344 19972655 : const ftype PS136 = PS11*P[2][3];
1345 19972655 : const ftype PS137 = -PS110 + PS136 + PS31 - PS34*P[2][11] + PS6*P[2][12] - PS9*P[2][10] + P[2][2];
1346 19972655 : const ftype PS138 = PS11*P[3][13] + PS12*P[0][13] - PS13*P[1][13] - PS34*P[11][13] + PS6*P[12][13] - PS9*P[10][13] + P[2][13];
1347 19972655 : const ftype PS139 = PS11*P[3][15] + PS12*P[0][15] - PS13*P[1][15] - PS34*P[11][15] + PS6*P[12][15] - PS9*P[10][15] + P[2][15];
1348 19972655 : const ftype PS140 = 2*PS139;
1349 19972655 : const ftype PS141 = 2*PS128;
1350 19972655 : const ftype PS142 = PS11*P[3][14] + PS12*P[0][14] - PS13*P[1][14] - PS34*P[11][14] + PS6*P[12][14] - PS9*P[10][14] + P[2][14];
1351 19972655 : const ftype PS143 = 2*PS142;
1352 19972655 : const ftype PS144 = 2*PS135;
1353 19972655 : const ftype PS145 = 2*PS130;
1354 19972655 : const ftype PS146 = 2*PS137;
1355 19972655 : const ftype PS147 = PS11*P[3][4] + PS12*P[0][4] - PS13*P[1][4] - PS34*P[4][11] + PS6*P[4][12] - PS9*P[4][10] + P[2][4];
1356 19972655 : const ftype PS148 = 2*PS138;
1357 19972655 : const ftype PS149 = PS11*P[3][5] + PS12*P[0][5] - PS13*P[1][5] - PS34*P[5][11] + PS6*P[5][12] - PS9*P[5][10] + P[2][5];
1358 19972655 : const ftype PS150 = PS11*P[3][6] + PS12*P[0][6] - PS13*P[1][6] - PS34*P[6][11] + PS6*P[6][12] - PS9*P[6][10] + P[2][6];
1359 19972655 : const ftype PS151 = PS106 - PS11*P[2][10] + PS12*P[1][10] + PS13*P[0][10] - PS15 + PS7*P[10][10] + P[3][10];
1360 19972655 : const ftype PS152 = PS12*P[1][1] + PS129 + PS24 - PS34*P[1][12] - PS6*P[1][11] + PS7*P[1][10] + P[1][3];
1361 19972655 : const ftype PS153 = -PS101 + PS13*P[0][0] + PS134 - PS34*P[0][12] - PS6*P[0][11] + PS7*P[0][10] + P[0][3];
1362 19972655 : const ftype PS154 = PS104 - PS11*P[2][12] + PS12*P[1][12] + PS13*P[0][12] - PS131 - PS34*P[12][12] + P[3][12];
1363 19972655 : const ftype PS155 = -PS11*P[2][11] + PS12*P[1][11] + PS126 + PS13*P[0][11] - PS6*P[11][11] + PS8 + P[3][11];
1364 19972655 : const ftype PS156 = -PS11*P[2][2] + PS21 - PS34*P[2][12] - PS6*P[2][11] + PS7*P[2][10] + PS98 + P[2][3];
1365 19972655 : const ftype PS157 = PS111 - PS136 + PS32 - PS34*P[3][12] - PS6*P[3][11] + PS7*P[3][10] + P[3][3];
1366 19972655 : const ftype PS158 = -PS11*P[2][13] + PS12*P[1][13] + PS13*P[0][13] - PS34*P[12][13] - PS6*P[11][13] + PS7*P[10][13] + P[3][13];
1367 19972655 : const ftype PS159 = -PS11*P[2][15] + PS12*P[1][15] + PS13*P[0][15] - PS34*P[12][15] - PS6*P[11][15] + PS7*P[10][15] + P[3][15];
1368 19972655 : const ftype PS160 = 2*PS159;
1369 19972655 : const ftype PS161 = 2*PS157;
1370 19972655 : const ftype PS162 = -PS11*P[2][14] + PS12*P[1][14] + PS13*P[0][14] - PS34*P[12][14] - PS6*P[11][14] + PS7*P[10][14] + P[3][14];
1371 19972655 : const ftype PS163 = 2*PS162;
1372 19972655 : const ftype PS164 = 2*PS152;
1373 19972655 : const ftype PS165 = 2*PS153;
1374 19972655 : const ftype PS166 = 2*PS156;
1375 19972655 : const ftype PS167 = -PS11*P[2][4] + PS12*P[1][4] + PS13*P[0][4] - PS34*P[4][12] - PS6*P[4][11] + PS7*P[4][10] + P[3][4];
1376 19972655 : const ftype PS168 = 2*PS158;
1377 19972655 : const ftype PS169 = -PS11*P[2][5] + PS12*P[1][5] + PS13*P[0][5] - PS34*P[5][12] - PS6*P[5][11] + PS7*P[5][10] + P[3][5];
1378 19972655 : const ftype PS170 = -PS11*P[2][6] + PS12*P[1][6] + PS13*P[0][6] - PS34*P[6][12] - PS6*P[6][11] + PS7*P[6][10] + P[3][6];
1379 19972655 : const ftype PS171 = 2*PS45;
1380 19972655 : const ftype PS172 = 2*PS56;
1381 19972655 : const ftype PS173 = 2*PS61;
1382 19972655 : const ftype PS174 = 2*PS66;
1383 19972655 : const ftype PS175 = 2*PS71;
1384 19972655 : const ftype PS176 = 2*PS54;
1385 19972655 : const ftype PS177 = -PS171*P[13][15] + PS172*P[13][14] + PS173*P[1][13] + PS174*P[0][13] + PS175*P[2][13] - PS176*P[3][13] + PS43*P[13][13] + P[4][13];
1386 19972655 : const ftype PS178 = -PS171*P[15][15] + PS172*P[14][15] + PS173*P[1][15] + PS174*P[0][15] + PS175*P[2][15] - PS176*P[3][15] + PS43*P[13][15] + P[4][15];
1387 19972655 : const ftype PS179 = -PS171*P[3][15] + PS172*P[3][14] + PS173*P[1][3] + PS174*P[0][3] + PS175*P[2][3] - PS176*P[3][3] + PS43*P[3][13] + P[3][4];
1388 19972655 : const ftype PS180 = -PS171*P[14][15] + PS172*P[14][14] + PS173*P[1][14] + PS174*P[0][14] + PS175*P[2][14] - PS176*P[3][14] + PS43*P[13][14] + P[4][14];
1389 19972655 : const ftype PS181 = -PS171*P[1][15] + PS172*P[1][14] + PS173*P[1][1] + PS174*P[0][1] + PS175*P[1][2] - PS176*P[1][3] + PS43*P[1][13] + P[1][4];
1390 19972655 : const ftype PS182 = -PS171*P[0][15] + PS172*P[0][14] + PS173*P[0][1] + PS174*P[0][0] + PS175*P[0][2] - PS176*P[0][3] + PS43*P[0][13] + P[0][4];
1391 19972655 : const ftype PS183 = -PS171*P[2][15] + PS172*P[2][14] + PS173*P[1][2] + PS174*P[0][2] + PS175*P[2][2] - PS176*P[2][3] + PS43*P[2][13] + P[2][4];
1392 19972655 : const ftype PS184 = 4*dvyVar;
1393 : const ftype PS185 = 4*dvzVar;
1394 19972655 : const ftype PS186 = -PS171*P[4][15] + PS172*P[4][14] + PS173*P[1][4] + PS174*P[0][4] + PS175*P[2][4] - PS176*P[3][4] + PS43*P[4][13] + P[4][4];
1395 19972655 : const ftype PS187 = 2*PS177;
1396 19972655 : const ftype PS188 = 2*PS182;
1397 19972655 : const ftype PS189 = 2*PS181;
1398 19972655 : const ftype PS190 = 2*PS81;
1399 19972655 : const ftype PS191 = 2*PS183;
1400 19972655 : const ftype PS192 = 2*PS179;
1401 19972655 : const ftype PS193 = 2*PS76;
1402 19972655 : const ftype PS194 = PS43*dvxVar;
1403 19972655 : const ftype PS195 = PS75*dvyVar;
1404 19972655 : const ftype PS196 = -PS171*P[5][15] + PS172*P[5][14] + PS173*P[1][5] + PS174*P[0][5] + PS175*P[2][5] - PS176*P[3][5] + PS43*P[5][13] + P[4][5];
1405 19972655 : const ftype PS197 = 2*PS88;
1406 19972655 : const ftype PS198 = PS87*dvzVar;
1407 19972655 : const ftype PS199 = 2*PS90;
1408 19972655 : const ftype PS200 = -PS171*P[6][15] + PS172*P[6][14] + PS173*P[1][6] + PS174*P[0][6] + PS175*P[2][6] - PS176*P[3][6] + PS43*P[6][13] + P[4][6];
1409 19972655 : const ftype PS201 = 2*PS83;
1410 19972655 : const ftype PS202 = 2*PS78;
1411 19972655 : const ftype PS203 = 2*PS85;
1412 19972655 : const ftype PS204 = 2*PS80;
1413 19972655 : const ftype PS205 = PS190*P[14][15] - PS193*P[13][14] + PS201*P[2][14] - PS202*P[0][14] + PS203*P[3][14] - PS204*P[1][14] + PS75*P[14][14] + P[5][14];
1414 19972655 : const ftype PS206 = PS190*P[13][15] - PS193*P[13][13] + PS201*P[2][13] - PS202*P[0][13] + PS203*P[3][13] - PS204*P[1][13] + PS75*P[13][14] + P[5][13];
1415 19972655 : const ftype PS207 = PS190*P[0][15] - PS193*P[0][13] + PS201*P[0][2] - PS202*P[0][0] + PS203*P[0][3] - PS204*P[0][1] + PS75*P[0][14] + P[0][5];
1416 19972655 : const ftype PS208 = PS190*P[1][15] - PS193*P[1][13] + PS201*P[1][2] - PS202*P[0][1] + PS203*P[1][3] - PS204*P[1][1] + PS75*P[1][14] + P[1][5];
1417 19972655 : const ftype PS209 = PS190*P[15][15] - PS193*P[13][15] + PS201*P[2][15] - PS202*P[0][15] + PS203*P[3][15] - PS204*P[1][15] + PS75*P[14][15] + P[5][15];
1418 19972655 : const ftype PS210 = PS190*P[2][15] - PS193*P[2][13] + PS201*P[2][2] - PS202*P[0][2] + PS203*P[2][3] - PS204*P[1][2] + PS75*P[2][14] + P[2][5];
1419 19972655 : const ftype PS211 = PS190*P[3][15] - PS193*P[3][13] + PS201*P[2][3] - PS202*P[0][3] + PS203*P[3][3] - PS204*P[1][3] + PS75*P[3][14] + P[3][5];
1420 : const ftype PS212 = 4*dvxVar;
1421 19972655 : const ftype PS213 = PS190*P[5][15] - PS193*P[5][13] + PS201*P[2][5] - PS202*P[0][5] + PS203*P[3][5] - PS204*P[1][5] + PS75*P[5][14] + P[5][5];
1422 19972655 : const ftype PS214 = 2*PS89;
1423 19972655 : const ftype PS215 = 2*PS91;
1424 19972655 : const ftype PS216 = 2*PS92;
1425 19972655 : const ftype PS217 = 2*PS93;
1426 19972655 : const ftype PS218 = PS190*P[6][15] - PS193*P[6][13] + PS201*P[2][6] - PS202*P[0][6] + PS203*P[3][6] - PS204*P[1][6] + PS75*P[6][14] + P[5][6];
1427 19972655 : const ftype PS219 = -PS197*P[14][15] + PS199*P[13][15] - PS214*P[2][15] + PS215*P[3][15] + PS216*P[0][15] + PS217*P[1][15] + PS87*P[15][15] + P[6][15];
1428 19972655 : const ftype PS220 = -PS197*P[14][14] + PS199*P[13][14] - PS214*P[2][14] + PS215*P[3][14] + PS216*P[0][14] + PS217*P[1][14] + PS87*P[14][15] + P[6][14];
1429 19972655 : const ftype PS221 = -PS197*P[13][14] + PS199*P[13][13] - PS214*P[2][13] + PS215*P[3][13] + PS216*P[0][13] + PS217*P[1][13] + PS87*P[13][15] + P[6][13];
1430 19972655 : const ftype PS222 = -PS197*P[6][14] + PS199*P[6][13] - PS214*P[2][6] + PS215*P[3][6] + PS216*P[0][6] + PS217*P[1][6] + PS87*P[6][15] + P[6][6];
1431 :
1432 19972655 : nextP[0][0] = PS0*PS1 - PS11*PS23 - PS12*PS26 - PS13*PS29 + PS14*PS6 + PS17*PS7 + PS2*PS3 + PS20*PS9 + PS33 + PS4*PS5;
1433 19972655 : nextP[0][1] = -PS1*PS36 + PS11*PS33 - PS12*PS29 + PS13*PS26 - PS14*PS34 + PS17*PS9 - PS20*PS7 + PS23 + PS3*PS35 - PS35*PS5;
1434 19972655 : nextP[1][1] = PS1*PS95 + PS100*PS11 + PS102*PS13 - PS105*PS34 - PS107*PS7 - PS109*PS12 + PS112 + PS2*PS5 + PS3*PS4 + PS9*PS97;
1435 19972655 : nextP[0][2] = -PS1*PS37 + PS11*PS29 + PS12*PS33 - PS13*PS23 - PS14*PS9 - PS17*PS34 + PS20*PS6 + PS26 - PS3*PS38 + PS37*PS5;
1436 19972655 : nextP[1][2] = PS1*PS40 + PS100*PS12 + PS102 - PS105*PS9 + PS107*PS6 + PS109*PS11 - PS112*PS13 - PS3*PS40 - PS34*PS97 - PS39*PS5;
1437 19972655 : nextP[2][2] = PS0*PS5 + PS1*PS4 + PS11*PS128 + PS12*PS130 + PS127*PS6 - PS13*PS135 - PS132*PS34 - PS133*PS9 + PS137 + PS3*PS95;
1438 19972655 : nextP[0][3] = PS1*PS39 - PS11*PS26 + PS12*PS23 + PS13*PS33 + PS14*PS7 - PS17*PS6 - PS20*PS34 + PS29 - PS3*PS39 - PS40*PS5;
1439 19972655 : nextP[1][3] = -PS1*PS38 + PS100*PS13 - PS102*PS11 + PS105*PS7 - PS107*PS34 + PS109 + PS112*PS12 - PS3*PS37 + PS38*PS5 - PS6*PS97;
1440 19972655 : nextP[2][3] = -PS1*PS35 - PS11*PS137 + PS12*PS135 - PS127*PS34 + PS128 + PS13*PS130 - PS132*PS6 + PS133*PS7 + PS3*PS36 - PS36*PS5;
1441 19972655 : nextP[3][3] = PS0*PS3 + PS1*PS2 - PS11*PS156 + PS12*PS152 + PS13*PS153 + PS151*PS7 - PS154*PS34 - PS155*PS6 + PS157 + PS5*PS95;
1442 :
1443 19972655 : if (quatCovResetOnly) {
1444 : // covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP
1445 : // to lower and upper half in P
1446 36905 : for (uint8_t row = 0; row <= 3; row++) {
1447 : // copy diagonals
1448 29524 : P[row][row] = constrain_ftype(nextP[row][row], 0.0f, 1.0f);
1449 : // copy off diagonals
1450 73810 : for (uint8_t column = 0 ; column < row; column++) {
1451 44286 : P[row][column] = P[column][row] = nextP[column][row];
1452 : }
1453 : }
1454 7381 : calcTiltErrorVariance();
1455 7381 : return;
1456 : }
1457 :
1458 19965274 : nextP[0][4] = PS43*PS44 - PS45*PS47 - PS54*PS55 + PS56*PS58 + PS61*PS62 + PS66*PS67 + PS71*PS72 + PS73;
1459 19965274 : nextP[1][4] = PS113*PS43 - PS115*PS45 - PS116*PS54 + PS118*PS56 + PS119*PS61 + PS120*PS66 + PS121*PS71 + PS122;
1460 19965274 : nextP[2][4] = PS138*PS43 - PS140*PS45 - PS141*PS54 + PS143*PS56 + PS144*PS61 + PS145*PS66 + PS146*PS71 + PS147;
1461 19965274 : nextP[3][4] = PS158*PS43 - PS160*PS45 - PS161*PS54 + PS163*PS56 + PS164*PS61 + PS165*PS66 + PS166*PS71 + PS167;
1462 19965274 : nextP[4][4] = -PS171*PS178 + PS172*PS180 + PS173*PS181 + PS174*PS182 + PS175*PS183 - PS176*PS179 + PS177*PS43 + PS184*sq(PS56) + PS185*sq(PS45) + PS186 + sq(PS43)*dvxVar;
1463 19965274 : nextP[0][5] = PS47*PS81 + PS55*PS85 + PS57*PS75 - PS62*PS80 - PS67*PS78 + PS72*PS83 - PS76*PS77 + PS86;
1464 19965274 : nextP[1][5] = PS115*PS81 + PS116*PS85 + PS117*PS75 - PS119*PS80 - PS120*PS78 + PS121*PS83 - PS123*PS76 + PS124;
1465 19965274 : nextP[2][5] = PS140*PS81 + PS141*PS85 + PS142*PS75 - PS144*PS80 - PS145*PS78 + PS146*PS83 - PS148*PS76 + PS149;
1466 19965274 : nextP[3][5] = PS160*PS81 + PS161*PS85 + PS162*PS75 - PS164*PS80 - PS165*PS78 + PS166*PS83 - PS168*PS76 + PS169;
1467 19965274 : nextP[4][5] = PS172*PS195 + PS178*PS190 + PS180*PS75 - PS185*PS45*PS81 - PS187*PS76 - PS188*PS78 - PS189*PS80 + PS191*PS83 + PS192*PS85 - PS193*PS194 + PS196;
1468 19965274 : nextP[5][5] = PS185*sq(PS81) + PS190*PS209 - PS193*PS206 + PS201*PS210 - PS202*PS207 + PS203*PS211 - PS204*PS208 + PS205*PS75 + PS212*sq(PS76) + PS213 + sq(PS75)*dvyVar;
1469 19965274 : nextP[0][6] = PS46*PS87 + PS55*PS91 - PS58*PS88 + PS62*PS93 + PS67*PS92 - PS72*PS89 + PS77*PS90 + PS94;
1470 19965274 : nextP[1][6] = PS114*PS87 + PS116*PS91 - PS118*PS88 + PS119*PS93 + PS120*PS92 - PS121*PS89 + PS123*PS90 + PS125;
1471 19965274 : nextP[2][6] = PS139*PS87 + PS141*PS91 - PS143*PS88 + PS144*PS93 + PS145*PS92 - PS146*PS89 + PS148*PS90 + PS150;
1472 19965274 : nextP[3][6] = PS159*PS87 + PS161*PS91 - PS163*PS88 + PS164*PS93 + PS165*PS92 - PS166*PS89 + PS168*PS90 + PS170;
1473 19965274 : nextP[4][6] = -PS171*PS198 + PS178*PS87 - PS180*PS197 - PS184*PS56*PS88 + PS187*PS90 + PS188*PS92 + PS189*PS93 - PS191*PS89 + PS192*PS91 + PS194*PS199 + PS200;
1474 19965274 : nextP[5][6] = PS190*PS198 - PS195*PS197 - PS197*PS205 + PS199*PS206 + PS207*PS216 + PS208*PS217 + PS209*PS87 - PS210*PS214 + PS211*PS215 - PS212*PS76*PS90 + PS218;
1475 19965274 : nextP[6][6] = PS184*sq(PS88) - PS197*PS220 + PS199*PS221 + PS212*sq(PS90) - PS214*(-PS197*P[2][14] + PS199*P[2][13] - PS214*P[2][2] + PS215*P[2][3] + PS216*P[0][2] + PS217*P[1][2] + PS87*P[2][15] + P[2][6]) + PS215*(-PS197*P[3][14] + PS199*P[3][13] - PS214*P[2][3] + PS215*P[3][3] + PS216*P[0][3] + PS217*P[1][3] + PS87*P[3][15] + P[3][6]) + PS216*(-PS197*P[0][14] + PS199*P[0][13] - PS214*P[0][2] + PS215*P[0][3] + PS216*P[0][0] + PS217*P[0][1] + PS87*P[0][15] + P[0][6]) + PS217*(-PS197*P[1][14] + PS199*P[1][13] - PS214*P[1][2] + PS215*P[1][3] + PS216*P[0][1] + PS217*P[1][1] + PS87*P[1][15] + P[1][6]) + PS219*PS87 + PS222 + sq(PS87)*dvzVar;
1476 19965274 : nextP[0][7] = -PS11*P[1][7] - PS12*P[2][7] - PS13*P[3][7] + PS6*P[7][10] + PS7*P[7][11] + PS73*dt + PS9*P[7][12] + P[0][7];
1477 19965274 : nextP[1][7] = PS11*P[0][7] - PS12*P[3][7] + PS122*dt + PS13*P[2][7] - PS34*P[7][10] - PS7*P[7][12] + PS9*P[7][11] + P[1][7];
1478 19965274 : nextP[2][7] = PS11*P[3][7] + PS12*P[0][7] - PS13*P[1][7] + PS147*dt - PS34*P[7][11] + PS6*P[7][12] - PS9*P[7][10] + P[2][7];
1479 19965274 : nextP[3][7] = -PS11*P[2][7] + PS12*P[1][7] + PS13*P[0][7] + PS167*dt - PS34*P[7][12] - PS6*P[7][11] + PS7*P[7][10] + P[3][7];
1480 19965274 : nextP[4][7] = -PS171*P[7][15] + PS172*P[7][14] + PS173*P[1][7] + PS174*P[0][7] + PS175*P[2][7] - PS176*P[3][7] + PS186*dt + PS43*P[7][13] + P[4][7];
1481 19965274 : nextP[5][7] = PS190*P[7][15] - PS193*P[7][13] + PS201*P[2][7] - PS202*P[0][7] + PS203*P[3][7] - PS204*P[1][7] + PS75*P[7][14] + P[5][7] + dt*(PS190*P[4][15] - PS193*P[4][13] + PS201*P[2][4] - PS202*P[0][4] + PS203*P[3][4] - PS204*P[1][4] + PS75*P[4][14] + P[4][5]);
1482 19965274 : nextP[6][7] = -PS197*P[7][14] + PS199*P[7][13] - PS214*P[2][7] + PS215*P[3][7] + PS216*P[0][7] + PS217*P[1][7] + PS87*P[7][15] + P[6][7] + dt*(-PS197*P[4][14] + PS199*P[4][13] - PS214*P[2][4] + PS215*P[3][4] + PS216*P[0][4] + PS217*P[1][4] + PS87*P[4][15] + P[4][6]);
1483 19965274 : nextP[7][7] = P[4][7]*dt + P[7][7] + dt*(P[4][4]*dt + P[4][7]);
1484 19965274 : nextP[0][8] = -PS11*P[1][8] - PS12*P[2][8] - PS13*P[3][8] + PS6*P[8][10] + PS7*P[8][11] + PS86*dt + PS9*P[8][12] + P[0][8];
1485 19965274 : nextP[1][8] = PS11*P[0][8] - PS12*P[3][8] + PS124*dt + PS13*P[2][8] - PS34*P[8][10] - PS7*P[8][12] + PS9*P[8][11] + P[1][8];
1486 19965274 : nextP[2][8] = PS11*P[3][8] + PS12*P[0][8] - PS13*P[1][8] + PS149*dt - PS34*P[8][11] + PS6*P[8][12] - PS9*P[8][10] + P[2][8];
1487 19965274 : nextP[3][8] = -PS11*P[2][8] + PS12*P[1][8] + PS13*P[0][8] + PS169*dt - PS34*P[8][12] - PS6*P[8][11] + PS7*P[8][10] + P[3][8];
1488 19965274 : nextP[4][8] = -PS171*P[8][15] + PS172*P[8][14] + PS173*P[1][8] + PS174*P[0][8] + PS175*P[2][8] - PS176*P[3][8] + PS196*dt + PS43*P[8][13] + P[4][8];
1489 19965274 : nextP[5][8] = PS190*P[8][15] - PS193*P[8][13] + PS201*P[2][8] - PS202*P[0][8] + PS203*P[3][8] - PS204*P[1][8] + PS213*dt + PS75*P[8][14] + P[5][8];
1490 19965274 : nextP[6][8] = -PS197*P[8][14] + PS199*P[8][13] - PS214*P[2][8] + PS215*P[3][8] + PS216*P[0][8] + PS217*P[1][8] + PS87*P[8][15] + P[6][8] + dt*(-PS197*P[5][14] + PS199*P[5][13] - PS214*P[2][5] + PS215*P[3][5] + PS216*P[0][5] + PS217*P[1][5] + PS87*P[5][15] + P[5][6]);
1491 19965274 : nextP[7][8] = P[4][8]*dt + P[7][8] + dt*(P[4][5]*dt + P[5][7]);
1492 19965274 : nextP[8][8] = P[5][8]*dt + P[8][8] + dt*(P[5][5]*dt + P[5][8]);
1493 19965274 : nextP[0][9] = -PS11*P[1][9] - PS12*P[2][9] - PS13*P[3][9] + PS6*P[9][10] + PS7*P[9][11] + PS9*P[9][12] + PS94*dt + P[0][9];
1494 19965274 : nextP[1][9] = PS11*P[0][9] - PS12*P[3][9] + PS125*dt + PS13*P[2][9] - PS34*P[9][10] - PS7*P[9][12] + PS9*P[9][11] + P[1][9];
1495 19965274 : nextP[2][9] = PS11*P[3][9] + PS12*P[0][9] - PS13*P[1][9] + PS150*dt - PS34*P[9][11] + PS6*P[9][12] - PS9*P[9][10] + P[2][9];
1496 19965274 : nextP[3][9] = -PS11*P[2][9] + PS12*P[1][9] + PS13*P[0][9] + PS170*dt - PS34*P[9][12] - PS6*P[9][11] + PS7*P[9][10] + P[3][9];
1497 19965274 : nextP[4][9] = -PS171*P[9][15] + PS172*P[9][14] + PS173*P[1][9] + PS174*P[0][9] + PS175*P[2][9] - PS176*P[3][9] + PS200*dt + PS43*P[9][13] + P[4][9];
1498 19965274 : nextP[5][9] = PS190*P[9][15] - PS193*P[9][13] + PS201*P[2][9] - PS202*P[0][9] + PS203*P[3][9] - PS204*P[1][9] + PS218*dt + PS75*P[9][14] + P[5][9];
1499 19965274 : nextP[6][9] = -PS197*P[9][14] + PS199*P[9][13] - PS214*P[2][9] + PS215*P[3][9] + PS216*P[0][9] + PS217*P[1][9] + PS222*dt + PS87*P[9][15] + P[6][9];
1500 19965274 : nextP[7][9] = P[4][9]*dt + P[7][9] + dt*(P[4][6]*dt + P[6][7]);
1501 19965274 : nextP[8][9] = P[5][9]*dt + P[8][9] + dt*(P[5][6]*dt + P[6][8]);
1502 19965274 : nextP[9][9] = P[6][9]*dt + P[9][9] + dt*(P[6][6]*dt + P[6][9]);
1503 :
1504 19965274 : if (stateIndexLim > 9) {
1505 19658046 : nextP[0][10] = PS14;
1506 19658046 : nextP[1][10] = PS105;
1507 19658046 : nextP[2][10] = PS133;
1508 19658046 : nextP[3][10] = PS151;
1509 19658046 : nextP[4][10] = -PS171*P[10][15] + PS172*P[10][14] + PS173*P[1][10] + PS174*P[0][10] + PS175*P[2][10] - PS176*P[3][10] + PS43*P[10][13] + P[4][10];
1510 19658046 : nextP[5][10] = PS190*P[10][15] - PS193*P[10][13] + PS201*P[2][10] - PS202*P[0][10] + PS203*P[3][10] - PS204*P[1][10] + PS75*P[10][14] + P[5][10];
1511 19658046 : nextP[6][10] = -PS197*P[10][14] + PS199*P[10][13] - PS214*P[2][10] + PS215*P[3][10] + PS216*P[0][10] + PS217*P[1][10] + PS87*P[10][15] + P[6][10];
1512 19658046 : nextP[7][10] = P[4][10]*dt + P[7][10];
1513 19658046 : nextP[8][10] = P[5][10]*dt + P[8][10];
1514 19658046 : nextP[9][10] = P[6][10]*dt + P[9][10];
1515 19658046 : nextP[10][10] = P[10][10];
1516 19658046 : nextP[0][11] = PS17;
1517 19658046 : nextP[1][11] = PS97;
1518 19658046 : nextP[2][11] = PS132;
1519 19658046 : nextP[3][11] = PS155;
1520 19658046 : nextP[4][11] = -PS171*P[11][15] + PS172*P[11][14] + PS173*P[1][11] + PS174*P[0][11] + PS175*P[2][11] - PS176*P[3][11] + PS43*P[11][13] + P[4][11];
1521 19658046 : nextP[5][11] = PS190*P[11][15] - PS193*P[11][13] + PS201*P[2][11] - PS202*P[0][11] + PS203*P[3][11] - PS204*P[1][11] + PS75*P[11][14] + P[5][11];
1522 19658046 : nextP[6][11] = -PS197*P[11][14] + PS199*P[11][13] - PS214*P[2][11] + PS215*P[3][11] + PS216*P[0][11] + PS217*P[1][11] + PS87*P[11][15] + P[6][11];
1523 19658046 : nextP[7][11] = P[4][11]*dt + P[7][11];
1524 19658046 : nextP[8][11] = P[5][11]*dt + P[8][11];
1525 19658046 : nextP[9][11] = P[6][11]*dt + P[9][11];
1526 19658046 : nextP[10][11] = P[10][11];
1527 19658046 : nextP[11][11] = P[11][11];
1528 19658046 : nextP[0][12] = PS20;
1529 19658046 : nextP[1][12] = PS107;
1530 19658046 : nextP[2][12] = PS127;
1531 19658046 : nextP[3][12] = PS154;
1532 19658046 : nextP[4][12] = -PS171*P[12][15] + PS172*P[12][14] + PS173*P[1][12] + PS174*P[0][12] + PS175*P[2][12] - PS176*P[3][12] + PS43*P[12][13] + P[4][12];
1533 19658046 : nextP[5][12] = PS190*P[12][15] - PS193*P[12][13] + PS201*P[2][12] - PS202*P[0][12] + PS203*P[3][12] - PS204*P[1][12] + PS75*P[12][14] + P[5][12];
1534 19658046 : nextP[6][12] = -PS197*P[12][14] + PS199*P[12][13] - PS214*P[2][12] + PS215*P[3][12] + PS216*P[0][12] + PS217*P[1][12] + PS87*P[12][15] + P[6][12];
1535 19658046 : nextP[7][12] = P[4][12]*dt + P[7][12];
1536 19658046 : nextP[8][12] = P[5][12]*dt + P[8][12];
1537 19658046 : nextP[9][12] = P[6][12]*dt + P[9][12];
1538 19658046 : nextP[10][12] = P[10][12];
1539 19658046 : nextP[11][12] = P[11][12];
1540 19658046 : nextP[12][12] = P[12][12];
1541 :
1542 19658046 : if (stateIndexLim > 12) {
1543 19658046 : nextP[0][13] = PS44;
1544 19658046 : nextP[1][13] = PS113;
1545 19658046 : nextP[2][13] = PS138;
1546 19658046 : nextP[3][13] = PS158;
1547 19658046 : nextP[4][13] = PS177;
1548 19658046 : nextP[5][13] = PS206;
1549 19658046 : nextP[6][13] = PS221;
1550 19658046 : nextP[7][13] = P[4][13]*dt + P[7][13];
1551 19658046 : nextP[8][13] = P[5][13]*dt + P[8][13];
1552 19658046 : nextP[9][13] = P[6][13]*dt + P[9][13];
1553 19658046 : nextP[10][13] = P[10][13];
1554 19658046 : nextP[11][13] = P[11][13];
1555 19658046 : nextP[12][13] = P[12][13];
1556 19658046 : nextP[13][13] = P[13][13];
1557 19658046 : nextP[0][14] = PS57;
1558 19658046 : nextP[1][14] = PS117;
1559 19658046 : nextP[2][14] = PS142;
1560 19658046 : nextP[3][14] = PS162;
1561 19658046 : nextP[4][14] = PS180;
1562 19658046 : nextP[5][14] = PS205;
1563 19658046 : nextP[6][14] = PS220;
1564 19658046 : nextP[7][14] = P[4][14]*dt + P[7][14];
1565 19658046 : nextP[8][14] = P[5][14]*dt + P[8][14];
1566 19658046 : nextP[9][14] = P[6][14]*dt + P[9][14];
1567 19658046 : nextP[10][14] = P[10][14];
1568 19658046 : nextP[11][14] = P[11][14];
1569 19658046 : nextP[12][14] = P[12][14];
1570 19658046 : nextP[13][14] = P[13][14];
1571 19658046 : nextP[14][14] = P[14][14];
1572 19658046 : nextP[0][15] = PS46;
1573 19658046 : nextP[1][15] = PS114;
1574 19658046 : nextP[2][15] = PS139;
1575 19658046 : nextP[3][15] = PS159;
1576 19658046 : nextP[4][15] = PS178;
1577 19658046 : nextP[5][15] = PS209;
1578 19658046 : nextP[6][15] = PS219;
1579 19658046 : nextP[7][15] = P[4][15]*dt + P[7][15];
1580 19658046 : nextP[8][15] = P[5][15]*dt + P[8][15];
1581 19658046 : nextP[9][15] = P[6][15]*dt + P[9][15];
1582 19658046 : nextP[10][15] = P[10][15];
1583 19658046 : nextP[11][15] = P[11][15];
1584 19658046 : nextP[12][15] = P[12][15];
1585 19658046 : nextP[13][15] = P[13][15];
1586 19658046 : nextP[14][15] = P[14][15];
1587 19658046 : nextP[15][15] = P[15][15];
1588 :
1589 19658046 : if (stateIndexLim > 15) {
1590 12095658 : nextP[0][16] = -PS11*P[1][16] - PS12*P[2][16] - PS13*P[3][16] + PS6*P[10][16] + PS7*P[11][16] + PS9*P[12][16] + P[0][16];
1591 12095658 : nextP[1][16] = PS11*P[0][16] - PS12*P[3][16] + PS13*P[2][16] - PS34*P[10][16] - PS7*P[12][16] + PS9*P[11][16] + P[1][16];
1592 12095658 : nextP[2][16] = PS11*P[3][16] + PS12*P[0][16] - PS13*P[1][16] - PS34*P[11][16] + PS6*P[12][16] - PS9*P[10][16] + P[2][16];
1593 12095658 : nextP[3][16] = -PS11*P[2][16] + PS12*P[1][16] + PS13*P[0][16] - PS34*P[12][16] - PS6*P[11][16] + PS7*P[10][16] + P[3][16];
1594 12095658 : nextP[4][16] = -PS171*P[15][16] + PS172*P[14][16] + PS173*P[1][16] + PS174*P[0][16] + PS175*P[2][16] - PS176*P[3][16] + PS43*P[13][16] + P[4][16];
1595 12095658 : nextP[5][16] = PS190*P[15][16] - PS193*P[13][16] + PS201*P[2][16] - PS202*P[0][16] + PS203*P[3][16] - PS204*P[1][16] + PS75*P[14][16] + P[5][16];
1596 12095658 : nextP[6][16] = -PS197*P[14][16] + PS199*P[13][16] - PS214*P[2][16] + PS215*P[3][16] + PS216*P[0][16] + PS217*P[1][16] + PS87*P[15][16] + P[6][16];
1597 12095658 : nextP[7][16] = P[4][16]*dt + P[7][16];
1598 12095658 : nextP[8][16] = P[5][16]*dt + P[8][16];
1599 12095658 : nextP[9][16] = P[6][16]*dt + P[9][16];
1600 12095658 : nextP[10][16] = P[10][16];
1601 12095658 : nextP[11][16] = P[11][16];
1602 12095658 : nextP[12][16] = P[12][16];
1603 12095658 : nextP[13][16] = P[13][16];
1604 12095658 : nextP[14][16] = P[14][16];
1605 12095658 : nextP[15][16] = P[15][16];
1606 12095658 : nextP[16][16] = P[16][16];
1607 12095658 : nextP[0][17] = -PS11*P[1][17] - PS12*P[2][17] - PS13*P[3][17] + PS6*P[10][17] + PS7*P[11][17] + PS9*P[12][17] + P[0][17];
1608 12095658 : nextP[1][17] = PS11*P[0][17] - PS12*P[3][17] + PS13*P[2][17] - PS34*P[10][17] - PS7*P[12][17] + PS9*P[11][17] + P[1][17];
1609 12095658 : nextP[2][17] = PS11*P[3][17] + PS12*P[0][17] - PS13*P[1][17] - PS34*P[11][17] + PS6*P[12][17] - PS9*P[10][17] + P[2][17];
1610 12095658 : nextP[3][17] = -PS11*P[2][17] + PS12*P[1][17] + PS13*P[0][17] - PS34*P[12][17] - PS6*P[11][17] + PS7*P[10][17] + P[3][17];
1611 12095658 : nextP[4][17] = -PS171*P[15][17] + PS172*P[14][17] + PS173*P[1][17] + PS174*P[0][17] + PS175*P[2][17] - PS176*P[3][17] + PS43*P[13][17] + P[4][17];
1612 12095658 : nextP[5][17] = PS190*P[15][17] - PS193*P[13][17] + PS201*P[2][17] - PS202*P[0][17] + PS203*P[3][17] - PS204*P[1][17] + PS75*P[14][17] + P[5][17];
1613 12095658 : nextP[6][17] = -PS197*P[14][17] + PS199*P[13][17] - PS214*P[2][17] + PS215*P[3][17] + PS216*P[0][17] + PS217*P[1][17] + PS87*P[15][17] + P[6][17];
1614 12095658 : nextP[7][17] = P[4][17]*dt + P[7][17];
1615 12095658 : nextP[8][17] = P[5][17]*dt + P[8][17];
1616 12095658 : nextP[9][17] = P[6][17]*dt + P[9][17];
1617 12095658 : nextP[10][17] = P[10][17];
1618 12095658 : nextP[11][17] = P[11][17];
1619 12095658 : nextP[12][17] = P[12][17];
1620 12095658 : nextP[13][17] = P[13][17];
1621 12095658 : nextP[14][17] = P[14][17];
1622 12095658 : nextP[15][17] = P[15][17];
1623 12095658 : nextP[16][17] = P[16][17];
1624 12095658 : nextP[17][17] = P[17][17];
1625 12095658 : nextP[0][18] = -PS11*P[1][18] - PS12*P[2][18] - PS13*P[3][18] + PS6*P[10][18] + PS7*P[11][18] + PS9*P[12][18] + P[0][18];
1626 12095658 : nextP[1][18] = PS11*P[0][18] - PS12*P[3][18] + PS13*P[2][18] - PS34*P[10][18] - PS7*P[12][18] + PS9*P[11][18] + P[1][18];
1627 12095658 : nextP[2][18] = PS11*P[3][18] + PS12*P[0][18] - PS13*P[1][18] - PS34*P[11][18] + PS6*P[12][18] - PS9*P[10][18] + P[2][18];
1628 12095658 : nextP[3][18] = -PS11*P[2][18] + PS12*P[1][18] + PS13*P[0][18] - PS34*P[12][18] - PS6*P[11][18] + PS7*P[10][18] + P[3][18];
1629 12095658 : nextP[4][18] = -PS171*P[15][18] + PS172*P[14][18] + PS173*P[1][18] + PS174*P[0][18] + PS175*P[2][18] - PS176*P[3][18] + PS43*P[13][18] + P[4][18];
1630 12095658 : nextP[5][18] = PS190*P[15][18] - PS193*P[13][18] + PS201*P[2][18] - PS202*P[0][18] + PS203*P[3][18] - PS204*P[1][18] + PS75*P[14][18] + P[5][18];
1631 12095658 : nextP[6][18] = -PS197*P[14][18] + PS199*P[13][18] - PS214*P[2][18] + PS215*P[3][18] + PS216*P[0][18] + PS217*P[1][18] + PS87*P[15][18] + P[6][18];
1632 12095658 : nextP[7][18] = P[4][18]*dt + P[7][18];
1633 12095658 : nextP[8][18] = P[5][18]*dt + P[8][18];
1634 12095658 : nextP[9][18] = P[6][18]*dt + P[9][18];
1635 12095658 : nextP[10][18] = P[10][18];
1636 12095658 : nextP[11][18] = P[11][18];
1637 12095658 : nextP[12][18] = P[12][18];
1638 12095658 : nextP[13][18] = P[13][18];
1639 12095658 : nextP[14][18] = P[14][18];
1640 12095658 : nextP[15][18] = P[15][18];
1641 12095658 : nextP[16][18] = P[16][18];
1642 12095658 : nextP[17][18] = P[17][18];
1643 12095658 : nextP[18][18] = P[18][18];
1644 12095658 : nextP[0][19] = -PS11*P[1][19] - PS12*P[2][19] - PS13*P[3][19] + PS6*P[10][19] + PS7*P[11][19] + PS9*P[12][19] + P[0][19];
1645 12095658 : nextP[1][19] = PS11*P[0][19] - PS12*P[3][19] + PS13*P[2][19] - PS34*P[10][19] - PS7*P[12][19] + PS9*P[11][19] + P[1][19];
1646 12095658 : nextP[2][19] = PS11*P[3][19] + PS12*P[0][19] - PS13*P[1][19] - PS34*P[11][19] + PS6*P[12][19] - PS9*P[10][19] + P[2][19];
1647 12095658 : nextP[3][19] = -PS11*P[2][19] + PS12*P[1][19] + PS13*P[0][19] - PS34*P[12][19] - PS6*P[11][19] + PS7*P[10][19] + P[3][19];
1648 12095658 : nextP[4][19] = -PS171*P[15][19] + PS172*P[14][19] + PS173*P[1][19] + PS174*P[0][19] + PS175*P[2][19] - PS176*P[3][19] + PS43*P[13][19] + P[4][19];
1649 12095658 : nextP[5][19] = PS190*P[15][19] - PS193*P[13][19] + PS201*P[2][19] - PS202*P[0][19] + PS203*P[3][19] - PS204*P[1][19] + PS75*P[14][19] + P[5][19];
1650 12095658 : nextP[6][19] = -PS197*P[14][19] + PS199*P[13][19] - PS214*P[2][19] + PS215*P[3][19] + PS216*P[0][19] + PS217*P[1][19] + PS87*P[15][19] + P[6][19];
1651 12095658 : nextP[7][19] = P[4][19]*dt + P[7][19];
1652 12095658 : nextP[8][19] = P[5][19]*dt + P[8][19];
1653 12095658 : nextP[9][19] = P[6][19]*dt + P[9][19];
1654 12095658 : nextP[10][19] = P[10][19];
1655 12095658 : nextP[11][19] = P[11][19];
1656 12095658 : nextP[12][19] = P[12][19];
1657 12095658 : nextP[13][19] = P[13][19];
1658 12095658 : nextP[14][19] = P[14][19];
1659 12095658 : nextP[15][19] = P[15][19];
1660 12095658 : nextP[16][19] = P[16][19];
1661 12095658 : nextP[17][19] = P[17][19];
1662 12095658 : nextP[18][19] = P[18][19];
1663 12095658 : nextP[19][19] = P[19][19];
1664 12095658 : nextP[0][20] = -PS11*P[1][20] - PS12*P[2][20] - PS13*P[3][20] + PS6*P[10][20] + PS7*P[11][20] + PS9*P[12][20] + P[0][20];
1665 12095658 : nextP[1][20] = PS11*P[0][20] - PS12*P[3][20] + PS13*P[2][20] - PS34*P[10][20] - PS7*P[12][20] + PS9*P[11][20] + P[1][20];
1666 12095658 : nextP[2][20] = PS11*P[3][20] + PS12*P[0][20] - PS13*P[1][20] - PS34*P[11][20] + PS6*P[12][20] - PS9*P[10][20] + P[2][20];
1667 12095658 : nextP[3][20] = -PS11*P[2][20] + PS12*P[1][20] + PS13*P[0][20] - PS34*P[12][20] - PS6*P[11][20] + PS7*P[10][20] + P[3][20];
1668 12095658 : nextP[4][20] = -PS171*P[15][20] + PS172*P[14][20] + PS173*P[1][20] + PS174*P[0][20] + PS175*P[2][20] - PS176*P[3][20] + PS43*P[13][20] + P[4][20];
1669 12095658 : nextP[5][20] = PS190*P[15][20] - PS193*P[13][20] + PS201*P[2][20] - PS202*P[0][20] + PS203*P[3][20] - PS204*P[1][20] + PS75*P[14][20] + P[5][20];
1670 12095658 : nextP[6][20] = -PS197*P[14][20] + PS199*P[13][20] - PS214*P[2][20] + PS215*P[3][20] + PS216*P[0][20] + PS217*P[1][20] + PS87*P[15][20] + P[6][20];
1671 12095658 : nextP[7][20] = P[4][20]*dt + P[7][20];
1672 12095658 : nextP[8][20] = P[5][20]*dt + P[8][20];
1673 12095658 : nextP[9][20] = P[6][20]*dt + P[9][20];
1674 12095658 : nextP[10][20] = P[10][20];
1675 12095658 : nextP[11][20] = P[11][20];
1676 12095658 : nextP[12][20] = P[12][20];
1677 12095658 : nextP[13][20] = P[13][20];
1678 12095658 : nextP[14][20] = P[14][20];
1679 12095658 : nextP[15][20] = P[15][20];
1680 12095658 : nextP[16][20] = P[16][20];
1681 12095658 : nextP[17][20] = P[17][20];
1682 12095658 : nextP[18][20] = P[18][20];
1683 12095658 : nextP[19][20] = P[19][20];
1684 12095658 : nextP[20][20] = P[20][20];
1685 12095658 : nextP[0][21] = -PS11*P[1][21] - PS12*P[2][21] - PS13*P[3][21] + PS6*P[10][21] + PS7*P[11][21] + PS9*P[12][21] + P[0][21];
1686 12095658 : nextP[1][21] = PS11*P[0][21] - PS12*P[3][21] + PS13*P[2][21] - PS34*P[10][21] - PS7*P[12][21] + PS9*P[11][21] + P[1][21];
1687 12095658 : nextP[2][21] = PS11*P[3][21] + PS12*P[0][21] - PS13*P[1][21] - PS34*P[11][21] + PS6*P[12][21] - PS9*P[10][21] + P[2][21];
1688 12095658 : nextP[3][21] = -PS11*P[2][21] + PS12*P[1][21] + PS13*P[0][21] - PS34*P[12][21] - PS6*P[11][21] + PS7*P[10][21] + P[3][21];
1689 12095658 : nextP[4][21] = -PS171*P[15][21] + PS172*P[14][21] + PS173*P[1][21] + PS174*P[0][21] + PS175*P[2][21] - PS176*P[3][21] + PS43*P[13][21] + P[4][21];
1690 12095658 : nextP[5][21] = PS190*P[15][21] - PS193*P[13][21] + PS201*P[2][21] - PS202*P[0][21] + PS203*P[3][21] - PS204*P[1][21] + PS75*P[14][21] + P[5][21];
1691 12095658 : nextP[6][21] = -PS197*P[14][21] + PS199*P[13][21] - PS214*P[2][21] + PS215*P[3][21] + PS216*P[0][21] + PS217*P[1][21] + PS87*P[15][21] + P[6][21];
1692 12095658 : nextP[7][21] = P[4][21]*dt + P[7][21];
1693 12095658 : nextP[8][21] = P[5][21]*dt + P[8][21];
1694 12095658 : nextP[9][21] = P[6][21]*dt + P[9][21];
1695 12095658 : nextP[10][21] = P[10][21];
1696 12095658 : nextP[11][21] = P[11][21];
1697 12095658 : nextP[12][21] = P[12][21];
1698 12095658 : nextP[13][21] = P[13][21];
1699 12095658 : nextP[14][21] = P[14][21];
1700 12095658 : nextP[15][21] = P[15][21];
1701 12095658 : nextP[16][21] = P[16][21];
1702 12095658 : nextP[17][21] = P[17][21];
1703 12095658 : nextP[18][21] = P[18][21];
1704 12095658 : nextP[19][21] = P[19][21];
1705 12095658 : nextP[20][21] = P[20][21];
1706 12095658 : nextP[21][21] = P[21][21];
1707 :
1708 12095658 : if (stateIndexLim > 21) {
1709 3552705 : nextP[0][22] = -PS11*P[1][22] - PS12*P[2][22] - PS13*P[3][22] + PS6*P[10][22] + PS7*P[11][22] + PS9*P[12][22] + P[0][22];
1710 3552705 : nextP[1][22] = PS11*P[0][22] - PS12*P[3][22] + PS13*P[2][22] - PS34*P[10][22] - PS7*P[12][22] + PS9*P[11][22] + P[1][22];
1711 3552705 : nextP[2][22] = PS11*P[3][22] + PS12*P[0][22] - PS13*P[1][22] - PS34*P[11][22] + PS6*P[12][22] - PS9*P[10][22] + P[2][22];
1712 3552705 : nextP[3][22] = -PS11*P[2][22] + PS12*P[1][22] + PS13*P[0][22] - PS34*P[12][22] - PS6*P[11][22] + PS7*P[10][22] + P[3][22];
1713 3552705 : nextP[4][22] = -PS171*P[15][22] + PS172*P[14][22] + PS173*P[1][22] + PS174*P[0][22] + PS175*P[2][22] - PS176*P[3][22] + PS43*P[13][22] + P[4][22];
1714 3552705 : nextP[5][22] = PS190*P[15][22] - PS193*P[13][22] + PS201*P[2][22] - PS202*P[0][22] + PS203*P[3][22] - PS204*P[1][22] + PS75*P[14][22] + P[5][22];
1715 3552705 : nextP[6][22] = -PS197*P[14][22] + PS199*P[13][22] - PS214*P[2][22] + PS215*P[3][22] + PS216*P[0][22] + PS217*P[1][22] + PS87*P[15][22] + P[6][22];
1716 3552705 : nextP[7][22] = P[4][22]*dt + P[7][22];
1717 3552705 : nextP[8][22] = P[5][22]*dt + P[8][22];
1718 3552705 : nextP[9][22] = P[6][22]*dt + P[9][22];
1719 3552705 : nextP[10][22] = P[10][22];
1720 3552705 : nextP[11][22] = P[11][22];
1721 3552705 : nextP[12][22] = P[12][22];
1722 3552705 : nextP[13][22] = P[13][22];
1723 3552705 : nextP[14][22] = P[14][22];
1724 3552705 : nextP[15][22] = P[15][22];
1725 3552705 : nextP[16][22] = P[16][22];
1726 3552705 : nextP[17][22] = P[17][22];
1727 3552705 : nextP[18][22] = P[18][22];
1728 3552705 : nextP[19][22] = P[19][22];
1729 3552705 : nextP[20][22] = P[20][22];
1730 3552705 : nextP[21][22] = P[21][22];
1731 3552705 : nextP[22][22] = P[22][22];
1732 3552705 : nextP[0][23] = -PS11*P[1][23] - PS12*P[2][23] - PS13*P[3][23] + PS6*P[10][23] + PS7*P[11][23] + PS9*P[12][23] + P[0][23];
1733 3552705 : nextP[1][23] = PS11*P[0][23] - PS12*P[3][23] + PS13*P[2][23] - PS34*P[10][23] - PS7*P[12][23] + PS9*P[11][23] + P[1][23];
1734 3552705 : nextP[2][23] = PS11*P[3][23] + PS12*P[0][23] - PS13*P[1][23] - PS34*P[11][23] + PS6*P[12][23] - PS9*P[10][23] + P[2][23];
1735 3552705 : nextP[3][23] = -PS11*P[2][23] + PS12*P[1][23] + PS13*P[0][23] - PS34*P[12][23] - PS6*P[11][23] + PS7*P[10][23] + P[3][23];
1736 3552705 : nextP[4][23] = -PS171*P[15][23] + PS172*P[14][23] + PS173*P[1][23] + PS174*P[0][23] + PS175*P[2][23] - PS176*P[3][23] + PS43*P[13][23] + P[4][23];
1737 3552705 : nextP[5][23] = PS190*P[15][23] - PS193*P[13][23] + PS201*P[2][23] - PS202*P[0][23] + PS203*P[3][23] - PS204*P[1][23] + PS75*P[14][23] + P[5][23];
1738 3552705 : nextP[6][23] = -PS197*P[14][23] + PS199*P[13][23] - PS214*P[2][23] + PS215*P[3][23] + PS216*P[0][23] + PS217*P[1][23] + PS87*P[15][23] + P[6][23];
1739 3552705 : nextP[7][23] = P[4][23]*dt + P[7][23];
1740 3552705 : nextP[8][23] = P[5][23]*dt + P[8][23];
1741 3552705 : nextP[9][23] = P[6][23]*dt + P[9][23];
1742 3552705 : nextP[10][23] = P[10][23];
1743 3552705 : nextP[11][23] = P[11][23];
1744 3552705 : nextP[12][23] = P[12][23];
1745 3552705 : nextP[13][23] = P[13][23];
1746 3552705 : nextP[14][23] = P[14][23];
1747 3552705 : nextP[15][23] = P[15][23];
1748 3552705 : nextP[16][23] = P[16][23];
1749 3552705 : nextP[17][23] = P[17][23];
1750 3552705 : nextP[18][23] = P[18][23];
1751 3552705 : nextP[19][23] = P[19][23];
1752 3552705 : nextP[20][23] = P[20][23];
1753 3552705 : nextP[21][23] = P[21][23];
1754 3552705 : nextP[22][23] = P[22][23];
1755 3552705 : nextP[23][23] = P[23][23];
1756 : }
1757 : }
1758 : }
1759 : }
1760 :
1761 : // add the general state process noise variances
1762 19965274 : if (stateIndexLim > 9) {
1763 217285680 : for (uint8_t i=10; i<=stateIndexLim; i++) {
1764 197627634 : nextP[i][i] = nextP[i][i] + processNoiseVariance[i-10];
1765 : }
1766 : }
1767 :
1768 : // if the total position variance exceeds 1e4 (100m), then stop covariance
1769 : // growth by setting the predicted to the previous values
1770 : // This prevent an ill conditioned matrix from occurring for long periods
1771 : // without GPS
1772 19965274 : if ((P[7][7] + P[8][8]) > 1e4f) {
1773 48912 : for (uint8_t i=7; i<=8; i++)
1774 : {
1775 706120 : for (uint8_t j=0; j<=stateIndexLim; j++)
1776 : {
1777 673512 : nextP[i][j] = P[i][j];
1778 673512 : nextP[j][i] = P[j][i];
1779 : }
1780 : }
1781 : }
1782 :
1783 : // covariance matrix is symmetrical, so copy diagonals and copy lower half in nextP
1784 : // to lower and upper half in P
1785 417245648 : for (uint8_t row = 0; row <= stateIndexLim; row++) {
1786 : // copy diagonals
1787 397280374 : P[row][row] = nextP[row][row];
1788 : // copy off diagonals
1789 4272560917 : for (uint8_t column = 0 ; column < row; column++) {
1790 3875280543 : P[row][column] = P[column][row] = nextP[column][row];
1791 : }
1792 : }
1793 :
1794 : // inactive delta velocity bias states have all covariances zeroed to
1795 : // prevent interaction with other states
1796 19965274 : if (!inhibitDelVelBiasStates) {
1797 78630296 : for (uint8_t index=0; index<3; index++) {
1798 58972722 : const uint8_t stateIndex = index + 13;
1799 58972722 : if (dvelBiasAxisInhibit[index]) {
1800 12076408 : zeroStatesVarCov(stateIndex, stateIndex);
1801 12076408 : P[stateIndex][stateIndex] = dvelBiasAxisVarPrev[index];
1802 : }
1803 : }
1804 : }
1805 :
1806 : // constrain values to prevent ill-conditioning
1807 19965274 : ConstrainVariances();
1808 :
1809 19965274 : if (vertVelVarClipCounter > 0) {
1810 0 : vertVelVarClipCounter--;
1811 : }
1812 :
1813 19965274 : calcTiltErrorVariance();
1814 :
1815 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
1816 19965274 : verifyTiltErrorVariance();
1817 : #endif
1818 : }
1819 :
1820 : // zero specified state variances and covariances in state covariance matrix
1821 57611682 : void NavEKF3_core::zeroStatesVarCov(uint8_t first, uint8_t last)
1822 : {
1823 : uint8_t row;
1824 217817790 : for (row=first; row<=last; row++)
1825 : {
1826 160206108 : zero_range(&P[row][0], 0, 23);
1827 : }
1828 :
1829 1440292050 : for (row=0; row<=23; row++)
1830 : {
1831 1382680368 : zero_range(&P[row][0], first, last);
1832 : }
1833 57611682 : }
1834 :
1835 : // reset the output data to the current EKF state
1836 3144 : void NavEKF3_core::StoreOutputReset()
1837 : {
1838 3144 : outputDataNew.quat = stateStruct.quat;
1839 3144 : outputDataNew.velocity = stateStruct.velocity;
1840 3144 : outputDataNew.position = stateStruct.position;
1841 : // write current measurement to entire table
1842 31713 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1843 28569 : storedOutput[i] = outputDataNew;
1844 : }
1845 3144 : outputDataDelayed = outputDataNew;
1846 : // reset the states for the complementary filter used to provide a vertical position derivative output
1847 3144 : vertCompFiltState.pos = stateStruct.position.z;
1848 3144 : vertCompFiltState.vel = stateStruct.velocity.z;
1849 3144 : }
1850 :
1851 : // Reset the stored output quaternion history to current EKF state
1852 0 : void NavEKF3_core::StoreQuatReset()
1853 : {
1854 0 : outputDataNew.quat = stateStruct.quat;
1855 : // write current measurement to entire table
1856 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1857 0 : storedOutput[i].quat = outputDataNew.quat;
1858 : }
1859 0 : outputDataDelayed.quat = outputDataNew.quat;
1860 0 : }
1861 :
1862 : // Rotate the stored output quaternion history through a quaternion rotation
1863 4237 : void NavEKF3_core::StoreQuatRotate(const QuaternionF &deltaQuat)
1864 : {
1865 4237 : outputDataNew.quat = outputDataNew.quat*deltaQuat;
1866 : // write current measurement to entire table
1867 42722 : for (uint8_t i=0; i<imu_buffer_length; i++) {
1868 38485 : storedOutput[i].quat = storedOutput[i].quat*deltaQuat;
1869 : }
1870 4237 : outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
1871 4237 : }
1872 :
1873 : // constrain variances (diagonal terms) in the state covariance matrix to prevent ill-conditioning
1874 : // if states are inactive, zero the corresponding off-diagonals
1875 39772176 : void NavEKF3_core::ConstrainVariances()
1876 : {
1877 : // Covariance constraints as of March 2025
1878 : // This table assumes normal operations, there are additional constraints for specific failure modes like "badIMUdata" and "inhibitDelAngBiasStates".
1879 : // +----------------------------------------------------------------------------------------------+
1880 : // | State Index | State Name | State Units | Variance Constraint Range |
1881 : // +----------------------------------------------------------------------------------------------+
1882 : // | 0 .. 3 | Attitude Quaternion | unitless | [0.0, 1.0] |
1883 : // | 4 .. 5 | Velocity (North, East) | m/s | [1e-4, 1e3] |
1884 : // | 6 | Velocity (Down) | m/s | dynamic |
1885 : // | 7 .. 9 | Position (North, East, Down) | m | [1e-4, 1e6] |
1886 : // | 10 .. 12 | Gyro Bias (X, Y, Z) | rad | [0.0, (0.175 * dtEkfAvg)^2] |
1887 : // | 13 .. 15 | Accel Bias (X, Y, Z) | m/s^2 | dynamic |
1888 : // | 16 .. 18 | Earth Magnetic Field (X, Y, Z) | Gauss | [0.0, 0.01] |
1889 : // | 19 .. 21 | Body Magnetic Field (X, Y, Z) | Gauss | [0.0, 0.01] |
1890 : // | 22 .. 23 | Wind Velocity (North, East) | m/s | [0.0, 400] |
1891 : // +----------------------------------------------------------------------------------------------+
1892 :
1893 198860880 : for (uint8_t i=0; i<=3; i++) P[i][i] = constrain_ftype(P[i][i],0.0,1.0); // attitude error
1894 119316528 : for (uint8_t i=4; i<=5; i++) P[i][i] = constrain_ftype(P[i][i], VEL_STATE_MIN_VARIANCE, 1.0e3); // NE velocity
1895 :
1896 : // if vibration affected use sensor observation variances to set a floor on the state variances
1897 39772176 : if (badIMUdata) {
1898 32410 : P[6][6] = fmaxF(P[6][6], sq(frontend->_gpsVertVelNoise));
1899 32410 : P[9][9] = fmaxF(P[9][9], sq(frontend->_baroAltNoise));
1900 39739766 : } else if (P[6][6] < VEL_STATE_MIN_VARIANCE) {
1901 : // handle collapse of the vertical velocity variance
1902 0 : P[6][6] = VEL_STATE_MIN_VARIANCE;
1903 : // this counter is decremented by 1 each prediction cycle in CovariancePrediction
1904 : // resulting in the count from each clip event fading to zero over 1 second which
1905 : // is sufficient to capture collapse from fusion of the lowest update rate sensor
1906 0 : vertVelVarClipCounter += EKF_TARGET_RATE_HZ;
1907 0 : if (vertVelVarClipCounter > VERT_VEL_VAR_CLIP_COUNT_LIM) {
1908 : // reset the corresponding covariances
1909 0 : zeroStatesVarCov(6, 6);
1910 :
1911 : // set the variances to the measurement variance
1912 : #if EK3_FEATURE_EXTERNAL_NAV
1913 0 : if (useExtNavVel) {
1914 0 : P[6][6] = sq(extNavVelDelayed.err);
1915 : } else
1916 : #endif
1917 : {
1918 0 : P[6][6] = sq(frontend->_gpsVertVelNoise);
1919 : }
1920 0 : vertVelVarClipCounter = 0;
1921 : }
1922 : }
1923 :
1924 159088704 : for (uint8_t i=7; i<=9; i++) P[i][i] = constrain_ftype(P[i][i], POS_STATE_MIN_VARIANCE, 1.0e6); // NED position
1925 :
1926 39772176 : if (!inhibitDelAngBiasStates) {
1927 157324912 : for (uint8_t i=10; i<=12; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,sq(0.175 * dtEkfAvg));
1928 : } else {
1929 440948 : zeroStatesVarCov(10, 12);
1930 : }
1931 :
1932 : const ftype minSafeStateVar = 5E-9;
1933 39772176 : if (!inhibitDelVelBiasStates) {
1934 :
1935 : // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
1936 : ftype maxStateVar = 0.0F;
1937 : bool resetRequired = false;
1938 157324912 : for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
1939 117993684 : if (P[stateIndex][stateIndex] > maxStateVar) {
1940 : maxStateVar = P[stateIndex][stateIndex];
1941 60168074 : } else if (P[stateIndex][stateIndex] < minSafeStateVar) {
1942 : resetRequired = true;
1943 : }
1944 : }
1945 :
1946 : // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
1947 : // not exceed 100 and the minimum variance must not fall below the target minimum
1948 39331228 : ftype minAllowedStateVar = fmaxF(0.01f * maxStateVar, minSafeStateVar);
1949 157324912 : for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
1950 117993684 : P[stateIndex][stateIndex] = constrain_ftype(P[stateIndex][stateIndex], minAllowedStateVar, sq(10.0f * dtEkfAvg));
1951 : }
1952 :
1953 : // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
1954 39331228 : if (resetRequired) {
1955 : // reset all delta velocity bias covariances
1956 0 : zeroStatesVarCov(13, 15);
1957 : // set all delta velocity bias variances to initial values and zero bias states
1958 0 : P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
1959 0 : P[14][14] = P[13][13];
1960 0 : P[15][15] = P[13][13];
1961 0 : stateStruct.accel_bias.zero();
1962 : }
1963 :
1964 : } else {
1965 440948 : zeroStatesVarCov(13, 15);
1966 : // set all delta velocity bias variances to a margin above the minimum safe value
1967 1763792 : for (uint8_t i=0; i<=2; i++) {
1968 1322844 : const uint8_t stateIndex = i + 13;
1969 1322844 : P[stateIndex][stateIndex] = fmaxF(P[stateIndex][stateIndex], minSafeStateVar * 10.0F);
1970 : }
1971 : }
1972 :
1973 39772176 : if (!inhibitMagStates) {
1974 102931204 : for (uint8_t i=16; i<=18; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // earth magnetic field
1975 102931204 : for (uint8_t i=19; i<=21; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,0.01f); // body magnetic field
1976 : } else {
1977 14039375 : zeroStatesVarCov(16, 21);
1978 : }
1979 :
1980 39772176 : if (!inhibitWindStates) {
1981 9313508 : if (treatWindStatesAsTruth) {
1982 85922 : zeroStatesVarCov(22, 23);
1983 : } else {
1984 27682758 : for (uint8_t i=22; i<=23; i++) P[i][i] = constrain_ftype(P[i][i],0.0f,WIND_VEL_VARIANCE_MAX);
1985 : }
1986 : } else {
1987 30458668 : zeroStatesVarCov(22, 23);
1988 : }
1989 39772176 : }
1990 :
1991 : // actually do fusion to update statesArray from Kfusion and P from KHP.
1992 : // returns true and skips fusion if variances would be driven negative.
1993 : // force skips this negative check; passing true is probably a bug!
1994 19806902 : bool NavEKF3_core::FinishFusion(ftype innov, bool force /*= false*/)
1995 : {
1996 19806902 : if (!force) {
1997 : // Check that we are not going to drive any variances negative and skip the update if so
1998 400613730 : for (auto s=0; s<=stateIndexLim; s++) {
1999 381966090 : if (KHP[s][s] > P[s][s]) {
2000 : return true;
2001 : }
2002 : }
2003 : }
2004 :
2005 : // correct the state vector using kalman gains filled in by caller
2006 429595280 : for (auto s=0; s<=stateIndexLim; s++) {
2007 409788378 : statesArray[s] -= Kfusion[s] * innov;
2008 : }
2009 19806902 : stateStruct.quat.normalize();
2010 :
2011 : // update the covariance matrix as P = P - KHP (KHP was filled by caller)
2012 429595280 : for (auto r=0; r<=stateIndexLim; r++) {
2013 4965514005 : for (auto c=0; c<=r; c++) {
2014 : // P must end up symmetric, so average the upper and lower
2015 : // differences, then store that result in both positions. it would
2016 : // be faster and more numerically stable to average the KHP entries
2017 : // instead, but we have no good proof P was symmetric before!
2018 4555725627 : const ftype lower = P[r][c] - KHP[r][c];
2019 4555725627 : const ftype upper = P[c][r] - KHP[c][r];
2020 4555725627 : const ftype res = 0.5f*(lower + upper);
2021 4555725627 : P[r][c] = res;
2022 4555725627 : P[c][r] = res;
2023 : }
2024 : }
2025 :
2026 : // limit the variances to prevent ill-conditioning
2027 19806902 : ConstrainVariances(); // can change statesArray!!
2028 :
2029 19806902 : return false;
2030 : }
2031 :
2032 : // constrain states using WMM tables and specified limit
2033 22890049 : void NavEKF3_core::MagTableConstrain(void)
2034 : {
2035 : // constrain to error from table earth field
2036 22890049 : ftype limit_ga = frontend->_mag_ef_limit * 0.001f;
2037 22890049 : stateStruct.earth_magfield.x = constrain_ftype(stateStruct.earth_magfield.x,
2038 : table_earth_field_ga.x-limit_ga,
2039 : table_earth_field_ga.x+limit_ga);
2040 22890049 : stateStruct.earth_magfield.y = constrain_ftype(stateStruct.earth_magfield.y,
2041 : table_earth_field_ga.y-limit_ga,
2042 : table_earth_field_ga.y+limit_ga);
2043 22890049 : stateStruct.earth_magfield.z = constrain_ftype(stateStruct.earth_magfield.z,
2044 : table_earth_field_ga.z-limit_ga,
2045 : table_earth_field_ga.z+limit_ga);
2046 22890049 : }
2047 :
2048 : // constrain states to prevent ill-conditioning
2049 19965274 : void NavEKF3_core::ConstrainStates()
2050 : {
2051 : // State constraints as of March 2025
2052 : // This table documents the limits applied to each EKF state.
2053 : // These are designed to keep state estimates within physically realistic bounds and prevent divergence.
2054 : // +---------------------------------------------------------------------------------------------------------+
2055 : // | State Index | State Name | State Units | State Constraint Range |
2056 : // +---------------------------------------------------------------------------------------------------------+
2057 : // | 0 .. 3 | Attitude Quaternion | unitless | [-1.0, 1.0] |
2058 : // | 4 .. 6 | Velocity (North, East, Down) | m/s | [-500, 500] |
2059 : // | 7 .. 8 | Position (North, East) | m | [-50e6,50e6] |
2060 : // | 9 (z) | Position (Down / Altitude) | m | [-40000, 10000] |
2061 : // | 10 .. 12 | Gyro Bias (X, Y, Z) | rad | [-0.5, 0.5] * dtEkfAvg |
2062 : // | 13 .. 15 | Accel Bias (X, Y, Z) | m/s² | [-_accBiasLim, _accBiasLim] * dtEkfAvg |
2063 : // | 16 .. 18 | Earth Magnetic Field (X, Y, Z) | Gauss | [-1.0, 1.0] | or constrained by MagTableConstrain() if available
2064 : // | 19 .. 21 | Body Magnetic Field (X, Y, Z) | Gauss | [-0.5, 0.5] |
2065 : // | 22 .. 23 | Wind Velocity (North, East) | m/s | [-100, 100] |
2066 : // +---------------------------------------------------------------------------------------------------------+
2067 :
2068 : // quaternions are limited between +-1
2069 99826370 : for (uint8_t i=0; i<=3; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f);
2070 : // velocity limit 500 m/sec (could set this based on some multiple of max airspeed * EAS2TAS)
2071 79861096 : for (uint8_t i=4; i<=6; i++) statesArray[i] = constrain_ftype(statesArray[i],-5.0e2f,5.0e2f);
2072 : // position limit TODO apply circular limit
2073 59895822 : for (uint8_t i=7; i<=8; i++) statesArray[i] = constrain_ftype(statesArray[i],-EK3_POSXY_STATE_LIMIT,EK3_POSXY_STATE_LIMIT);
2074 : // height limit covers home alt on everest through to home alt at SL and balloon drop
2075 19965274 : stateStruct.position.z = constrain_ftype(stateStruct.position.z,-4.0e4f,1.0e4f);
2076 : // gyro bias limit (this needs to be set based on manufacturers specs)
2077 19965274 : const ftype gyro_bias_limit = getGyroBiasLimit();
2078 79861096 : for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_ftype(statesArray[i],-gyro_bias_limit*dtEkfAvg,gyro_bias_limit*dtEkfAvg);
2079 : // the accelerometer bias limit is controlled by a user adjustable parameter
2080 79861096 : for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_ftype(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg);
2081 : // earth magnetic field limit
2082 19965274 : if (frontend->_mag_ef_limit <= 0 || !have_table_earth_field) {
2083 : // constrain to +/-1Ga
2084 8270760 : for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_ftype(statesArray[i],-1.0f,1.0f);
2085 : } else {
2086 : // use table constrain
2087 17897584 : MagTableConstrain();
2088 : }
2089 : // body magnetic field limit
2090 79861096 : for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_ftype(statesArray[i],-0.5f,0.5f);
2091 : // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit
2092 59895822 : for (uint8_t i=22; i<=23; i++) statesArray[i] = constrain_ftype(statesArray[i],-100.0f,100.0f);
2093 : // constrain the terrain state to be below the vehicle height unless we are using terrain as the height datum
2094 19965274 : if (!inhibitGndState) {
2095 19953064 : terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
2096 : }
2097 19965274 : }
2098 :
2099 : // calculate the NED earth spin vector in rad/sec
2100 4878 : void NavEKF3_core::calcEarthRateNED(Vector3F &omega, int32_t latitude) const
2101 : {
2102 4878 : ftype lat_rad = radians(latitude*1.0e-7f);
2103 4878 : omega.x = earthRate*cosF(lat_rad);
2104 4878 : omega.y = 0;
2105 4878 : omega.z = -earthRate*sinF(lat_rad);
2106 4878 : }
2107 :
2108 : // set yaw from a single magnetometer sample
2109 4182 : void NavEKF3_core::setYawFromMag()
2110 : {
2111 4182 : if (!use_compass()) {
2112 0 : return;
2113 : }
2114 :
2115 : // read the magnetometer data
2116 4182 : readMagData();
2117 :
2118 : // use best of either 312 or 321 rotation sequence when calculating yaw
2119 : rotationOrder order;
2120 4182 : bestRotationOrder(order);
2121 4182 : Vector3F eulerAngles;
2122 4182 : Matrix3F Tbn_zeroYaw;
2123 4182 : if (order == rotationOrder::TAIT_BRYAN_321) {
2124 : // rolled more than pitched so use 321 rotation order
2125 2988 : stateStruct.quat.to_euler(eulerAngles);
2126 2988 : Tbn_zeroYaw.from_euler(eulerAngles.x, eulerAngles.y, 0.0f);
2127 1194 : } else if (order == rotationOrder::TAIT_BRYAN_312) {
2128 : // pitched more than rolled so use 312 rotation order
2129 1194 : eulerAngles = stateStruct.quat.to_vector312();
2130 1194 : Tbn_zeroYaw.from_euler312(eulerAngles.x, eulerAngles.y, 0.0f);
2131 : } else {
2132 : // rotation order not supported
2133 : return;
2134 : }
2135 :
2136 4182 : Vector3F magMeasNED = Tbn_zeroYaw * magDataDelayed.mag;
2137 4182 : ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
2138 :
2139 : // update quaternion states and covariances
2140 4182 : resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)), order);
2141 : }
2142 :
2143 : // update mag field states and associated variances using magnetomer and declination data
2144 4205 : void NavEKF3_core::resetMagFieldStates()
2145 : {
2146 : // Rotate Mag measurements into NED to set initial NED magnetic field states
2147 :
2148 : // update rotation matrix from body to NED frame
2149 4205 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
2150 :
2151 4205 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
2152 1513 : stateStruct.earth_magfield = table_earth_field_ga;
2153 : } else {
2154 2692 : stateStruct.earth_magfield = prevTnb.transposed() * magDataDelayed.mag;
2155 : }
2156 :
2157 : // set the NE earth magnetic field states using the published declination
2158 : // and set the corresponding variances and covariances
2159 4205 : alignMagStateDeclination();
2160 :
2161 : // set the remaining variances and covariances
2162 4205 : zeroStatesVarCov(18, 21);
2163 4205 : P[18][18] = sq(frontend->_magNoise);
2164 4205 : P[19][19] = P[18][18];
2165 4205 : P[20][20] = P[18][18];
2166 4205 : P[21][21] = P[18][18];
2167 :
2168 : // record the fact we have initialised the magnetic field states
2169 4205 : recordMagReset();
2170 4205 : }
2171 :
2172 : // calculate the tilt error variance
2173 19972655 : void NavEKF3_core::calcTiltErrorVariance()
2174 : {
2175 19972655 : const ftype &q0 = stateStruct.quat[0];
2176 19972655 : const ftype &q1 = stateStruct.quat[1];
2177 19972655 : const ftype &q2 = stateStruct.quat[2];
2178 19972655 : const ftype &q3 = stateStruct.quat[3];
2179 :
2180 : // equations generated by quaternion_error_propagation(): in derivation/generate_2.py
2181 : // only diagonals have been used
2182 19972655 : const ftype PS0 = q0*q1 + q2*q3;
2183 19972655 : const ftype PS1 = PS0*q1;
2184 19972655 : const ftype PS2 = sq(q0) - sq(q1) - sq(q2) + sq(q3);
2185 19972655 : const ftype PS3 = PS2*q0;
2186 19972655 : const ftype PS4 = PS0*q2;
2187 19972655 : const ftype PS5 = PS2*q3;
2188 19972655 : const ftype PS6 = PS0*q3;
2189 19972655 : const ftype PS7 = PS2*q2;
2190 19972655 : const ftype PS8 = PS0*q0;
2191 19972655 : const ftype PS9 = PS2*q1;
2192 19972655 : const ftype PS10 = q0*q2 - q1*q3;
2193 19972655 : const ftype PS11 = PS10*q2;
2194 19972655 : const ftype PS12 = PS10*q3;
2195 19972655 : const ftype PS13 = PS10*q0;
2196 19972655 : const ftype PS14 = PS10*q1;
2197 :
2198 19972655 : tiltErrorVariance = 4*P[0][0]*sq(2*PS8 - PS9) + 4*P[1][1]*sq(2*PS1 + PS3) + 4*P[2][2]*sq(2*PS4 + PS5) + 4*P[3][3]*sq(-2*PS6 + PS7);
2199 19972655 : tiltErrorVariance += 4*P[0][0]*sq(2*PS13 - PS7) + 4*P[1][1]*sq(2*PS14 - PS5) + 4*P[2][2]*sq(2*PS11 + PS3) + 4*P[3][3]*sq(2*PS12 + PS9);
2200 19972655 : tiltErrorVariance += 16*P[0][0]*sq(PS14 - PS4) + 16*P[1][1]*sq(PS13 + PS6) + 16*P[2][2]*sq(-PS12 + PS8) + 16*P[3][3]*sq(PS1 + PS11);
2201 :
2202 19972655 : tiltErrorVariance = constrain_ftype(tiltErrorVariance, 0.0f, sq(radians(30.0f)));
2203 19972655 : }
2204 :
2205 4210 : void NavEKF3_core::bestRotationOrder(rotationOrder &order)
2206 : {
2207 4210 : if (fabsF(prevTnb[2][0]) < fabsF(prevTnb[2][1])) {
2208 : // rolled more than pitched so use 321 sequence
2209 2990 : order = rotationOrder::TAIT_BRYAN_321;
2210 : } else {
2211 : // pitched more than rolled so use 312 sequence
2212 1220 : order = rotationOrder::TAIT_BRYAN_312;
2213 : }
2214 4210 : }
2215 :
2216 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
2217 : // calculate the tilt error variance using an alternative numerical difference technique
2218 : // and log with value generated by NavEKF3_core::calcTiltErrorVariance()
2219 19965274 : void NavEKF3_core::verifyTiltErrorVariance()
2220 : {
2221 : #if HAL_LOGGING_ENABLED
2222 19965274 : const Vector3f gravity_ef = Vector3f(0.0f,0.0f,1.0f);
2223 19965274 : Matrix3f Tnb;
2224 : const float quat_delta = 0.001f;
2225 19965274 : float tiltErrorVarianceAlt = 0.0f;
2226 99826370 : for (uint8_t index = 0; index<4; index++) {
2227 79861096 : QuaternionF quat = stateStruct.quat;
2228 :
2229 : // Add a positive increment to the quaternion element and calculate the tilt error vector
2230 79861096 : quat[index] = stateStruct.quat[index] + quat_delta;
2231 79861096 : quat.inverse().rotation_matrix(Tnb);
2232 79861096 : const Vector3f gravity_bf_plus = Tnb * gravity_ef;
2233 :
2234 : // Add a negative increment to the quaternion element and calculate the tilt error vector
2235 79861096 : quat[index] = stateStruct.quat[index] - quat_delta;
2236 79861096 : quat.inverse().rotation_matrix(Tnb);
2237 79861096 : const Vector3f gravity_bf_minus = Tnb * gravity_ef;
2238 :
2239 : // calculate the angular difference between the two vectors using a small angle assumption
2240 79861096 : const Vector3f tilt_diff_vec = gravity_bf_minus % gravity_bf_plus;
2241 :
2242 : // calculate the partial derivative of angle error wrt the quaternion element
2243 79861096 : const float tilt_error_derivative = tilt_diff_vec.length() / (2.0f * quat_delta);
2244 :
2245 : // sum the contribution of the quaternion elemnent variance to the tilt angle error variance
2246 79861096 : tiltErrorVarianceAlt += P[index][index] * sq(tilt_error_derivative);
2247 : }
2248 :
2249 19965274 : tiltErrorVarianceAlt = MIN(tiltErrorVarianceAlt, sq(radians(30.0f)));
2250 19965274 : if (imuSampleTime_ms - lastLogTime_ms > 500) {
2251 551396 : lastLogTime_ms = imuSampleTime_ms;
2252 551396 : const struct log_XKTV msg {
2253 : LOG_PACKET_HEADER_INIT(LOG_XKTV_MSG),
2254 551396 : time_us : dal.micros64(),
2255 551396 : core : core_index,
2256 551396 : tvs : float(tiltErrorVariance),
2257 : tvd : float(tiltErrorVarianceAlt),
2258 551396 : };
2259 551396 : AP::logger().WriteBlock(&msg, sizeof(msg));
2260 : }
2261 : #endif // HAL_LOGGING_ENABLED
2262 19965274 : }
2263 : #endif
2264 :
2265 : /*
2266 : move the EKF origin to the current position at 1Hz. The public_origin doesn't move.
2267 : By moving the EKF origin we keep the distortion due to spherical
2268 : shape of the earth to a minimum.
2269 : */
2270 280584 : void NavEKF3_core::moveEKFOrigin(void)
2271 : {
2272 : // only move origin when we have a origin and we're using GPS
2273 280584 : if (!frontend->common_origin_valid || !filterStatus.flags.using_gps) {
2274 64806 : return;
2275 : }
2276 :
2277 : // move the origin to the current state location
2278 215778 : Location loc = EKF_origin;
2279 215778 : loc.offset(stateStruct.position.x, stateStruct.position.y);
2280 215778 : const Vector2F diffNE = loc.get_distance_NE_ftype(EKF_origin);
2281 215778 : EKF_origin = loc;
2282 :
2283 : // now fix all output states
2284 215778 : stateStruct.position.xy() += diffNE;
2285 215778 : outputDataNew.position.xy() += diffNE;
2286 215778 : outputDataDelayed.position.xy() += diffNE;
2287 :
2288 2166343 : for (unsigned index=0; index < imu_buffer_length; index++) {
2289 1950565 : storedOutput[index].position.xy() += diffNE;
2290 : }
2291 : }
|
