Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF2.h"
4 : #include "AP_NavEKF2_core.h"
5 : #include <GCS_MAVLink/GCS.h>
6 :
7 : extern const AP_HAL::HAL& hal;
8 :
9 : /********************************************************
10 : * RESET FUNCTIONS *
11 : ********************************************************/
12 :
13 : // Control reset of yaw and magnetic field states
14 607858 : void NavEKF2_core::controlMagYawReset()
15 : {
16 :
17 : // Vehicles that can use a zero sideslip assumption (Planes) are a special case
18 : // They can use the GPS velocity to recover from bad initial compass data
19 : // This allows recovery for heading alignment errors due to compass faults
20 607858 : if (assume_zero_sideslip() && !finalInflightYawInit && inFlight ) {
21 416036 : gpsYawResetRequest = true;
22 416036 : return;
23 : } else {
24 191822 : gpsYawResetRequest = false;
25 : }
26 :
27 : // QuaternionF and delta rotation vector that are re-used for different calculations
28 191822 : Vector3F deltaRotVecTemp;
29 191822 : QuaternionF deltaQuatTemp;
30 :
31 : bool flightResetAllowed = false;
32 : bool initialResetAllowed = false;
33 191822 : if (!finalInflightYawInit) {
34 : // Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time
35 168284 : deltaQuatTemp = stateStruct.quat / prevQuatMagReset;
36 168284 : prevQuatMagReset = stateStruct.quat;
37 :
38 : // convert the quaternion to a rotation vector and find its length
39 168284 : deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
40 :
41 : // check if the spin rate is OK - high spin rates can cause angular alignment errors
42 168284 : bool angRateOK = deltaRotVecTemp.length() < 0.1745f;
43 :
44 : initialResetAllowed = angRateOK;
45 168284 : flightResetAllowed = angRateOK && !onGround;
46 :
47 : }
48 :
49 : // reset the limit on the number of magnetic anomaly resets for each takeoff
50 191822 : if (onGround) {
51 163108 : magYawAnomallyCount = 0;
52 : }
53 :
54 : // Check if conditions for a interim or final yaw/mag reset are met
55 : bool finalResetRequest = false;
56 : bool interimResetRequest = false;
57 191822 : if (flightResetAllowed && !assume_zero_sideslip()) {
58 : // check that we have reached a height where ground magnetic interference effects are insignificant
59 : // and can perform a final reset of the yaw and field states
60 4398 : finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF2_MAG_FINAL_RESET_ALT;
61 :
62 : // check for increasing height
63 4398 : bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
64 4398 : ftype yawInnovIncrease = fabsF(innovYaw) - fabsF(yawInnovAtLastMagReset);
65 :
66 : // check for increasing yaw innovations
67 4398 : bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
68 :
69 : // check that the yaw innovations haven't been caused by a large change in attitude
70 4398 : deltaQuatTemp = quatAtLastMagReset / stateStruct.quat;
71 4398 : deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
72 4398 : bool largeAngleChange = deltaRotVecTemp.length() > yawInnovIncrease;
73 :
74 : // if yaw innovations and height have increased and we haven't rotated much
75 : // then we are climbing away from a ground based magnetic anomaly and need to reset
76 4398 : interimResetRequest = !finalInflightYawInit
77 4398 : && !finalResetRequest
78 4380 : && (magYawAnomallyCount < MAG_ANOMALY_RESET_MAX)
79 : && hgtIncreasing
80 4380 : && yawInnovIncreasing
81 4398 : && !largeAngleChange;
82 : }
83 :
84 : // an initial reset is required if we have not yet aligned the yaw angle
85 191822 : bool initialResetRequest = initialResetAllowed && !yawAlignComplete;
86 :
87 : // a combined yaw angle and magnetic field reset can be initiated by:
88 383644 : magYawResetRequest = magYawResetRequest || // an external request
89 191564 : initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer
90 382830 : interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly
91 : finalResetRequest; // the final reset when we have achieved enough height to be in stable magnetic field environment
92 :
93 : // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
94 191822 : if (magYawResetRequest || magStateResetRequest || extNavYawResetRequest) {
95 :
96 : // if a yaw reset has been requested, apply the updated quaternion to the current state
97 850 : if (extNavYawResetRequest) {
98 : // get the euler angles from the current state estimate
99 0 : Vector3F eulerAnglesOld;
100 0 : stateStruct.quat.to_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesOld.z);
101 :
102 : // previous value used to calculate a reset delta
103 0 : QuaternionF prevQuat = stateStruct.quat;
104 :
105 : // Get the Euler angles from the external vision data
106 0 : Vector3F eulerAnglesNew;
107 0 : extNavDataDelayed.quat.to_euler(eulerAnglesNew.x, eulerAnglesNew.y, eulerAnglesNew.z);
108 :
109 : // the new quaternion uses the old roll/pitch and new yaw angle
110 0 : stateStruct.quat.from_euler(eulerAnglesOld.x, eulerAnglesOld.y, eulerAnglesNew.z);
111 :
112 : // calculate the change in the quaternion state and apply it to the output history buffer
113 0 : prevQuat = stateStruct.quat/prevQuat;
114 0 : StoreQuatRotate(prevQuat);
115 :
116 : // send initial alignment status to console
117 0 : if (!yawAlignComplete) {
118 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u ext nav yaw alignment complete",(unsigned)imu_index);
119 : }
120 :
121 : // record the reset as complete and also record the in-flight reset as complete to stop further resets when height is gained
122 : // in-flight reset is unnecessary because we do not need to consider ground based magnetic anomaly effects
123 0 : yawAlignComplete = true;
124 0 : finalInflightYawInit = true;
125 :
126 : // clear the yaw reset request flag
127 0 : extNavYawResetRequest = false;
128 :
129 850 : } else if (magYawResetRequest || magStateResetRequest) {
130 : // get the euler angles from the current state estimate
131 850 : Vector3F eulerAngles;
132 850 : stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
133 :
134 : // Use the Euler angles and magnetometer measurement to update the magnetic field states
135 : // and get an updated quaternion
136 850 : QuaternionF newQuat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
137 :
138 850 : if (magYawResetRequest) {
139 : // previous value used to calculate a reset delta
140 832 : QuaternionF prevQuat = stateStruct.quat;
141 :
142 : // update the quaternion states using the new yaw angle
143 832 : stateStruct.quat = newQuat;
144 :
145 : // calculate the change in the quaternion state and apply it to the ouput history buffer
146 832 : prevQuat = stateStruct.quat/prevQuat;
147 832 : StoreQuatRotate(prevQuat);
148 :
149 : // send initial alignment status to console
150 832 : if (!yawAlignComplete) {
151 556 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u MAG%u initial yaw alignment complete",(unsigned)imu_index, (unsigned)magSelectIndex);
152 : }
153 :
154 : // send in-flight yaw alignment status to console
155 832 : if (finalResetRequest) {
156 18 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u MAG%u in-flight yaw alignment complete",(unsigned)imu_index, (unsigned)magSelectIndex);
157 814 : } else if (interimResetRequest) {
158 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u MAG%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index, (unsigned)magSelectIndex);
159 : }
160 :
161 : // update the yaw reset completed status
162 832 : recordYawReset();
163 :
164 : // clear the yaw reset request flag
165 832 : magYawResetRequest = false;
166 :
167 : // clear the complete flags if an interim reset has been performed to allow subsequent
168 : // and final reset to occur
169 832 : if (interimResetRequest) {
170 0 : finalInflightYawInit = false;
171 0 : finalInflightMagInit = false;
172 : }
173 : }
174 : }
175 : }
176 : }
177 :
178 : // this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
179 : // vector from GPS. It is used to align the yaw angle after launch or takeoff.
180 2080921 : void NavEKF2_core::realignYawGPS()
181 : {
182 2080921 : if ((sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y)) > 25.0f) {
183 : // get quaternion from existing filter states and calculate roll, pitch and yaw angles
184 2070901 : Vector3F eulerAngles;
185 2070901 : stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
186 :
187 : // calculate course yaw angle
188 2070901 : ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x);
189 :
190 : // calculate course yaw angle from GPS velocity
191 2070901 : ftype gpsYaw = atan2F(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
192 :
193 : // Check the yaw angles for consistency
194 2070901 : ftype yawErr = MAX(fabsF(wrap_PI(gpsYaw - velYaw)),fabsF(wrap_PI(gpsYaw - eulerAngles.z)));
195 :
196 : // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad
197 2070901 : bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
198 :
199 : // correct yaw angle using GPS ground course if compass yaw bad
200 : if (badMagYaw) {
201 :
202 : // calculate new filter quaternion states from Euler angles
203 10 : stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, gpsYaw);
204 : // reset the velocity and position states as they will be inaccurate due to bad yaw
205 10 : ResetVelocity();
206 10 : ResetPosition();
207 :
208 : // send yaw alignment information to console
209 10 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
210 :
211 : // zero the attitude covariances because the correlations will now be invalid
212 10 : zeroAttCovOnly();
213 :
214 : // record the yaw reset event
215 10 : recordYawReset();
216 :
217 : // clear all pending yaw reset requests
218 10 : gpsYawResetRequest = false;
219 10 : magYawResetRequest = false;
220 :
221 10 : if (use_compass()) {
222 : // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
223 8 : magStateResetRequest = true;
224 : // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
225 8 : allMagSensorsFailed = false;
226 : }
227 : }
228 : }
229 2080921 : }
230 :
231 : /********************************************************
232 : * FUSE MEASURED_DATA *
233 : ********************************************************/
234 :
235 : // select fusion of magnetometer data
236 3248540 : void NavEKF2_core::SelectMagFusion()
237 : {
238 : // clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
239 : // used for load levelling
240 3248540 : magFusePerformed = false;
241 :
242 : // Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
243 : // flight using the output from the GSF yaw estimator.
244 3248540 : if (!use_compass() && tiltAlignComplete) {
245 37966 : if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastYawTime_ms > 140)) {
246 1666 : fuseEulerYaw();
247 : }
248 37966 : if (yawAlignComplete) {
249 : return;
250 : }
251 760 : yawAlignComplete = EKFGSF_resetMainFilterYaw();
252 760 : return;
253 : }
254 :
255 : // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
256 3210574 : if (magHealth) {
257 3197008 : magTimeout = false;
258 3197008 : lastHealthyMagTime_ms = imuSampleTime_ms;
259 13566 : } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) {
260 1482 : magTimeout = true;
261 : }
262 :
263 : // check for and read new magnetometer measurements
264 3210574 : readMagData();
265 :
266 : // check for availability of magnetometer data to fuse
267 3210574 : magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
268 :
269 : // Control reset of yaw and magnetic field states if we are using compass data
270 3210574 : if (magDataToFuse && use_compass()) {
271 607858 : controlMagYawReset();
272 : }
273 :
274 : // determine if conditions are right to start a new fusion cycle
275 : // wait until the EKF time horizon catches up with the measurement
276 3210574 : bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
277 : if (dataReady) {
278 : // use the simple method of declination to maintain heading if we cannot use the magnetic field states
279 607826 : if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
280 168862 : fuseEulerYaw();
281 : // zero the test ratio output from the inactive 3-axis magnetometer fusion
282 168862 : magTestRatio.zero();
283 : } else {
284 : // if we are not doing aiding with earth relative observations (eg GPS) then the declination is
285 : // maintained by fusing declination as a synthesised observation
286 : // We also fuse declination if we are using the WMM tables
287 438964 : if (PV_AidingMode != AID_ABSOLUTE ||
288 412263 : (frontend->_mag_ef_limit > 0 && have_table_earth_field)) {
289 438964 : FuseDeclination(0.34f);
290 : }
291 :
292 : // fuse the three magnetometer componenents using sequential fusion of each axis
293 438964 : FuseMagnetometer();
294 :
295 : // zero the test ratio output from the inactive simple magnetometer yaw fusion
296 438964 : yawTestRatio = 0.0f;
297 : }
298 : }
299 :
300 : // If the final yaw reset has been performed and the state variances are sufficiently low
301 : // record that the earth field has been learned.
302 3210574 : if (!magFieldLearned && finalInflightMagInit) {
303 214372 : magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
304 : }
305 :
306 : // record the last learned field variances
307 3210574 : if (magFieldLearned && !inhibitMagStates) {
308 72513 : earthMagFieldVar.x = P[16][16];
309 72513 : earthMagFieldVar.y = P[17][17];
310 72513 : earthMagFieldVar.z = P[18][18];
311 72513 : bodyMagFieldVar.x = P[19][19];
312 72513 : bodyMagFieldVar.y = P[20][20];
313 72513 : bodyMagFieldVar.z = P[21][21];
314 : }
315 : }
316 :
317 : /*
318 : * Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
319 : * The script file used to generate these and other equations in this filter can be found here:
320 : * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
321 : */
322 438964 : void NavEKF2_core::FuseMagnetometer()
323 : {
324 : // declarations
325 : ftype &q0 = mag_state.q0;
326 : ftype &q1 = mag_state.q1;
327 : ftype &q2 = mag_state.q2;
328 : ftype &q3 = mag_state.q3;
329 : ftype &magN = mag_state.magN;
330 : ftype &magE = mag_state.magE;
331 : ftype &magD = mag_state.magD;
332 : ftype &magXbias = mag_state.magXbias;
333 : ftype &magYbias = mag_state.magYbias;
334 : ftype &magZbias = mag_state.magZbias;
335 438964 : Matrix3F &DCM = mag_state.DCM;
336 438964 : Vector3F &MagPred = mag_state.MagPred;
337 : ftype &R_MAG = mag_state.R_MAG;
338 : ftype *SH_MAG = &mag_state.SH_MAG[0];
339 : Vector24 H_MAG;
340 : Vector6 SK_MX;
341 : Vector6 SK_MY;
342 : Vector6 SK_MZ;
343 :
344 438964 : q0 = stateStruct.quat[0];
345 438964 : q1 = stateStruct.quat[1];
346 438964 : q2 = stateStruct.quat[2];
347 438964 : q3 = stateStruct.quat[3];
348 438964 : magN = stateStruct.earth_magfield[0];
349 438964 : magE = stateStruct.earth_magfield[1];
350 438964 : magD = stateStruct.earth_magfield[2];
351 438964 : magXbias = stateStruct.body_magfield[0];
352 438964 : magYbias = stateStruct.body_magfield[1];
353 438964 : magZbias = stateStruct.body_magfield[2];
354 :
355 : // rotate predicted earth components into body axes and calculate
356 : // predicted measurements
357 438964 : DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
358 438964 : DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
359 438964 : DCM[0][2] = 2.0f*(q1*q3-q0*q2);
360 438964 : DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
361 438964 : DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
362 438964 : DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
363 438964 : DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
364 438964 : DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
365 438964 : DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
366 438964 : MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
367 438964 : MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
368 438964 : MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
369 :
370 : // calculate the measurement innovation for each axis
371 1755856 : for (uint8_t i = 0; i<=2; i++) {
372 1316892 : innovMag[i] = MagPred[i] - magDataDelayed.mag[i];
373 : }
374 :
375 : // scale magnetometer observation error with total angular rate to allow for timing errors
376 438964 : R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT);
377 :
378 : // calculate common expressions used to calculate observation jacobians an innovation variance for each component
379 438964 : SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
380 438964 : SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
381 438964 : SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
382 438964 : SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
383 438964 : SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
384 438964 : SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
385 438964 : SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
386 438964 : SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
387 438964 : SH_MAG[8] = 2.0f*q0*q3;
388 :
389 : // Calculate the innovation variance for each axis
390 : // X axis
391 438964 : varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
392 438964 : if (varInnovMag[0] >= R_MAG) {
393 438964 : faultStatus.bad_xmag = false;
394 : } else {
395 : // the calculation is badly conditioned, so we cannot perform fusion on this step
396 : // we reset the covariance matrix and try again next measurement
397 0 : CovarianceInit();
398 0 : faultStatus.bad_xmag = true;
399 1289 : return;
400 : }
401 :
402 : // Y axis
403 438964 : varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
404 438964 : if (varInnovMag[1] >= R_MAG) {
405 438964 : faultStatus.bad_ymag = false;
406 : } else {
407 : // the calculation is badly conditioned, so we cannot perform fusion on this step
408 : // we reset the covariance matrix and try again next measurement
409 0 : CovarianceInit();
410 0 : faultStatus.bad_ymag = true;
411 0 : return;
412 : }
413 :
414 : // Z axis
415 438964 : varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
416 438964 : if (varInnovMag[2] >= R_MAG) {
417 438964 : faultStatus.bad_zmag = false;
418 : } else {
419 : // the calculation is badly conditioned, so we cannot perform fusion on this step
420 : // we reset the covariance matrix and try again next measurement
421 0 : CovarianceInit();
422 0 : faultStatus.bad_zmag = true;
423 0 : return;
424 : }
425 :
426 : // calculate the innovation test ratios
427 1755856 : for (uint8_t i = 0; i<=2; i++) {
428 1316892 : magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
429 : }
430 :
431 : // check the last values from all components and set magnetometer health accordingly
432 438964 : magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
433 :
434 : // if the magnetometer is unhealthy, do not proceed further
435 438964 : if (!magHealth) {
436 : return;
437 : }
438 :
439 : // perform sequential fusion of magnetometer measurements.
440 : // this assumes that the errors in the different components are
441 : // uncorrelated which is not true, however in the absence of covariance
442 : // data fit is the only assumption we can make
443 : // so we might as well take advantage of the computational efficiencies
444 : // associated with sequential fusion
445 : // calculate observation jacobians and Kalman gains
446 1750700 : for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) {
447 1313025 : if (obsIndex == 0)
448 : {
449 : // calculate observation jacobians
450 437675 : ZERO_FARRAY(H_MAG);
451 437675 : H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
452 437675 : H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
453 437675 : H_MAG[16] = SH_MAG[1];
454 437675 : H_MAG[17] = SH_MAG[4];
455 437675 : H_MAG[18] = SH_MAG[7];
456 437675 : H_MAG[19] = 1.0f;
457 :
458 : // calculate Kalman gain
459 437675 : SK_MX[0] = 1.0f / varInnovMag[0];
460 437675 : SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
461 437675 : SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
462 437675 : SK_MX[3] = SH_MAG[7];
463 437675 : Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
464 437675 : Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
465 437675 : Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
466 437675 : Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
467 437675 : Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
468 437675 : Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
469 437675 : Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
470 437675 : Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
471 437675 : Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
472 437675 : Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
473 437675 : Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
474 437675 : Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
475 437675 : Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
476 437675 : Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
477 437675 : Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
478 437675 : Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
479 :
480 : // zero Kalman gains to inhibit wind state estimation
481 437675 : if (!inhibitWindStates) {
482 399810 : Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
483 399810 : Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
484 : } else {
485 37865 : Kfusion[22] = 0.0f;
486 37865 : Kfusion[23] = 0.0f;
487 : }
488 : // zero Kalman gains to inhibit magnetic field state estimation
489 437675 : if (!inhibitMagStates) {
490 437675 : Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
491 437675 : Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
492 437675 : Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
493 437675 : Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
494 437675 : Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
495 437675 : Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
496 : } else {
497 0 : for (uint8_t i=16; i<=21; i++) {
498 0 : Kfusion[i] = 0.0f;
499 : }
500 : }
501 :
502 : // set flags to indicate to other processes that fusion has been performed
503 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
504 437675 : magFusePerformed = true;
505 :
506 : }
507 875350 : else if (obsIndex == 1) // we are now fusing the Y measurement
508 : {
509 : // calculate observation jacobians
510 437675 : ZERO_FARRAY(H_MAG);
511 437675 : H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
512 437675 : H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
513 437675 : H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8];
514 437675 : H_MAG[17] = SH_MAG[0];
515 437675 : H_MAG[18] = SH_MAG[3];
516 437675 : H_MAG[20] = 1.0f;
517 :
518 : // calculate Kalman gain
519 437675 : SK_MY[0] = 1.0f / varInnovMag[1];
520 437675 : SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
521 437675 : SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
522 437675 : SK_MY[3] = SH_MAG[8] - 2.0f*q1*q2;
523 437675 : Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
524 437675 : Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
525 437675 : Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
526 437675 : Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
527 437675 : Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
528 437675 : Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
529 437675 : Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
530 437675 : Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
531 437675 : Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
532 437675 : Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
533 437675 : Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
534 437675 : Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
535 437675 : Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
536 437675 : Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
537 437675 : Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
538 437675 : Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
539 : // zero Kalman gains to inhibit wind state estimation
540 437675 : if (!inhibitWindStates) {
541 399810 : Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
542 399810 : Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
543 : } else {
544 37865 : Kfusion[22] = 0.0f;
545 37865 : Kfusion[23] = 0.0f;
546 : }
547 : // zero Kalman gains to inhibit magnetic field state estimation
548 437675 : if (!inhibitMagStates) {
549 437675 : Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
550 437675 : Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
551 437675 : Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
552 437675 : Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
553 437675 : Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
554 437675 : Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
555 : } else {
556 0 : for (uint8_t i=16; i<=21; i++) {
557 0 : Kfusion[i] = 0.0f;
558 : }
559 : }
560 :
561 : // set flags to indicate to other processes that fusion has been performed
562 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
563 437675 : magFusePerformed = true;
564 :
565 : }
566 : else if (obsIndex == 2) // we are now fusing the Z measurement
567 : {
568 : // calculate observation jacobians
569 437675 : ZERO_FARRAY(H_MAG);
570 437675 : H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
571 437675 : H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
572 437675 : H_MAG[16] = SH_MAG[5];
573 437675 : H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
574 437675 : H_MAG[18] = SH_MAG[2];
575 437675 : H_MAG[21] = 1.0f;
576 :
577 : // calculate Kalman gain
578 437675 : SK_MZ[0] = 1.0f / varInnovMag[2];
579 437675 : SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
580 437675 : SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
581 437675 : SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
582 437675 : Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
583 437675 : Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
584 437675 : Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
585 437675 : Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
586 437675 : Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
587 437675 : Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
588 437675 : Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
589 437675 : Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
590 437675 : Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
591 437675 : Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
592 437675 : Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
593 437675 : Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
594 437675 : Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
595 437675 : Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
596 437675 : Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
597 437675 : Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
598 : // zero Kalman gains to inhibit wind state estimation
599 437675 : if (!inhibitWindStates) {
600 399810 : Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
601 399810 : Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
602 : } else {
603 37865 : Kfusion[22] = 0.0f;
604 37865 : Kfusion[23] = 0.0f;
605 : }
606 : // zero Kalman gains to inhibit magnetic field state estimation
607 437675 : if (!inhibitMagStates) {
608 437675 : Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
609 437675 : Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
610 437675 : Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
611 437675 : Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
612 437675 : Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
613 437675 : Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
614 : } else {
615 0 : for (uint8_t i=16; i<=21; i++) {
616 0 : Kfusion[i] = 0.0f;
617 : }
618 : }
619 :
620 : // set flags to indicate to other processes that fusion has been performed
621 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
622 437675 : magFusePerformed = true;
623 :
624 : }
625 :
626 : // correct the covariance P = (I - K*H)*P
627 : // take advantage of the empty columns in KH to reduce the
628 : // number of operations
629 32598435 : for (unsigned i = 0; i<=stateIndexLim; i++) {
630 125141640 : for (unsigned j = 0; j<=2; j++) {
631 93856230 : KH[i][j] = Kfusion[i] * H_MAG[j];
632 : }
633 437995740 : for (unsigned j = 3; j<=15; j++) {
634 406710330 : KH[i][j] = 0.0f;
635 : }
636 218997870 : for (unsigned j = 16; j<=21; j++) {
637 187712460 : KH[i][j] = Kfusion[i] * H_MAG[j];
638 : }
639 93856230 : for (unsigned j = 22; j<=23; j++) {
640 62570820 : KH[i][j] = 0.0f;
641 : }
642 : }
643 32598435 : for (unsigned j = 0; j<=stateIndexLim; j++) {
644 777137070 : for (unsigned i = 0; i<=stateIndexLim; i++) {
645 : ftype res = 0;
646 745851660 : res += KH[i][0] * P[0][j];
647 745851660 : res += KH[i][1] * P[1][j];
648 745851660 : res += KH[i][2] * P[2][j];
649 745851660 : res += KH[i][16] * P[16][j];
650 745851660 : res += KH[i][17] * P[17][j];
651 745851660 : res += KH[i][18] * P[18][j];
652 745851660 : res += KH[i][19] * P[19][j];
653 745851660 : res += KH[i][20] * P[20][j];
654 745851660 : res += KH[i][21] * P[21][j];
655 745851660 : KHP[i][j] = res;
656 : }
657 : }
658 : // Check that we are not going to drive any variances negative and skip the update if so
659 : bool healthyFusion = true;
660 32598435 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
661 31285410 : if (KHP[i][i] > P[i][i]) {
662 : healthyFusion = false;
663 : }
664 : }
665 1313025 : if (healthyFusion) {
666 : // update the covariance matrix
667 32598435 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
668 777137070 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
669 745851660 : P[i][j] = P[i][j] - KHP[i][j];
670 : }
671 : }
672 :
673 : // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
674 1313025 : ForceSymmetry();
675 1313025 : ConstrainVariances();
676 :
677 : // update the states
678 : // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
679 1313025 : stateStruct.angErr.zero();
680 :
681 : // correct the state vector
682 32598435 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
683 31285410 : statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
684 : }
685 :
686 : // add table constraint here for faster convergence
687 1313025 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
688 1311957 : MagTableConstrain();
689 : }
690 :
691 : // the first 3 states represent the angular misalignment vector.
692 : // This is used to correct the estimated quaternion on the current time step
693 1313025 : stateStruct.quat.rotate(stateStruct.angErr);
694 :
695 : } else {
696 : // record bad axis
697 0 : if (obsIndex == 0) {
698 0 : faultStatus.bad_xmag = true;
699 0 : } else if (obsIndex == 1) {
700 0 : faultStatus.bad_ymag = true;
701 : } else if (obsIndex == 2) {
702 0 : faultStatus.bad_zmag = true;
703 : }
704 0 : CovarianceInit();
705 0 : return;
706 : }
707 : }
708 : }
709 :
710 :
711 : /*
712 : * Fuse magnetic heading measurement using explicit algebraic equations generated with Matlab symbolic toolbox.
713 : * The script file used to generate these and other equations in this filter can be found here:
714 : * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
715 : * This fusion method only modifies the orientation, does not require use of the magnetic field states and is computationally cheaper.
716 : * It is suitable for use when the external magnetic field environment is disturbed (eg close to metal structures, on ground).
717 : * It is not as robust to magnetometer failures.
718 : * It is not suitable for operation where the horizontal magnetic field strength is weak (within 30 degrees latitude of the magnetic poles)
719 : */
720 170528 : void NavEKF2_core::fuseEulerYaw()
721 : {
722 170528 : ftype q0 = stateStruct.quat[0];
723 170528 : ftype q1 = stateStruct.quat[1];
724 170528 : ftype q2 = stateStruct.quat[2];
725 170528 : ftype q3 = stateStruct.quat[3];
726 :
727 : // compass measurement error variance (rad^2) set to parameter value as a default
728 170528 : ftype R_YAW = sq(frontend->_yawNoise);
729 :
730 : // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
731 : // determine if a 321 or 312 Euler sequence is best
732 : ftype predicted_yaw;
733 : ftype measured_yaw;
734 : ftype H_YAW[3];
735 170528 : Matrix3F Tbn_zeroYaw;
736 :
737 170528 : if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) {
738 : // calculate observation jacobian when we are observing the first rotation in a 321 sequence
739 118138 : ftype t2 = q0*q0;
740 118138 : ftype t3 = q1*q1;
741 118138 : ftype t4 = q2*q2;
742 118138 : ftype t5 = q3*q3;
743 118138 : ftype t6 = t2+t3-t4-t5;
744 118138 : ftype t7 = q0*q3*2.0f;
745 118138 : ftype t8 = q1*q2*2.0f;
746 118138 : ftype t9 = t7+t8;
747 118138 : ftype t10 = sq(t6);
748 118138 : if (t10 > 1e-6f) {
749 117609 : t10 = 1.0f / t10;
750 : } else {
751 529 : return;
752 : }
753 117609 : ftype t11 = t9*t9;
754 117609 : ftype t12 = t10*t11;
755 117609 : ftype t13 = t12+1.0f;
756 : ftype t14;
757 117609 : if (fabsF(t13) > 1e-3f) {
758 117609 : t14 = 1.0f/t13;
759 : } else {
760 : return;
761 : }
762 117609 : ftype t15 = 1.0f/t6;
763 117609 : H_YAW[0] = 0.0f;
764 117609 : H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
765 117609 : H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
766 :
767 : // calculate predicted and measured yaw angle
768 117609 : Vector3F euler321;
769 117609 : stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
770 117609 : predicted_yaw = euler321.z;
771 117609 : if (use_compass() && yawAlignComplete && magStateInitComplete) {
772 : // Use measured mag components rotated into earth frame to measure yaw
773 116573 : Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
774 116573 : Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
775 116573 : measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
776 1036 : } else if (extNavUsedForYaw) {
777 : // Get the yaw angle from the external vision data
778 0 : R_YAW = sq(extNavDataDelayed.angErr);
779 0 : extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
780 0 : measured_yaw = euler321.z;
781 : } else {
782 1036 : if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
783 : ftype gsfYaw, gsfYawVariance, velInnovLength;
784 1894 : if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
785 858 : is_positive(gsfYawVariance) &&
786 2752 : gsfYawVariance < sq(radians(15.0f)) &&
787 1716 : (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
788 858 : measured_yaw = gsfYaw;
789 858 : R_YAW = gsfYawVariance;
790 : } else {
791 : // use predicted to prevent unconstrained variance growth
792 : measured_yaw = predicted_yaw;
793 : }
794 1036 : } else {
795 : // use predicted to prevent unconstrained variance growth
796 : measured_yaw = predicted_yaw;
797 : }
798 : }
799 : } else {
800 : // calculate observation jacobian when we are observing a rotation in a 312 sequence
801 52390 : ftype t2 = q0*q0;
802 52390 : ftype t3 = q1*q1;
803 52390 : ftype t4 = q2*q2;
804 52390 : ftype t5 = q3*q3;
805 52390 : ftype t6 = t2-t3+t4-t5;
806 52390 : ftype t7 = q0*q3*2.0f;
807 52390 : ftype t10 = q1*q2*2.0f;
808 52390 : ftype t8 = t7-t10;
809 52390 : ftype t9 = sq(t6);
810 52390 : if (t9 > 1e-6f) {
811 52362 : t9 = 1.0f/t9;
812 : } else {
813 28 : return;
814 : }
815 52362 : ftype t11 = t8*t8;
816 52362 : ftype t12 = t9*t11;
817 52362 : ftype t13 = t12+1.0f;
818 : ftype t14;
819 52362 : if (fabsF(t13) > 1e-3f) {
820 52362 : t14 = 1.0f/t13;
821 : } else {
822 : return;
823 : }
824 52362 : ftype t15 = 1.0f/t6;
825 52362 : H_YAW[0] = -t14*(t15*(q0*q2*2.0+q1*q3*2.0)-t8*t9*(q0*q1*2.0-q2*q3*2.0));
826 52362 : H_YAW[1] = 0.0f;
827 52362 : H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));
828 :
829 : // calculate predicted and measured yaw angle
830 52362 : Vector3F euler312 = stateStruct.quat.to_vector312();
831 : predicted_yaw = euler312.z;
832 52362 : if (use_compass() && yawAlignComplete && magStateInitComplete) {
833 : // Use measured mag components rotated into earth frame to measure yaw
834 51710 : Tbn_zeroYaw.from_euler312(euler312.x, euler312.y, 0.0f);
835 51710 : Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
836 51710 : measured_yaw = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
837 652 : } else if (extNavUsedForYaw) {
838 : // Get the yaw angle from the external vision data
839 0 : R_YAW = sq(extNavDataDelayed.angErr);
840 0 : euler312 = extNavDataDelayed.quat.to_vector312();
841 : measured_yaw = euler312.z;
842 : } else {
843 652 : if (imuSampleTime_ms - prevBetaStep_ms > 1000 && yawEstimator != nullptr) {
844 : ftype gsfYaw, gsfYawVariance, velInnovLength;
845 1094 : if (yawEstimator->getYawData(gsfYaw, gsfYawVariance) &&
846 466 : is_positive(gsfYawVariance) &&
847 1560 : gsfYawVariance < sq(radians(15.0f)) &&
848 932 : (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
849 466 : measured_yaw = gsfYaw;
850 466 : R_YAW = gsfYawVariance;
851 : } else {
852 : // use predicted to prevent unconstrained variance growth
853 : measured_yaw = predicted_yaw;
854 : }
855 628 : } else {
856 : // use predicted to prevent unconstrained variance growth
857 : measured_yaw = predicted_yaw;
858 : }
859 : }
860 : }
861 :
862 : // Calculate the innovation
863 169971 : ftype innovation = wrap_PI(predicted_yaw - measured_yaw);
864 :
865 : // Copy raw value to output variable used for data logging
866 169971 : innovYaw = innovation;
867 :
868 : // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
869 : ftype PH[3];
870 : ftype varInnov = R_YAW;
871 679884 : for (uint8_t rowIndex=0; rowIndex<=2; rowIndex++) {
872 509913 : PH[rowIndex] = 0.0f;
873 2039652 : for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
874 1529739 : PH[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
875 : }
876 509913 : varInnov += H_YAW[rowIndex]*PH[rowIndex];
877 : }
878 : ftype varInnovInv;
879 169971 : if (varInnov >= R_YAW) {
880 169971 : varInnovInv = 1.0f / varInnov;
881 : // output numerical health status
882 169971 : faultStatus.bad_yaw = false;
883 : } else {
884 : // the calculation is badly conditioned, so we cannot perform fusion on this step
885 : // we reset the covariance matrix and try again next measurement
886 0 : CovarianceInit();
887 : // output numerical health status
888 0 : faultStatus.bad_yaw = true;
889 0 : return;
890 : }
891 :
892 : // calculate Kalman gain
893 2904767 : for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
894 2734796 : Kfusion[rowIndex] = 0.0f;
895 10939184 : for (uint8_t colIndex=0; colIndex<=2; colIndex++) {
896 8204388 : Kfusion[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
897 : }
898 2734796 : Kfusion[rowIndex] *= varInnovInv;
899 : }
900 :
901 : // calculate the innovation test ratio
902 169971 : yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov);
903 :
904 : // Declare the magnetometer unhealthy if the innovation test fails
905 169971 : if (yawTestRatio > 1.0f) {
906 705 : magHealth = false;
907 : // On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects
908 : // If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data
909 705 : if (inFlight) {
910 : return;
911 : }
912 : } else {
913 169266 : magHealth = true;
914 : }
915 :
916 : // limit the innovation so that initial corrections are not too large
917 169671 : if (innovation > 0.5f) {
918 : innovation = 0.5f;
919 167618 : } else if (innovation < -0.5f) {
920 : innovation = -0.5f;
921 : }
922 :
923 : // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
924 : // calculate K*H*P
925 2897867 : for (uint8_t row = 0; row <= stateIndexLim; row++) {
926 10912784 : for (uint8_t column = 0; column <= 2; column++) {
927 8184588 : KH[row][column] = Kfusion[row] * H_YAW[column];
928 : }
929 : }
930 2897867 : for (uint8_t row = 0; row <= stateIndexLim; row++) {
931 46699900 : for (uint8_t column = 0; column <= stateIndexLim; column++) {
932 43971704 : ftype tmp = KH[row][0] * P[0][column];
933 43971704 : tmp += KH[row][1] * P[1][column];
934 43971704 : tmp += KH[row][2] * P[2][column];
935 43971704 : KHP[row][column] = tmp;
936 : }
937 : }
938 :
939 : // Check that we are not going to drive any variances negative and skip the update if so
940 : bool healthyFusion = true;
941 2897867 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
942 2728196 : if (KHP[i][i] > P[i][i]) {
943 : healthyFusion = false;
944 : }
945 : }
946 169671 : if (healthyFusion) {
947 : // update the covariance matrix
948 2897799 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
949 46698812 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
950 43970680 : P[i][j] = P[i][j] - KHP[i][j];
951 : }
952 : }
953 :
954 : // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
955 169667 : ForceSymmetry();
956 169667 : ConstrainVariances();
957 :
958 : // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
959 169667 : stateStruct.angErr.zero();
960 :
961 : // correct the state vector
962 2897799 : for (uint8_t i=0; i<=stateIndexLim; i++) {
963 2728132 : statesArray[i] -= Kfusion[i] * innovation;
964 : }
965 :
966 : // the first 3 states represent the angular misalignment vector.
967 : // This is used to correct the estimated quaternion on the current time step
968 169667 : stateStruct.quat.rotate(stateStruct.angErr);
969 :
970 : // record fusion event
971 169667 : faultStatus.bad_yaw = false;
972 169667 : lastYawTime_ms = imuSampleTime_ms;
973 :
974 :
975 : } else {
976 : // record fusion numerical health status
977 4 : faultStatus.bad_yaw = true;
978 : }
979 : }
980 :
981 : /*
982 : * Fuse declination angle using explicit algebraic equations generated with Matlab symbolic toolbox.
983 : * The script file used to generate these and other equations in this filter can be found here:
984 : * https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
985 : * This is used to prevent the declination of the EKF earth field states from drifting during operation without GPS
986 : * or some other absolute position or velocity reference
987 : */
988 439296 : void NavEKF2_core::FuseDeclination(ftype declErr)
989 : {
990 : // declination error variance (rad^2)
991 439296 : const ftype R_DECL = sq(declErr);
992 :
993 : // copy required states to local variables
994 439296 : ftype magN = stateStruct.earth_magfield.x;
995 439296 : ftype magE = stateStruct.earth_magfield.y;
996 :
997 : // prevent bad earth field states from causing numerical errors or exceptions
998 439296 : if (magN < 1e-3f) {
999 : return;
1000 : }
1001 :
1002 : // Calculate observation Jacobian and Kalman gains
1003 439296 : ftype t2 = magE*magE;
1004 439296 : ftype t3 = magN*magN;
1005 439296 : ftype t4 = t2+t3;
1006 439296 : ftype t5 = 1.0f/t4;
1007 439296 : ftype t22 = magE*t5;
1008 439296 : ftype t23 = magN*t5;
1009 439296 : ftype t6 = P[16][16]*t22;
1010 439296 : ftype t13 = P[17][16]*t23;
1011 439296 : ftype t7 = t6-t13;
1012 439296 : ftype t8 = t22*t7;
1013 439296 : ftype t9 = P[16][17]*t22;
1014 439296 : ftype t14 = P[17][17]*t23;
1015 439296 : ftype t10 = t9-t14;
1016 439296 : ftype t15 = t23*t10;
1017 439296 : ftype t11 = R_DECL+t8-t15; // innovation variance
1018 439296 : if (t11 < R_DECL) {
1019 : return;
1020 : }
1021 439296 : ftype t12 = 1.0f/t11;
1022 :
1023 : ftype H_MAG[24];
1024 :
1025 : H_MAG[16] = -magE*t5;
1026 : H_MAG[17] = magN*t5;
1027 :
1028 7468032 : for (uint8_t i=0; i<=15; i++) {
1029 7028736 : Kfusion[i] = -t12*(P[i][16]*t22-P[i][17]*t23);
1030 : }
1031 439296 : Kfusion[16] = -t12*(t6-P[16][17]*t23);
1032 439296 : Kfusion[17] = t12*(t14-P[17][16]*t22);
1033 3514368 : for (uint8_t i=17; i<=23; i++) {
1034 3075072 : Kfusion[i] = -t12*(P[i][16]*t22-P[i][17]*t23);
1035 : }
1036 :
1037 : // get the magnetic declination
1038 439296 : ftype magDecAng = MagDeclination();
1039 :
1040 : // Calculate the innovation
1041 439296 : ftype innovation = atan2F(magE , magN) - magDecAng;
1042 :
1043 : // limit the innovation to protect against data errors
1044 439296 : if (innovation > 0.5f) {
1045 : innovation = 0.5f;
1046 439296 : } else if (innovation < -0.5f) {
1047 : innovation = -0.5f;
1048 : }
1049 :
1050 : // correct the covariance P = (I - K*H)*P
1051 : // take advantage of the empty columns in KH to reduce the
1052 : // number of operations
1053 10903866 : for (unsigned i = 0; i<=stateIndexLim; i++) {
1054 177897690 : for (unsigned j = 0; j<=15; j++) {
1055 167433120 : KH[i][j] = 0.0f;
1056 : }
1057 10464570 : KH[i][16] = Kfusion[i] * H_MAG[16];
1058 10464570 : KH[i][17] = Kfusion[i] * H_MAG[17];
1059 73251990 : for (unsigned j = 18; j<=23; j++) {
1060 62787420 : KH[i][j] = 0.0f;
1061 : }
1062 : }
1063 10903866 : for (unsigned j = 0; j<=stateIndexLim; j++) {
1064 259896966 : for (unsigned i = 0; i<=stateIndexLim; i++) {
1065 249432396 : KHP[i][j] = KH[i][16] * P[16][j] + KH[i][17] * P[17][j];
1066 : }
1067 : }
1068 :
1069 : // Check that we are not going to drive any variances negative and skip the update if so
1070 : bool healthyFusion = true;
1071 10903866 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
1072 10464570 : if (KHP[i][i] > P[i][i]) {
1073 : healthyFusion = false;
1074 : }
1075 : }
1076 :
1077 439296 : if (healthyFusion) {
1078 : // update the covariance matrix
1079 10903866 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
1080 259896966 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
1081 249432396 : P[i][j] = P[i][j] - KHP[i][j];
1082 : }
1083 : }
1084 :
1085 : // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
1086 439296 : ForceSymmetry();
1087 439296 : ConstrainVariances();
1088 :
1089 : // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
1090 439296 : stateStruct.angErr.zero();
1091 :
1092 : // correct the state vector
1093 10903866 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
1094 10464570 : statesArray[j] = statesArray[j] - Kfusion[j] * innovation;
1095 : }
1096 :
1097 : // the first 3 states represent the angular misalignment vector.
1098 : // This is used to correct the estimated quaternion on the current time step
1099 439296 : stateStruct.quat.rotate(stateStruct.angErr);
1100 :
1101 : // record fusion health status
1102 439296 : faultStatus.bad_decl = false;
1103 : } else {
1104 : // record fusion health status
1105 0 : faultStatus.bad_decl = true;
1106 : }
1107 : }
1108 :
1109 : /********************************************************
1110 : * MISC FUNCTIONS *
1111 : ********************************************************/
1112 :
1113 : // align the NE earth magnetic field states with the published declination
1114 1334 : void NavEKF2_core::alignMagStateDeclination()
1115 : {
1116 : // don't do this if we already have a learned magnetic field
1117 1334 : if (magFieldLearned) {
1118 0 : return;
1119 : }
1120 :
1121 : // get the magnetic declination
1122 1334 : ftype magDecAng = MagDeclination();
1123 :
1124 : // rotate the NE values so that the declination matches the published value
1125 1334 : Vector3F initMagNED = stateStruct.earth_magfield;
1126 1334 : ftype magLengthNE = initMagNED.xy().length();
1127 1334 : stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
1128 1334 : stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
1129 :
1130 1334 : if (!inhibitMagStates) {
1131 : // zero the corresponding state covariances if magnetic field state learning is active
1132 332 : ftype var_16 = P[16][16];
1133 332 : ftype var_17 = P[17][17];
1134 332 : zeroRows(P,16,17);
1135 332 : zeroCols(P,16,17);
1136 332 : P[16][16] = var_16;
1137 332 : P[17][17] = var_17;
1138 :
1139 : // fuse the declination angle to establish covariances and prevent large swings in declination
1140 : // during initial fusion
1141 332 : FuseDeclination(0.1f);
1142 :
1143 : }
1144 : }
1145 :
1146 : // record a magnetic field state reset event
1147 850 : void NavEKF2_core::recordMagReset()
1148 : {
1149 850 : magStateInitComplete = true;
1150 850 : if (inFlight) {
1151 44 : finalInflightMagInit = true;
1152 : }
1153 : // take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference
1154 : // for post alignment checks
1155 850 : posDownAtLastMagReset = stateStruct.position.z;
1156 850 : quatAtLastMagReset = stateStruct.quat;
1157 850 : yawInnovAtLastMagReset = innovYaw;
1158 850 : }
1159 :
1160 : // Reset states using yaw from EKF-GSF and velocity and position from GPS
1161 764 : bool NavEKF2_core::EKFGSF_resetMainFilterYaw()
1162 : {
1163 : // Don't do a reset unless permitted by the EK2_GSF_USE_MASK and EKF2_GSF_RUN_MASK parameter masks
1164 764 : if ((yawEstimator == nullptr)
1165 764 : || !(frontend->_gsfUseMask & (1U<<core_index))
1166 1528 : || EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount) {
1167 0 : return false;
1168 : };
1169 :
1170 : ftype yawEKFGSF, yawVarianceEKFGSF, velInnovLength;
1171 768 : if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) &&
1172 4 : is_positive(yawVarianceEKFGSF) &&
1173 772 : yawVarianceEKFGSF < sq(radians(15.0f)) &&
1174 8 : (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
1175 :
1176 : // keep roll and pitch and reset yaw
1177 4 : resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false);
1178 :
1179 : // record the emergency reset event
1180 4 : EKFGSF_yaw_reset_request_ms = 0;
1181 4 : EKFGSF_yaw_reset_ms = imuSampleTime_ms;
1182 4 : EKFGSF_yaw_reset_count++;
1183 :
1184 4 : if (!use_compass()) {
1185 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u yaw aligned using GPS",(unsigned)imu_index);
1186 : } else {
1187 4 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u emergency yaw reset",(unsigned)imu_index);
1188 : }
1189 :
1190 : // Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
1191 4 : allMagSensorsFailed = true;
1192 :
1193 : // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
1194 4 : ResetVelocity();
1195 4 : ResetPosition();
1196 :
1197 : // reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
1198 4 : velTestRatio = 0.0f;
1199 4 : posTestRatio = 0.0f;
1200 :
1201 4 : return true;
1202 :
1203 : }
1204 :
1205 : return false;
1206 :
1207 : }
1208 :
1209 4 : void NavEKF2_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, bool isDeltaYaw)
1210 : {
1211 4 : QuaternionF quatBeforeReset = stateStruct.quat;
1212 :
1213 : // check if we should use a 321 or 312 Rotation sequence and update the quaternion
1214 : // states using the preferred yaw definition
1215 4 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
1216 4 : Vector3F eulerAngles;
1217 4 : if (fabsF(prevTnb[2][0]) < fabsF(prevTnb[2][1])) {
1218 : // rolled more than pitched so use 321 rotation order
1219 4 : stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
1220 4 : if (isDeltaYaw) {
1221 0 : yaw = wrap_PI(yaw + eulerAngles.z);
1222 : }
1223 4 : stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, yaw);
1224 : } else {
1225 : // pitched more than rolled so use 312 rotation order
1226 0 : eulerAngles = stateStruct.quat.to_vector312();
1227 0 : if (isDeltaYaw) {
1228 0 : yaw = wrap_PI(yaw + eulerAngles.z);
1229 : }
1230 0 : stateStruct.quat.from_vector312(eulerAngles.x, eulerAngles.y, yaw);
1231 : }
1232 :
1233 : // Update the rotation matrix
1234 4 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
1235 :
1236 : // calculate the change in the quaternion state and apply it to the output history buffer
1237 4 : QuaternionF quat_delta = stateStruct.quat / quatBeforeReset;
1238 4 : StoreQuatRotate(quat_delta);
1239 :
1240 : // rotate attitude error variances into earth frame
1241 4 : Vector3F bf_variances = Vector3F(P[0][0], P[1][1], P[2][2]);
1242 4 : Vector3F ef_variances = prevTnb.transposed() * bf_variances;
1243 :
1244 : // reset vertical component to supplied value
1245 4 : ef_variances.z = yawVariance;
1246 :
1247 : // rotate back into body frame
1248 4 : bf_variances = prevTnb * ef_variances;
1249 :
1250 : // Reset all attitude error state covariances
1251 4 : zeroRows(P, 0, 2);
1252 4 : zeroCols(P, 0, 2);
1253 :
1254 : // Initialise variances
1255 4 : P[0][0] = bf_variances.x;
1256 4 : P[1][1] = bf_variances.y;
1257 4 : P[2][2] = bf_variances.z;
1258 :
1259 : // record the yaw reset event
1260 4 : yawResetCount++;
1261 4 : recordYawReset();
1262 :
1263 : // clear all pending yaw reset requests
1264 4 : gpsYawResetRequest = false;
1265 4 : magYawResetRequest = false;
1266 :
1267 4 : }
|
