Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_core.h"
5 :
6 : #include <GCS_MAVLink/GCS.h>
7 : #include <AP_DAL/AP_DAL.h>
8 : #include <AP_GPS/AP_GPS_config.h>
9 :
10 : // minimum GPS horizontal speed required to use GPS ground course for yaw alignment (m/s)
11 : #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
12 : #define GPS_VEL_YAW_ALIGN_MIN_SPD 5.0F
13 : #else
14 : #define GPS_VEL_YAW_ALIGN_MIN_SPD 1.0F
15 : #endif
16 :
17 : /********************************************************
18 : * RESET FUNCTIONS *
19 : ********************************************************/
20 :
21 : // Control reset of yaw and magnetic field states
22 2787354 : void NavEKF3_core::controlMagYawReset()
23 : {
24 :
25 : // Vehicles that can use a zero sideslip assumption (Planes) are a special case
26 : // They can use the GPS velocity to recover from bad initial compass data
27 : // This allows recovery for heading alignment errors due to compass faults
28 2787354 : if (assume_zero_sideslip() && (!finalInflightYawInit || !yawAlignComplete) && inFlight) {
29 738 : gpsYawResetRequest = true;
30 738 : return;
31 : } else {
32 2786616 : gpsYawResetRequest = false;
33 : }
34 :
35 : // Quaternion and delta rotation vector that are re-used for different calculations
36 2786616 : Vector3F deltaRotVecTemp;
37 2786616 : QuaternionF deltaQuatTemp;
38 :
39 : bool flightResetAllowed = false;
40 : bool initialResetAllowed = false;
41 2786616 : if (!finalInflightYawInit) {
42 : // Use a quaternion division to calculate the delta quaternion between the rotation at the current and last time
43 1116719 : deltaQuatTemp = stateStruct.quat / prevQuatMagReset;
44 1116719 : prevQuatMagReset = stateStruct.quat;
45 :
46 : // convert the quaternion to a rotation vector and find its length
47 1116719 : deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
48 :
49 : // check if the spin rate is OK - high spin rates can cause angular alignment errors
50 1116719 : bool angRateOK = deltaRotVecTemp.length() < 0.1745f;
51 :
52 1116719 : initialResetAllowed = angRateOK && tiltAlignComplete;
53 1116719 : flightResetAllowed = angRateOK && !onGround;
54 :
55 : }
56 :
57 : // reset the limit on the number of magnetic anomaly resets for each takeoff
58 2786616 : if (onGround) {
59 855769 : magYawAnomallyCount = 0;
60 : }
61 :
62 : // Check if conditions for a interim or final yaw/mag reset are met
63 : bool finalResetRequest = false;
64 : bool interimResetRequest = false;
65 2786616 : if (flightResetAllowed && !assume_zero_sideslip()) {
66 : #if APM_BUILD_TYPE(APM_BUILD_ArduSub)
67 : // for sub, we'd like to be far enough away from metal structures like docks and vessels
68 : // diving 0.5m is reasonable for both open water and pools
69 15222 : finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) > EKF3_MAG_FINAL_RESET_ALT_SUB;
70 : #else
71 : // check that we have reached a height where ground magnetic interference effects are insignificant
72 : // and can perform a final reset of the yaw and field states
73 220177 : finalResetRequest = (stateStruct.position.z - posDownAtTakeoff) < -EKF3_MAG_FINAL_RESET_ALT;
74 : #endif
75 :
76 : // check for increasing height
77 235399 : bool hgtIncreasing = (posDownAtLastMagReset-stateStruct.position.z) > 0.5f;
78 235399 : ftype yawInnovIncrease = fabsF(innovYaw) - fabsF(yawInnovAtLastMagReset);
79 :
80 : // check for increasing yaw innovations
81 235399 : bool yawInnovIncreasing = yawInnovIncrease > 0.25f;
82 :
83 : // check that the yaw innovations haven't been caused by a large change in attitude
84 235399 : deltaQuatTemp = quatAtLastMagReset / stateStruct.quat;
85 235399 : deltaQuatTemp.to_axis_angle(deltaRotVecTemp);
86 235399 : bool largeAngleChange = deltaRotVecTemp.length() > yawInnovIncrease;
87 :
88 : // if yaw innovations and height have increased and we haven't rotated much
89 : // then we are climbing away from a ground based magnetic anomaly and need to reset
90 235399 : interimResetRequest = !finalInflightYawInit
91 235399 : && !finalResetRequest
92 234136 : && (magYawAnomallyCount < MAG_ANOMALY_RESET_MAX)
93 : && hgtIncreasing
94 234136 : && yawInnovIncreasing
95 235400 : && !largeAngleChange;
96 : }
97 :
98 : // an initial reset is required if we have not yet aligned the yaw angle
99 2786616 : bool initialResetRequest = initialResetAllowed && !yawAlignComplete;
100 :
101 : // a combined yaw angle and magnetic field reset can be initiated by:
102 5573232 : magYawResetRequest = magYawResetRequest || // an external request
103 2783697 : initialResetRequest || // an initial alignment performed by all vehicle types using magnetometer
104 5570313 : interimResetRequest || // an interim alignment required to recover from ground based magnetic anomaly
105 : finalResetRequest; // the final reset when we have achieved enough height to be in stable magnetic field environment
106 :
107 : // Perform a reset of magnetic field states and reset yaw to corrected magnetic heading
108 2786616 : if (magYawResetRequest && use_compass()) {
109 : // send initial alignment status to console
110 4182 : if (!yawAlignComplete) {
111 2703 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u MAG%u initial yaw alignment complete",(unsigned)imu_index, (unsigned)magSelectIndex);
112 : }
113 :
114 : // set yaw from a single mag sample
115 4182 : setYawFromMag();
116 :
117 : // send in-flight yaw alignment status to console
118 4182 : if (finalResetRequest) {
119 1263 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u MAG%u in-flight yaw alignment complete",(unsigned)imu_index, (unsigned)magSelectIndex);
120 2919 : } else if (interimResetRequest) {
121 0 : magYawAnomallyCount++;
122 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u MAG%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index, (unsigned)magSelectIndex);
123 : }
124 :
125 : // clear the complete flags if an interim reset has been performed to allow subsequent
126 : // and final reset to occur
127 4182 : if (interimResetRequest) {
128 0 : finalInflightYawInit = false;
129 0 : finalInflightMagInit = false;
130 : }
131 :
132 : // mag states
133 4182 : if (!magFieldLearned) {
134 4182 : resetMagFieldStates();
135 : }
136 : }
137 :
138 2786616 : if (magStateResetRequest) {
139 23 : resetMagFieldStates();
140 : }
141 : }
142 :
143 : // this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
144 : // vector from GPS. It is used to align the yaw angle after launch or takeoff.
145 24356 : void NavEKF3_core::realignYawGPS(bool emergency_reset)
146 : {
147 : // get quaternion from existing filter states and calculate roll, pitch and yaw angles
148 24356 : Vector3F eulerAngles;
149 24356 : stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
150 :
151 24356 : if (gpsDataDelayed.vel.xy().length_squared() > sq(GPS_VEL_YAW_ALIGN_MIN_SPD)) {
152 : // calculate course yaw angle
153 1576 : ftype velYaw = atan2F(stateStruct.velocity.y,stateStruct.velocity.x);
154 :
155 : // calculate course yaw angle from GPS velocity
156 1576 : ftype gpsYaw = atan2F(gpsDataDelayed.vel.y,gpsDataDelayed.vel.x);
157 :
158 : // Check the yaw angles for consistency
159 1576 : ftype yawErr = MAX(fabsF(wrap_PI(gpsYaw - velYaw)),fabsF(wrap_PI(gpsYaw - eulerAngles.z)));
160 :
161 : // 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
162 1576 : bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete;
163 :
164 : // get yaw variance from GPS speed uncertainty
165 1576 : const ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
166 1576 : const ftype gps_yaw_variance = sq(asinF(constrain_float(gpsVelAcc/gpsDataDelayed.vel.xy().length(), -1.0F, 1.0F)));
167 1576 : if (gps_yaw_variance < sq(radians(GPS_VEL_YAW_ALIGN_MAX_ANG_ERR))) {
168 1544 : yawAlignGpsValidCount++;
169 : } else {
170 32 : yawAlignGpsValidCount = 0;
171 : }
172 :
173 : // correct yaw angle using GPS ground course if compass yaw bad
174 1576 : if (badMagYaw) {
175 : // attempt to use EKF-GSF estimate if available as it is more robust to GPS glitches
176 : // by default fly forward vehicles use ground course for initial yaw unless the GSF is explicitly selected as the yaw source
177 154 : const bool useGSF = !assume_zero_sideslip() || (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF);
178 54 : if (useGSF && EKFGSF_resetMainFilterYaw(emergency_reset)) {
179 2 : return;
180 : }
181 :
182 152 : if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
183 24 : yawAlignGpsValidCount = 0;
184 : // keep roll and pitch and reset yaw
185 : rotationOrder order;
186 24 : bestRotationOrder(order);
187 24 : resetQuatStateYawOnly(gpsYaw, gps_yaw_variance, order);
188 :
189 : // reset the velocity and position states as they will be inaccurate due to bad yaw
190 24 : ResetVelocity(resetDataSource::GPS);
191 24 : ResetPosition(resetDataSource::GPS);
192 :
193 : // send yaw alignment information to console
194 24 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned to GPS velocity",(unsigned)imu_index);
195 :
196 24 : if (use_compass()) {
197 : // request a mag field reset which may enable us to use the magnetometer if the previous fault was due to bad initialisation
198 0 : magStateResetRequest = true;
199 : // clear the all sensors failed status so that the magnetometers sensors get a second chance now that we are flying
200 0 : allMagSensorsFailed = false;
201 : }
202 : }
203 1422 : } else if (yawAlignGpsValidCount >= GPS_VEL_YAW_ALIGN_COUNT_THRESHOLD) {
204 : // There is no need to do a yaw reset
205 284 : yawAlignGpsValidCount = 0;
206 284 : recordYawResetsCompleted();
207 : }
208 : } else {
209 22780 : yawAlignGpsValidCount = 0;
210 : }
211 : }
212 :
213 : /*
214 : build the body-to-earth rotation matrix at the current state attitude with
215 : yaw set to zero, using the given Euler rotation order. Optionally returns
216 : the yaw angle removed. Returns false if the rotation order is not supported
217 : */
218 1088306 : bool NavEKF3_core::buildTbnZeroYaw(rotationOrder order, Matrix3F &Tbn, ftype *yawAng) const
219 : {
220 1088306 : switch (order) {
221 867049 : case rotationOrder::TAIT_BRYAN_321: {
222 867049 : Vector3F euler321;
223 867049 : stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
224 867049 : Tbn.from_euler(euler321.x, euler321.y, 0.0f);
225 867049 : if (yawAng != nullptr) {
226 862625 : *yawAng = euler321.z;
227 : }
228 : return true;
229 : }
230 221257 : case rotationOrder::TAIT_BRYAN_312: {
231 221257 : const Vector3F euler312 = stateStruct.quat.to_vector312();
232 221257 : Tbn.from_euler312(euler312.x, euler312.y, 0.0f);
233 221257 : if (yawAng != nullptr) {
234 221257 : *yawAng = euler312.z;
235 : }
236 : return true;
237 : }
238 : }
239 : // rotation order not supported
240 : return false;
241 : }
242 :
243 : #if EK3_FEATURE_MOVING_BASELINE
244 : /*
245 : Correct a yaw measurement calculated from a moving baseline antenna offset
246 : for vehicle attitude.
247 :
248 : The GPS driver recovers the vehicle yaw from the reported baseline heading
249 : by subtracting the bearing of the body-frame antenna offset, which is only
250 : exact when the vehicle is level. The residual attitude correction is applied
251 : here using this core's own roll and pitch estimate at the fusion time
252 : horizon, which is time-aligned with the measurement. Using the core's own
253 : attitude estimate rather than the published AHRS attitude keeps each lane
254 : independent of the others and allows Replay to reproduce the fusion exactly.
255 :
256 : The correction rotates attitude uncertainty into the yaw measurement in
257 : proportion to the vertical component of the baseline, so yawAngErr is
258 : inflated to match.
259 :
260 : Returns false when the antenna baseline is too close to vertical at the
261 : estimated attitude for the reported heading to contain usable yaw
262 : information, or when the rotation order is not supported, in which case the
263 : measurement must not be fused.
264 : */
265 4954 : bool NavEKF3_core::correctGPSYawForAntennaOffset(yaw_elements &yawAngData) const
266 : {
267 4954 : const Vector3F &antOffsetBody = yawAngData.antOffset;
268 4954 : if (antOffsetBody.is_zero()) {
269 : // no antenna offset supplied so the measurement is used as reported
270 : return true;
271 : }
272 :
273 : // calculate the rotation from body to earth frame with yaw set to zero
274 : // using the rotation order of the measurement
275 4424 : Matrix3F Tbn_zeroYaw;
276 4424 : if (!buildTbnZeroYaw(yawAngData.order, Tbn_zeroYaw)) {
277 : // rotation order not supported: fail closed rather than fusing a
278 : // measurement known to need a correction that cannot be applied
279 : return false;
280 : }
281 :
282 4424 : const Vector3F antOffsetLevel = Tbn_zeroYaw * antOffsetBody;
283 4424 : const ftype antOffsetLevelXY = antOffsetLevel.xy().length();
284 :
285 : // the heading noise of the receiver scales inversely with the horizontal
286 : // separation of the antennas, so a baseline close to vertical at the
287 : // estimated attitude carries no usable yaw information. The same
288 : // threshold is enforced by the GPS driver against the receiver-reported
289 : // baseline
290 : const ftype minHorizontalSeparation = AP_GPS_MB_MIN_ANTENNA_SEPARATION_M;
291 4424 : if (antOffsetLevelXY < minHorizontalSeparation) {
292 : return false;
293 : }
294 :
295 : // an attitude error rotates the vertical component of the baseline into
296 : // the horizontal plane, changing the bearing of the offset by up to
297 : // |z| / |xy| radians per radian of tilt error, so the correction adds
298 : // that much uncertainty to the measurement
299 3970 : const ftype tiltSensitivity = fabsF(antOffsetLevel.z) / antOffsetLevelXY;
300 3970 : yawAngData.yawAngErr = sqrtF(sq(yawAngData.yawAngErr) + sq(tiltSensitivity) * MAX(tiltErrorVariance, 0.0f));
301 :
302 : // replace the body-frame bearing of the antenna offset subtracted by the
303 : // driver with the bearing of the offset at the estimated attitude
304 3970 : const ftype bearingBody = atan2F(-antOffsetBody.y, -antOffsetBody.x);
305 3970 : const ftype bearingLevel = atan2F(-antOffsetLevel.y, -antOffsetLevel.x);
306 3970 : yawAngData.yawAng = wrap_PI(yawAngData.yawAng + bearingBody - bearingLevel);
307 3970 : return true;
308 : }
309 : #endif // EK3_FEATURE_MOVING_BASELINE
310 :
311 : // align the yaw angle for the quaternion states to the given yaw angle which should be at the fusion horizon
312 27 : void NavEKF3_core::alignYawAngle(const yaw_elements &yawAngData)
313 : {
314 : // update quaternion states and covariances
315 27 : resetQuatStateYawOnly(yawAngData.yawAng, sq(MAX(yawAngData.yawAngErr, 1.0e-2)), yawAngData.order);
316 :
317 : // send yaw alignment information to console
318 27 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned",(unsigned)imu_index);
319 27 : }
320 :
321 : /********************************************************
322 : * FUSE MEASURED_DATA *
323 : ********************************************************/
324 :
325 : // select fusion of magnetometer data
326 19965274 : void NavEKF3_core::SelectMagFusion()
327 : {
328 : // clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
329 : // used for load levelling
330 19965274 : magFusePerformed = false;
331 :
332 : // Store yaw angle when moving for use as a static reference when not moving
333 19965274 : if (!onGroundNotMoving) {
334 14178195 : if (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) {
335 : // A 321 rotation order is best conditioned because the X axis is closer to horizontal than the Y axis
336 6949421 : yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_321;
337 6949421 : yawAngDataStatic.yawAng = atan2F(prevTnb[0][1], prevTnb[0][0]);
338 : } else {
339 : // A 312 rotation order is best conditioned because the Y axis is closer to horizontal than the X axis
340 7228774 : yawAngDataStatic.order = rotationOrder::TAIT_BRYAN_312;
341 7228774 : yawAngDataStatic.yawAng = atan2F(-prevTnb[1][0], prevTnb[1][1]);
342 : }
343 14178195 : yawAngDataStatic.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
344 14178195 : yawAngDataStatic.time_ms = imuDataDelayed.time_ms;
345 : }
346 :
347 : // Handle case where we are not using a yaw sensor of any type and attempt to reset the yaw in
348 : // flight using the output from the GSF yaw estimator or GPS ground course.
349 39930548 : if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) ||
350 19965274 : (!use_compass() &&
351 118902 : yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS &&
352 76667 : yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK &&
353 : yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
354 :
355 48398 : if ((!yawAlignComplete || yaw_source_reset) && ((yaw_source_last != AP_NavEKF_Source::SourceYaw::GSF) || (EKFGSF_yaw_valid_count >= GSF_YAW_VALID_HISTORY_THRESHOLD))) {
356 20704 : realignYawGPS(false);
357 20704 : yaw_source_reset = false;
358 : } else {
359 27694 : yaw_source_reset = false;
360 : }
361 :
362 48398 : if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
363 : // use the EKF-GSF yaw estimator output as this is more robust than the EKF can achieve without a yaw measurement
364 : // for non fixed wing platform types
365 : ftype gsfYaw, gsfYawVariance;
366 4461 : const bool didUseEKFGSF = yawAlignComplete && (yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) && EKFGSF_getYaw(gsfYaw, gsfYawVariance) && !assume_zero_sideslip() && fuseEulerYaw(yawFusionMethod::GSF);
367 :
368 : // fallback methods
369 : if (!didUseEKFGSF) {
370 4461 : if (onGroundNotMoving) {
371 : // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
372 846 : fuseEulerYaw(yawFusionMethod::STATIC);
373 3615 : } else if (onGround || PV_AidingMode == AID_NONE || (P[0][0]+P[1][1]+P[2][2]+P[3][3] > 0.01f)) {
374 : // prevent uncontrolled yaw variance growth that can destabilise the covariance matrix
375 : // by fusing a zero innovation
376 416 : fuseEulerYaw(yawFusionMethod::PREDICTED);
377 : }
378 : }
379 4461 : magTestRatio.zero();
380 4461 : yawTestRatio = 0.0f;
381 4461 : lastSynthYawTime_ms = imuSampleTime_ms;
382 : }
383 48398 : return;
384 : }
385 :
386 : // Handle case where we are using GPS yaw sensor instead of a magnetomer
387 19916876 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
388 : bool have_fused_gps_yaw = false;
389 : // correct the measurement for vehicle attitude using this core's own attitude
390 : // estimate. This is done once per recalled measurement, before all consumers of
391 : // yawAngDataDelayed (alignYawAngle, fuseEulerYaw and learnMagBiasFromGPS).
392 : // A measurement with no usable yaw information is treated the same as no
393 : // measurement: it is not fused or used for alignment (which matters most for
394 : // alignYawAngle as it accepts the measurement without an innovation check), it
395 : // does not refresh last_gps_yaw_ms (so the compass fallback and the yaw source
396 : // health reporting see the loss of usable yaw) and it does not block the
397 : // synthetic yaw fusion and yaw source reset recovery branches below
398 : #if EK3_FEATURE_MOVING_BASELINE
399 86284 : if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms) &&
400 4954 : correctGPSYawForAntennaOffset(yawAngDataDelayed)) {
401 : #else
402 : if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
403 : #endif
404 4500 : if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
405 22 : alignYawAngle(yawAngDataDelayed);
406 22 : yaw_source_reset = false;
407 : have_fused_gps_yaw = true;
408 22 : lastSynthYawTime_ms = imuSampleTime_ms;
409 22 : last_gps_yaw_fuse_ms = imuSampleTime_ms;
410 22 : recordYawResetsCompleted();
411 4478 : } else if (tiltAlignComplete && yawAlignComplete) {
412 4328 : have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS);
413 4328 : if (have_fused_gps_yaw) {
414 4327 : last_gps_yaw_fuse_ms = imuSampleTime_ms;
415 : }
416 : }
417 4500 : last_gps_yaw_ms = imuSampleTime_ms;
418 76830 : } else if (tiltAlignComplete && !yawAlignComplete) {
419 : // External yaw sources can take significant time to start providing yaw data so
420 : // wuile waiting, fuse a 'fake' yaw observation at 7Hz to keeop the filter stable
421 1491 : if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
422 132 : yawAngDataDelayed.yawAngErr = MAX(frontend->_yawNoise, 0.05f);
423 : // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
424 132 : if (!onGroundNotMoving) {
425 : // prevent uncontrolled yaw variance growth by fusing a zero innovation
426 2 : fuseEulerYaw(yawFusionMethod::PREDICTED);
427 : } else {
428 : // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
429 130 : fuseEulerYaw(yawFusionMethod::STATIC);
430 : }
431 132 : lastSynthYawTime_ms = imuSampleTime_ms;
432 : }
433 75339 : } else if (tiltAlignComplete && yawAlignComplete && onGround && imuSampleTime_ms - last_gps_yaw_fuse_ms > 10000) {
434 : // handle scenario where we were using GPS yaw previously, but the yaw fusion has timed out.
435 1206 : yaw_source_reset = true;
436 : }
437 :
438 81330 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS) {
439 : // no fallback
440 : return;
441 : }
442 :
443 : // get new mag data into delay buffer
444 39097 : readMagData();
445 :
446 39097 : if (have_fused_gps_yaw) {
447 1932 : if (gps_yaw_mag_fallback_active) {
448 0 : gps_yaw_mag_fallback_active = false;
449 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw external",(unsigned)imu_index);
450 : }
451 : // update mag bias from GPS yaw
452 1932 : gps_yaw_mag_fallback_ok = learnMagBiasFromGPS();
453 1932 : return;
454 : }
455 :
456 : // we don't have GPS yaw data and are configured for
457 : // fallback. If we've only just lost GPS yaw
458 37165 : if (imuSampleTime_ms - last_gps_yaw_ms < 10000) {
459 : // don't fallback to magnetometer fusion for 10s
460 : return;
461 : }
462 6353 : if (!gps_yaw_mag_fallback_ok) {
463 : // mag was not consistent enough with GPS to use it as
464 : // fallback
465 : return;
466 : }
467 5775 : if (!inFlight) {
468 : // don't fall back if not flying but reset to GPS yaw if it becomes available
469 : return;
470 : }
471 5505 : if (!gps_yaw_mag_fallback_active) {
472 2 : gps_yaw_mag_fallback_active = true;
473 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw fallback active",(unsigned)imu_index);
474 : }
475 : // fall through to magnetometer fusion
476 : }
477 :
478 : #if EK3_FEATURE_EXTERNAL_NAV
479 : // Handle case where we are using an external nav for yaw
480 19841051 : const bool extNavYawDataToFuse = storedExtNavYawAng.recall(extNavYawAngDataDelayed, imuDataDelayed.time_ms);
481 19841051 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
482 28269 : if (extNavYawDataToFuse) {
483 6342 : if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
484 5 : alignYawAngle(extNavYawAngDataDelayed);
485 5 : yaw_source_reset = false;
486 6337 : } else if (tiltAlignComplete && yawAlignComplete) {
487 6254 : fuseEulerYaw(yawFusionMethod::EXTNAV);
488 : }
489 6342 : last_extnav_yaw_fusion_ms = imuSampleTime_ms;
490 21927 : } else if (tiltAlignComplete && !yawAlignComplete) {
491 : // External yaw sources can take significant time to start providing yaw data so
492 : // while waiting, fuse a 'fake' yaw observation at 7Hz to keep the filter stable
493 2 : if (imuSampleTime_ms - lastSynthYawTime_ms > 140) {
494 : // update the yaw angle using the last estimate which will be used as a static yaw reference when movement stops
495 1 : if (!onGroundNotMoving) {
496 : // prevent uncontrolled yaw variance growth by fusing a zero innovation
497 0 : fuseEulerYaw(yawFusionMethod::PREDICTED);
498 : } else {
499 : // fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
500 1 : fuseEulerYaw(yawFusionMethod::STATIC);
501 : }
502 1 : lastSynthYawTime_ms = imuSampleTime_ms;
503 : }
504 : }
505 : }
506 : #endif // EK3_FEATURE_EXTERNAL_NAV
507 :
508 : // If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
509 19841051 : if (magHealth) {
510 19278043 : magTimeout = false;
511 19278043 : lastHealthyMagTime_ms = imuSampleTime_ms;
512 563008 : } else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) {
513 43266 : magTimeout = true;
514 : }
515 :
516 19841051 : if (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
517 : // check for and read new magnetometer measurements. We don't
518 : // read for GPS_COMPASS_FALLBACK as it has already been read
519 : // above
520 19835546 : readMagData();
521 : }
522 :
523 : // check for availability of magnetometer or other yaw data to fuse
524 19841051 : magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
525 :
526 : // Control reset of yaw and magnetic field states if we are using compass data
527 19841051 : if (magDataToFuse) {
528 2787354 : if (yaw_source_reset && (yaw_source_last == AP_NavEKF_Source::SourceYaw::COMPASS ||
529 : yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
530 33 : magYawResetRequest = true;
531 33 : yaw_source_reset = false;
532 : }
533 2787354 : controlMagYawReset();
534 : }
535 :
536 : // determine if conditions are right to start a new fusion cycle
537 : // wait until the EKF time horizon catches up with the measurement
538 19841051 : bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
539 : if (dataReady) {
540 : // use the simple method of declination to maintain heading if we cannot use the magnetic field states
541 2743449 : if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
542 1070607 : magFusionSel = MagFuseSel::FUSE_YAW;
543 1070607 : fuseEulerYaw(yawFusionMethod::MAGNETOMETER);
544 :
545 : // zero the test ratio output from the inactive 3-axis magnetometer fusion
546 1070607 : magTestRatio.zero();
547 :
548 : } else {
549 1672842 : magFusionSel = MagFuseSel::FUSE_MAG;
550 : // a stationary vehicle gives 3-axis fusion no yaw observability - a yaw error can be
551 : // absorbed by the body field states with zero innovation, so anchor the yaw to the
552 : // measured magnetic heading
553 : bool yawAnchored = false;
554 1672842 : if (onGroundNotMoving && effectiveMagCal == MagCal::GROUND_AND_INFLIGHT) {
555 1298 : yawAnchored = fuseEulerYaw(yawFusionMethod::MAGNETOMETER);
556 : // bad_yaw covers the case where FinishFusion skipped the update
557 1298 : if (yawAnchored && !faultStatus.bad_yaw) {
558 1298 : magFusionSel = MagFuseSel::FUSE_MAG_ANCHORED;
559 : }
560 : }
561 : // if we are not doing aiding with earth relative observations (eg GPS) then the declination is
562 : // maintained by fusing declination as a synthesised observation
563 : // We also fuse declination if we are using the WMM tables
564 1672842 : if (PV_AidingMode != AID_ABSOLUTE ||
565 1644478 : (frontend->_mag_ef_limit > 0 && have_table_earth_field)) {
566 1672842 : FuseDeclination(0.34f);
567 : }
568 : // fuse the three magnetometer componenents using sequential fusion for each axis
569 1672842 : FuseMagnetometer();
570 1672842 : if (!yawAnchored) {
571 : // zero the test ratio output from the inactive simple magnetometer yaw fusion
572 1671544 : yawTestRatio = 0.0f;
573 : }
574 : }
575 : }
576 :
577 : // If the final yaw reset has been performed and the state variances are sufficiently low
578 : // record that the earth field has been learned.
579 19841051 : if (!magFieldLearned && finalInflightMagInit) {
580 981782 : magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
581 : }
582 :
583 : // record the last learned field variances
584 19841051 : if (magFieldLearned && !inhibitMagStates) {
585 9068669 : earthMagFieldVar.x = P[16][16];
586 9068669 : earthMagFieldVar.y = P[17][17];
587 9068669 : earthMagFieldVar.z = P[18][18];
588 9068669 : bodyMagFieldVar.x = P[19][19];
589 9068669 : bodyMagFieldVar.y = P[20][20];
590 9068669 : bodyMagFieldVar.z = P[21][21];
591 : }
592 : }
593 :
594 : /*
595 : * Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
596 : * The script file used to generate these and other equations in this filter can be found here:
597 : * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
598 : */
599 1672842 : void NavEKF3_core::FuseMagnetometer()
600 : {
601 : // perform sequential fusion of magnetometer measurements.
602 : // this assumes that the errors in the different components are
603 : // uncorrelated which is not true, however in the absence of covariance
604 : // data fit is the only assumption we can make
605 : // so we might as well take advantage of the computational efficiencies
606 : // associated with sequential fusion
607 : // calculate observation jacobians and Kalman gains
608 :
609 : // create aliases for state to make code easier to read:
610 1672842 : const ftype q0 = stateStruct.quat[0];
611 1672842 : const ftype q1 = stateStruct.quat[1];
612 1672842 : const ftype q2 = stateStruct.quat[2];
613 1672842 : const ftype q3 = stateStruct.quat[3];
614 1672842 : const ftype magN = stateStruct.earth_magfield[0];
615 1672842 : const ftype magE = stateStruct.earth_magfield[1];
616 1672842 : const ftype magD = stateStruct.earth_magfield[2];
617 1672842 : const ftype magXbias = stateStruct.body_magfield[0];
618 1672842 : const ftype magYbias = stateStruct.body_magfield[1];
619 1672842 : const ftype magZbias = stateStruct.body_magfield[2];
620 :
621 : // rotate predicted earth components into body axes and calculate
622 : // predicted measurements
623 : const Matrix3F DCM {
624 1672842 : q0*q0 + q1*q1 - q2*q2 - q3*q3,
625 1672842 : 2.0f*(q1*q2 + q0*q3),
626 1672842 : 2.0f*(q1*q3-q0*q2),
627 1672842 : 2.0f*(q1*q2 - q0*q3),
628 1672842 : q0*q0 - q1*q1 + q2*q2 - q3*q3,
629 1672842 : 2.0f*(q2*q3 + q0*q1),
630 1672842 : 2.0f*(q1*q3 + q0*q2),
631 1672842 : 2.0f*(q2*q3 - q0*q1),
632 1672842 : q0*q0 - q1*q1 - q2*q2 + q3*q3
633 1672842 : };
634 :
635 : const Vector3F MagPred {
636 1672842 : DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias,
637 1672842 : DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias,
638 1672842 : DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias
639 5018526 : };
640 :
641 : // calculate the measurement innovation for each axis
642 1672842 : innovMag = MagPred - magDataDelayed.mag;
643 :
644 : // scale magnetometer observation error with total angular rate to allow for timing errors
645 1672842 : const ftype R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*imuDataDelayed.delAng.length() / imuDataDelayed.delAngDT);
646 :
647 : // calculate common expressions used to calculate observation jacobians an innovation variance for each component
648 : const Vector9 SH_MAG {
649 1672842 : 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1,
650 1672842 : 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2,
651 1672842 : 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3,
652 1672842 : sq(q3),
653 1672842 : sq(q2),
654 1672842 : sq(q1),
655 1672842 : sq(q0),
656 1672842 : 2.0f*magN*q0,
657 1672842 : 2.0f*magE*q3
658 : };
659 :
660 : // Calculate the innovation variance for each axis
661 : // X axis
662 1672842 : varInnovMag[0] = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] - P[2][19]*SH_MAG[1] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] - P[2][17]*SH_MAG[1] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][17]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] - P[2][18]*SH_MAG[1] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][18]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] - P[2][0]*SH_MAG[1] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][0]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[17][19]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][19]*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] - P[2][1]*SH_MAG[1] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][1]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P[19][2] + P[1][2]*SH_MAG[0] - P[2][2]*SH_MAG[1] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][2]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] - P[2][3]*SH_MAG[1] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][3]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] - P[2][16]*SH_MAG[1] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2.0f*q0*q3 + 2.0f*q1*q2) - P[18][16]*(2.0f*q0*q2 - 2.0f*q1*q3) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
663 1672842 : if (varInnovMag[0] >= R_MAG) {
664 1672842 : faultStatus.bad_xmag = false;
665 : } else {
666 : // the calculation is badly conditioned, so we cannot perform fusion on this step
667 : // we reset the covariance matrix and try again next measurement
668 0 : CovarianceInit();
669 0 : faultStatus.bad_xmag = true;
670 5640 : return;
671 : }
672 :
673 : // Y axis
674 1672842 : varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][16]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][18]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][3]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[16][20]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][20]*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][0]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][1]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][2]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[18][17]*(2.0f*q0*q1 + 2.0f*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
675 1672842 : if (varInnovMag[1] >= R_MAG) {
676 1672842 : faultStatus.bad_ymag = false;
677 : } else {
678 : // the calculation is badly conditioned, so we cannot perform fusion on this step
679 : // we reset the covariance matrix and try again next measurement
680 0 : CovarianceInit();
681 0 : faultStatus.bad_ymag = true;
682 0 : return;
683 : }
684 :
685 : // Z axis
686 1672842 : varInnovMag[2] = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] - P[1][21]*SH_MAG[2] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] - P[1][16]*SH_MAG[2] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] - P[1][17]*SH_MAG[2] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] - P[1][2]*SH_MAG[2] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][2]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[16][21]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] - P[1][0]*SH_MAG[2] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P[21][1] + P[0][1]*SH_MAG[1] - P[1][1]*SH_MAG[2] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] - P[1][3]*SH_MAG[2] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][3]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] - P[1][18]*SH_MAG[2] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2.0f*q0*q2 + 2.0f*q1*q3) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
687 1672842 : if (varInnovMag[2] >= R_MAG) {
688 1672842 : faultStatus.bad_zmag = false;
689 : } else {
690 : // the calculation is badly conditioned, so we cannot perform fusion on this step
691 : // we reset the covariance matrix and try again next measurement
692 0 : CovarianceInit();
693 0 : faultStatus.bad_zmag = true;
694 0 : return;
695 : }
696 :
697 : // calculate the innovation test ratios
698 6691368 : for (uint8_t i = 0; i<=2; i++) {
699 5018526 : magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
700 : }
701 :
702 : // check the last values from all components and set magnetometer health accordingly
703 1672842 : magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
704 :
705 : // if the magnetometer is unhealthy, do not proceed further
706 1672842 : if (!magHealth) {
707 : return;
708 : }
709 :
710 : Vector24 H_MAG;
711 : // index of H_MAG which is exactly 1, for more efficient computation below
712 : int H_MAG_unit_index;
713 6668808 : for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) {
714 :
715 5001606 : if (obsIndex == 0) {
716 :
717 39563628 : for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
718 1667202 : H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
719 1667202 : H_MAG[1] = SH_MAG[0];
720 1667202 : H_MAG[2] = -SH_MAG[1];
721 1667202 : H_MAG[3] = SH_MAG[2];
722 1667202 : H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
723 1667202 : H_MAG[17] = 2.0f*q0*q3 + 2.0f*q1*q2;
724 1667202 : H_MAG[18] = 2.0f*q1*q3 - 2.0f*q0*q2;
725 : H_MAG[19] = 1.0f;
726 : H_MAG[20] = 0.0f;
727 : H_MAG[21] = 0.0f;
728 : H_MAG_unit_index = 19;
729 :
730 : // calculate Kalman gain
731 : const Vector5 SK_MX {
732 1667202 : 1.0f / varInnovMag[0],
733 : SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6],
734 : SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2,
735 : 2.0f*q0*q2 - 2.0f*q1*q3,
736 : 2.0f*q0*q3 + 2.0f*q1*q2
737 : };
738 :
739 : uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero)
740 :
741 1667202 : if (!inhibitDelAngBiasStates) {
742 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
743 : }
744 :
745 1667202 : if (!inhibitDelVelBiasStates) {
746 6668568 : for (uint8_t index = 0; index < 3; index++) {
747 5001426 : const uint8_t stateIndex = index + 13;
748 5001426 : if (!dvelBiasAxisInhibit[index]) {
749 4997570 : kalman_mask |= (1<<stateIndex);
750 : }
751 : }
752 : }
753 :
754 : // zero Kalman gains to inhibit magnetic field state estimation
755 1667202 : if (!inhibitMagStates) {
756 1667202 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
757 : }
758 :
759 : // zero Kalman gains to inhibit wind state estimation
760 1667202 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
761 602571 : kalman_mask |= (1<<22) | (1<<23);
762 : }
763 :
764 41680050 : for (auto i=0; i<24; i++) {
765 : ftype res = 0;
766 40012848 : if (kalman_mask & (1<<i)) {
767 37879370 : res = SK_MX[0]*(P[i][19] + P[i][1]*SH_MAG[0] - P[i][2]*SH_MAG[1] + P[i][3]*SH_MAG[2] + P[i][0]*SK_MX[2] - P[i][16]*SK_MX[1] + P[i][17]*SK_MX[4] - P[i][18]*SK_MX[3]);
768 : }
769 40012848 : Kfusion[i] = res;
770 : }
771 :
772 : // set flags to indicate to other processes that fusion has been performed and is required on the next frame
773 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
774 1667202 : magFusePerformed = true;
775 3334404 : } else if (obsIndex == 1) { // Fuse Y axis
776 :
777 : // calculate observation jacobians
778 39563628 : for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
779 1667202 : H_MAG[0] = SH_MAG[2];
780 1667202 : H_MAG[1] = SH_MAG[1];
781 1667202 : H_MAG[2] = SH_MAG[0];
782 1667202 : H_MAG[3] = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
783 1667202 : H_MAG[16] = 2.0f*q1*q2 - 2.0f*q0*q3;
784 1667202 : H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
785 1667202 : H_MAG[18] = 2.0f*q0*q1 + 2.0f*q2*q3;
786 : H_MAG[19] = 0.0f;
787 : H_MAG[20] = 1.0f;
788 : H_MAG[21] = 0.0f;
789 : H_MAG_unit_index = 20;
790 :
791 : // calculate Kalman gain
792 : const Vector5 SK_MY {
793 1667202 : 1.0f / varInnovMag[1],
794 : SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6],
795 : SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2,
796 : 2.0f*q0*q3 - 2.0f*q1*q2,
797 : 2.0f*q0*q1 + 2.0f*q2*q3
798 : };
799 :
800 : uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero)
801 :
802 1667202 : if (!inhibitDelAngBiasStates) {
803 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
804 : }
805 :
806 1667202 : if (!inhibitDelVelBiasStates) {
807 6668568 : for (uint8_t index = 0; index < 3; index++) {
808 5001426 : const uint8_t stateIndex = index + 13;
809 5001426 : if (!dvelBiasAxisInhibit[index]) {
810 4997570 : kalman_mask |= (1<<stateIndex);
811 : }
812 : }
813 : }
814 :
815 : // zero Kalman gains to inhibit magnetic field state estimation
816 1667202 : if (!inhibitMagStates) {
817 1667202 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
818 : }
819 :
820 : // zero Kalman gains to inhibit wind state estimation
821 1667202 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
822 602571 : kalman_mask |= (1<<22) | (1<<23);
823 : }
824 :
825 41680050 : for (auto i=0; i<24; i++) {
826 : ftype res = 0;
827 40012848 : if (kalman_mask & (1<<i)) {
828 37879370 : res = SK_MY[0]*(P[i][20] + P[i][0]*SH_MAG[2] + P[i][1]*SH_MAG[1] + P[i][2]*SH_MAG[0] - P[i][3]*SK_MY[2] - P[i][17]*SK_MY[1] - P[i][16]*SK_MY[3] + P[i][18]*SK_MY[4]);
829 : }
830 40012848 : Kfusion[i] = res;
831 : }
832 :
833 : // set flags to indicate to other processes that fusion has been performed and is required on the next frame
834 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
835 1667202 : magFusePerformed = true;
836 : }
837 : else if (obsIndex == 2) // we are now fusing the Z measurement
838 : {
839 : // calculate observation jacobians
840 39563628 : for (uint8_t i = 0; i<=stateIndexLim; i++) H_MAG[i] = 0.0f;
841 1667202 : H_MAG[0] = SH_MAG[1];
842 1667202 : H_MAG[1] = -SH_MAG[2];
843 1667202 : H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
844 1667202 : H_MAG[3] = SH_MAG[0];
845 1667202 : H_MAG[16] = 2.0f*q0*q2 + 2.0f*q1*q3;
846 1667202 : H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
847 1667202 : H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
848 : H_MAG[19] = 0.0f;
849 : H_MAG[20] = 0.0f;
850 : H_MAG[21] = 1.0f;
851 : H_MAG_unit_index = 21;
852 :
853 : // calculate Kalman gain
854 : const Vector5 SK_MZ {
855 1667202 : 1.0f / varInnovMag[2],
856 : SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6],
857 : SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2,
858 : 2.0f*q0*q1 - 2.0f*q2*q3,
859 : 2.0f*q0*q2 + 2.0f*q1*q3
860 : };
861 :
862 : uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero)
863 :
864 1667202 : if (!inhibitDelAngBiasStates) {
865 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
866 : }
867 :
868 1667202 : if (!inhibitDelVelBiasStates) {
869 6668568 : for (uint8_t index = 0; index < 3; index++) {
870 5001426 : const uint8_t stateIndex = index + 13;
871 5001426 : if (!dvelBiasAxisInhibit[index]) {
872 4997570 : kalman_mask |= (1<<stateIndex);
873 : }
874 : }
875 : }
876 :
877 : // zero Kalman gains to inhibit magnetic field state estimation
878 1667202 : if (!inhibitMagStates) {
879 1667202 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
880 : }
881 :
882 : // zero Kalman gains to inhibit wind state estimation
883 1667202 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
884 602571 : kalman_mask |= (1<<22) | (1<<23);
885 : }
886 :
887 41680050 : for (auto i=0; i<24; i++) {
888 : ftype res = 0;
889 40012848 : if (kalman_mask & (1<<i)) {
890 37879370 : res = SK_MZ[0]*(P[i][21] + P[i][0]*SH_MAG[1] - P[i][1]*SH_MAG[2] + P[i][3]*SH_MAG[0] + P[i][2]*SK_MZ[2] + P[i][18]*SK_MZ[1] + P[i][16]*SK_MZ[4] - P[i][17]*SK_MZ[3]);
891 : }
892 40012848 : Kfusion[i] = res;
893 : }
894 :
895 : // set flags to indicate to other processes that fusion has been performed and is required on the next frame
896 : // this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
897 1667202 : magFusePerformed = true;
898 : }
899 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
900 : // the zero elements of H to reduce the number of operations.
901 118690884 : for (unsigned i = 0; i<=stateIndexLim; i++) {
902 : // j as the inner loop allows the compiler to hoist the KH product
903 : // to save computation, and do the inner indexing more efficiently.
904 2702548098 : for (unsigned j = 0; j<=stateIndexLim; j++) {
905 : ftype res = 0;
906 2588858820 : res += (Kfusion[i] * H_MAG[0]) * P[0][j];
907 2588858820 : res += (Kfusion[i] * H_MAG[1]) * P[1][j];
908 2588858820 : res += (Kfusion[i] * H_MAG[2]) * P[2][j];
909 2588858820 : res += (Kfusion[i] * H_MAG[3]) * P[3][j];
910 2588858820 : res += (Kfusion[i] * H_MAG[16]) * P[16][j];
911 2588858820 : res += (Kfusion[i] * H_MAG[17]) * P[17][j];
912 2588858820 : res += (Kfusion[i] * H_MAG[18]) * P[18][j];
913 : // one value in H is always 1, and the others not mentioned here
914 : // are zero, so we can skip that H product to save an operation.
915 2588858820 : res += Kfusion[i] * P[H_MAG_unit_index][j];
916 2588858820 : KHP[i][j] = res;
917 : }
918 : }
919 :
920 : // finish fusion from KHP and Kfusion
921 5001606 : if (!FinishFusion(innovMag[obsIndex])) { // no fault?
922 : // add table constraint here for faster convergence
923 5001606 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
924 4992465 : MagTableConstrain();
925 : }
926 : } else {
927 : // record bad axis
928 0 : if (obsIndex == 0) {
929 0 : faultStatus.bad_xmag = true;
930 0 : } else if (obsIndex == 1) {
931 0 : faultStatus.bad_ymag = true;
932 : } else if (obsIndex == 2) {
933 0 : faultStatus.bad_zmag = true;
934 : }
935 0 : CovarianceInit();
936 0 : return;
937 : }
938 : }
939 : }
940 :
941 : /*
942 : * Fuse direct yaw measurements using explicit algebraic equations auto-generated from
943 : * derivation/generate_2.py with output recorded in derivation/generated/yaw_generated.cpp
944 : * Returns true if the fusion was successful
945 : */
946 1083882 : bool NavEKF3_core::fuseEulerYaw(yawFusionMethod method)
947 : {
948 1083882 : const ftype &q0 = stateStruct.quat[0];
949 1083882 : const ftype &q1 = stateStruct.quat[1];
950 1083882 : const ftype &q2 = stateStruct.quat[2];
951 1083882 : const ftype &q3 = stateStruct.quat[3];
952 :
953 : ftype gsfYaw, gsfYawVariance;
954 1083882 : if (method == yawFusionMethod::GSF) {
955 0 : if (!EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
956 : return false;
957 : }
958 : }
959 :
960 : // yaw measurement error variance (rad^2)
961 : ftype R_YAW;
962 1083882 : switch (method) {
963 4328 : case yawFusionMethod::GPS:
964 4328 : R_YAW = sq(yawAngDataDelayed.yawAngErr);
965 4328 : break;
966 :
967 0 : case yawFusionMethod::GSF:
968 0 : R_YAW = gsfYawVariance;
969 0 : break;
970 :
971 977 : case yawFusionMethod::STATIC:
972 977 : R_YAW = sq(yawAngDataStatic.yawAngErr);
973 977 : break;
974 :
975 1072323 : case yawFusionMethod::MAGNETOMETER:
976 : case yawFusionMethod::PREDICTED:
977 : default:
978 1072323 : R_YAW = sq(frontend->_yawNoise);
979 1072323 : break;
980 :
981 : #if EK3_FEATURE_EXTERNAL_NAV
982 6254 : case yawFusionMethod::EXTNAV:
983 6254 : R_YAW = sq(MAX(extNavYawAngDataDelayed.yawAngErr, 0.05f));
984 6254 : break;
985 : #endif
986 : }
987 :
988 : // determine if a 321 or 312 Euler sequence is best
989 : rotationOrder order;
990 1083882 : switch (method) {
991 4328 : case yawFusionMethod::GPS:
992 4328 : order = yawAngDataDelayed.order;
993 4328 : break;
994 :
995 977 : case yawFusionMethod::STATIC:
996 977 : order = yawAngDataStatic.order;
997 977 : break;
998 :
999 1072323 : case yawFusionMethod::MAGNETOMETER:
1000 : case yawFusionMethod::GSF:
1001 : case yawFusionMethod::PREDICTED:
1002 : default:
1003 : // determined automatically
1004 1072323 : order = (fabsF(prevTnb[0][2]) < fabsF(prevTnb[1][2])) ? rotationOrder::TAIT_BRYAN_321 : rotationOrder::TAIT_BRYAN_312;
1005 1072323 : break;
1006 :
1007 : #if EK3_FEATURE_EXTERNAL_NAV
1008 6254 : case yawFusionMethod::EXTNAV:
1009 6254 : order = extNavYawAngDataDelayed.order;
1010 6254 : break;
1011 : #endif
1012 : }
1013 :
1014 : // calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
1015 : ftype yawAngPredicted;
1016 : ftype H_YAW[4];
1017 1083882 : Matrix3F Tbn_zeroYaw;
1018 :
1019 1083882 : if (order == rotationOrder::TAIT_BRYAN_321) {
1020 : // calculate 321 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
1021 : bool canUseA = false;
1022 862625 : const ftype SA0 = 2*q3;
1023 862625 : const ftype SA1 = 2*q2;
1024 862625 : const ftype SA2 = SA0*q0 + SA1*q1;
1025 862625 : const ftype SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
1026 : ftype SA4, SA5_inv;
1027 862625 : if (is_positive(sq(SA3))) {
1028 853330 : SA4 = 1.0F/sq(SA3);
1029 853330 : SA5_inv = sq(SA2)*SA4 + 1;
1030 853330 : canUseA = is_positive(fabsF(SA5_inv));
1031 : }
1032 :
1033 : bool canUseB = false;
1034 862625 : const ftype SB0 = 2*q0;
1035 862625 : const ftype SB1 = 2*q1;
1036 862625 : const ftype SB2 = SB0*q3 + SB1*q2;
1037 862625 : const ftype SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
1038 : ftype SB3, SB5_inv;
1039 862625 : if (is_positive(sq(SB2))) {
1040 860647 : SB3 = 1.0F/sq(SB2);
1041 860647 : SB5_inv = SB3*sq(SB4) + 1;
1042 860647 : canUseB = is_positive(fabsF(SB5_inv));
1043 : }
1044 :
1045 862625 : if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
1046 535628 : const ftype SA5 = 1.0F/SA5_inv;
1047 535628 : const ftype SA6 = 1.0F/(SA3);
1048 535628 : const ftype SA7 = SA2*SA4;
1049 535628 : const ftype SA8 = 2*SA7;
1050 535628 : const ftype SA9 = 2*SA6;
1051 :
1052 535628 : H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
1053 535628 : H_YAW[1] = SA5*(SA1*SA6 - SA8*q1);
1054 535628 : H_YAW[2] = SA5*(SA1*SA7 + SA9*q1);
1055 535628 : H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
1056 862625 : } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
1057 326997 : const ftype SB5 = 1.0F/SB5_inv;
1058 326997 : const ftype SB6 = 1.0F/(SB2);
1059 326997 : const ftype SB7 = SB3*SB4;
1060 326997 : const ftype SB8 = 2*SB7;
1061 326997 : const ftype SB9 = 2*SB6;
1062 :
1063 326997 : H_YAW[0] = -SB5*(SB0*SB6 - SB8*q3);
1064 326997 : H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
1065 326997 : H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
1066 326997 : H_YAW[3] = -SB5*(-SB0*SB7 - SB9*q3);
1067 326997 : } else {
1068 : return false;
1069 : }
1070 :
1071 : // predicted yaw and zero yaw body to earth rotation matrix for this order
1072 862625 : if (!buildTbnZeroYaw(order, Tbn_zeroYaw, &yawAngPredicted)) {
1073 : // rotation order not supported: cannot build the predicted yaw and zero-yaw rotation
1074 : return false;
1075 : }
1076 :
1077 221257 : } else if (order == rotationOrder::TAIT_BRYAN_312) {
1078 : // calculate 312 yaw observation matrix - option A or B to avoid singularity in derivation at +-90 degrees yaw
1079 : bool canUseA = false;
1080 221257 : const ftype SA0 = 2*q3;
1081 221257 : const ftype SA1 = 2*q2;
1082 221257 : const ftype SA2 = SA0*q0 - SA1*q1;
1083 221257 : const ftype SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
1084 : ftype SA4, SA5_inv;
1085 221257 : if (is_positive(sq(SA3))) {
1086 219500 : SA4 = 1.0F/sq(SA3);
1087 219500 : SA5_inv = sq(SA2)*SA4 + 1;
1088 219500 : canUseA = is_positive(fabsF(SA5_inv));
1089 : }
1090 :
1091 : bool canUseB = false;
1092 221257 : const ftype SB0 = 2*q0;
1093 221257 : const ftype SB1 = 2*q1;
1094 221257 : const ftype SB2 = -SB0*q3 + SB1*q2;
1095 221257 : const ftype SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
1096 : ftype SB3, SB5_inv;
1097 221257 : if (is_positive(sq(SB2))) {
1098 220462 : SB3 = 1.0F/sq(SB2);
1099 220462 : SB5_inv = SB3*sq(SB4) + 1;
1100 220462 : canUseB = is_positive(fabsF(SB5_inv));
1101 : }
1102 :
1103 221257 : if (canUseA && (!canUseB || fabsF(SA5_inv) >= fabsF(SB5_inv))) {
1104 89320 : const ftype SA5 = 1.0F/SA5_inv;
1105 89320 : const ftype SA6 = 1.0F/(SA3);
1106 89320 : const ftype SA7 = SA2*SA4;
1107 89320 : const ftype SA8 = 2*SA7;
1108 89320 : const ftype SA9 = 2*SA6;
1109 :
1110 89320 : H_YAW[0] = SA5*(SA0*SA6 - SA8*q0);
1111 89320 : H_YAW[1] = SA5*(-SA1*SA6 + SA8*q1);
1112 89320 : H_YAW[2] = SA5*(-SA1*SA7 - SA9*q1);
1113 89320 : H_YAW[3] = SA5*(SA0*SA7 + SA9*q0);
1114 221257 : } else if (canUseB && (!canUseA || fabsF(SB5_inv) > fabsF(SA5_inv))) {
1115 131937 : const ftype SB5 = 1.0F/SB5_inv;
1116 131937 : const ftype SB6 = 1.0F/(SB2);
1117 131937 : const ftype SB7 = SB3*SB4;
1118 131937 : const ftype SB8 = 2*SB7;
1119 131937 : const ftype SB9 = 2*SB6;
1120 :
1121 131937 : H_YAW[0] = -SB5*(-SB0*SB6 + SB8*q3);
1122 131937 : H_YAW[1] = -SB5*(SB1*SB6 - SB8*q2);
1123 131937 : H_YAW[2] = -SB5*(-SB1*SB7 - SB9*q2);
1124 131937 : H_YAW[3] = -SB5*(SB0*SB7 + SB9*q3);
1125 131937 : } else {
1126 : return false;
1127 : }
1128 :
1129 : // predicted yaw and zero yaw body to earth rotation matrix for this order
1130 221257 : if (!buildTbnZeroYaw(order, Tbn_zeroYaw, &yawAngPredicted)) {
1131 : // rotation order not supported: cannot build the predicted yaw and zero-yaw rotation
1132 : return false;
1133 : }
1134 : } else {
1135 : // order not supported
1136 : return false;
1137 : }
1138 :
1139 : // Calculate the innovation
1140 1083882 : switch (method) {
1141 1071905 : case yawFusionMethod::MAGNETOMETER:
1142 : {
1143 : // Use the difference between the horizontal projection and declination to give the measured yaw
1144 : // rotate measured mag components into earth frame
1145 1071905 : Vector3F magMeasNED = Tbn_zeroYaw*magDataDelayed.mag;
1146 1071905 : ftype yawAngMeasured = wrap_PI(-atan2F(magMeasNED.y, magMeasNED.x) + MagDeclination());
1147 1071905 : innovYaw = wrap_PI(yawAngPredicted - yawAngMeasured);
1148 : break;
1149 : }
1150 :
1151 4328 : case yawFusionMethod::GPS:
1152 4328 : innovYaw = wrap_PI(yawAngPredicted - yawAngDataDelayed.yawAng);
1153 4328 : break;
1154 :
1155 977 : case yawFusionMethod::STATIC:
1156 977 : innovYaw = wrap_PI(yawAngPredicted - yawAngDataStatic.yawAng);
1157 977 : break;
1158 :
1159 0 : case yawFusionMethod::GSF:
1160 0 : innovYaw = wrap_PI(yawAngPredicted - gsfYaw);
1161 0 : break;
1162 :
1163 418 : case yawFusionMethod::PREDICTED:
1164 : default:
1165 418 : innovYaw = 0.0f;
1166 418 : break;
1167 :
1168 : #if EK3_FEATURE_EXTERNAL_NAV
1169 6254 : case yawFusionMethod::EXTNAV:
1170 6254 : innovYaw = wrap_PI(yawAngPredicted - extNavYawAngDataDelayed.yawAng);
1171 6254 : break;
1172 : #endif
1173 : }
1174 :
1175 : // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 4 elements in H are non zero
1176 : ftype PH[4];
1177 : ftype varInnov = R_YAW;
1178 5419410 : for (uint8_t rowIndex=0; rowIndex<=3; rowIndex++) {
1179 4335528 : PH[rowIndex] = 0.0f;
1180 21677640 : for (uint8_t colIndex=0; colIndex<=3; colIndex++) {
1181 17342112 : PH[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
1182 : }
1183 4335528 : varInnov += H_YAW[rowIndex]*PH[rowIndex];
1184 : }
1185 : ftype varInnovInv;
1186 1083882 : if (varInnov >= R_YAW) {
1187 1083882 : varInnovInv = 1.0f / varInnov;
1188 : // output numerical health status
1189 1083882 : faultStatus.bad_yaw = false;
1190 : } else {
1191 : // the calculation is badly conditioned, so we cannot perform fusion on this step
1192 : // we reset the covariance matrix and try again next measurement
1193 0 : CovarianceInit();
1194 : // output numerical health status
1195 0 : faultStatus.bad_yaw = true;
1196 0 : return false;
1197 : }
1198 :
1199 : // calculate Kalman gain
1200 18451124 : for (uint8_t rowIndex=0; rowIndex<=stateIndexLim; rowIndex++) {
1201 17367242 : Kfusion[rowIndex] = 0.0f;
1202 86836210 : for (uint8_t colIndex=0; colIndex<=3; colIndex++) {
1203 69468968 : Kfusion[rowIndex] += P[rowIndex][colIndex]*H_YAW[colIndex];
1204 : }
1205 17367242 : Kfusion[rowIndex] *= varInnovInv;
1206 : }
1207 :
1208 : // calculate the innovation test ratio
1209 1083882 : yawTestRatio = sq(innovYaw) / (sq(MAX(0.01f * (ftype)frontend->_yawInnovGate, 1.0f)) * varInnov);
1210 :
1211 : // Declare the magnetometer unhealthy if the innovation test fails
1212 1083882 : if (yawTestRatio > 1.0f) {
1213 24780 : magHealth = false;
1214 : // On the ground a large innovation could be due to large initial gyro bias or magnetic interference from nearby objects
1215 : // If we are flying, then it is more likely due to a magnetometer fault and we should not fuse the data
1216 24780 : if (inFlight) {
1217 : return false;
1218 : }
1219 : } else {
1220 1059102 : magHealth = true;
1221 : }
1222 :
1223 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
1224 : // the zero elements of H to reduce the number of operations.
1225 18444766 : for (unsigned i = 0; i<=stateIndexLim; i++) {
1226 : // j as the inner loop allows the compiler to hoist the KH product
1227 : // to save computation, and do the inner indexing more efficiently.
1228 295736574 : for (unsigned j = 0; j<=stateIndexLim; j++) {
1229 : ftype res = 0;
1230 278375316 : res += (Kfusion[i] * H_YAW[0]) * P[0][j];
1231 278375316 : res += (Kfusion[i] * H_YAW[1]) * P[1][j];
1232 278375316 : res += (Kfusion[i] * H_YAW[2]) * P[2][j];
1233 278375316 : res += (Kfusion[i] * H_YAW[3]) * P[3][j];
1234 278375316 : KHP[i][j] = res;
1235 : }
1236 : }
1237 :
1238 1083508 : const ftype innovFusion = constrain_ftype(innovYaw, -0.5f, 0.5f);
1239 : // finish fusion from KHP and Kfusion then record health status
1240 1083508 : faultStatus.bad_yaw = FinishFusion(innovFusion);
1241 :
1242 1083508 : return true;
1243 : }
1244 :
1245 : /*
1246 : * Fuse declination angle using explicit algebraic equations generated in
1247 : * derivation/generate_2.py with output recorded in derivation/generated/yaw_generated.cpp
1248 : */
1249 1674940 : void NavEKF3_core::FuseDeclination(ftype declErr)
1250 : {
1251 : // declination error variance (rad^2)
1252 1674940 : const ftype R_DECL = sq(declErr);
1253 :
1254 : // copy required states to local variables
1255 1674940 : ftype magN = stateStruct.earth_magfield.x;
1256 1674940 : ftype magE = stateStruct.earth_magfield.y;
1257 :
1258 : // prevent bad earth field states from causing numerical errors or exceptions
1259 1674940 : if (magN < 1e-3f) {
1260 : return;
1261 : }
1262 :
1263 : // Calculate observation Jacobian and Kalman gains
1264 : // Calculate intermediate variables
1265 1674932 : const ftype HK0 = sq(magE) + sq(magN);
1266 : // if the horizontal magnetic field is too small, this calculation will be badly conditioned
1267 1674932 : if (HK0 < 1e-4f) {
1268 : return;
1269 : }
1270 1674932 : const ftype HK1 = 1.0F/(HK0);
1271 1674932 : const ftype HK2 = P[16][16]*magE - P[16][17]*magN;
1272 1674932 : const ftype HK3 = P[16][17]*magE - P[17][17]*magN;
1273 1674932 : const ftype HK4_denom = (sq(HK0)*R_DECL + HK2*magE - HK3*magN);
1274 : ftype HK4;
1275 1674932 : if (fabsF(HK4_denom) > 1e-6f) {
1276 1674932 : HK4 = HK0/HK4_denom;
1277 : } else {
1278 : return;
1279 : }
1280 :
1281 : // Calculate the observation Jacobian
1282 : // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
1283 : ftype Hfusion[24] = {};
1284 1674932 : Hfusion[16] = -HK1*magE;
1285 1674932 : Hfusion[17] = HK1*magN;
1286 :
1287 : uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero)
1288 :
1289 1674932 : if (!inhibitDelAngBiasStates) {
1290 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
1291 : }
1292 :
1293 1674932 : if (!inhibitDelVelBiasStates) {
1294 6699472 : for (uint8_t index = 0; index < 3; index++) {
1295 5024604 : const uint8_t stateIndex = index + 13;
1296 5024604 : if (!dvelBiasAxisInhibit[index]) {
1297 5020720 : kalman_mask |= (1<<stateIndex);
1298 : }
1299 : }
1300 : }
1301 :
1302 1674932 : if (!inhibitMagStates) {
1303 1674932 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
1304 : }
1305 :
1306 1674932 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
1307 605889 : kalman_mask |= (1<<22) | (1<<23);
1308 : }
1309 :
1310 41873300 : for (auto i=0; i<24; i++) {
1311 : ftype res = 0;
1312 40198368 : if (kalman_mask & (1<<i)) {
1313 38056014 : res = -HK4*(P[i][16]*magE-P[i][17]*magN);
1314 : }
1315 40198368 : Kfusion[i] = res;
1316 : }
1317 :
1318 : // get the magnetic declination
1319 1674932 : ftype magDecAng = MagDeclination();
1320 :
1321 : // Calculate the innovation
1322 1674932 : ftype innovation = atan2F(magE , magN) - magDecAng;
1323 :
1324 : // limit the innovation to protect against data errors
1325 1674932 : if (innovation > 0.5f) {
1326 : innovation = 0.5f;
1327 1674932 : } else if (innovation < -0.5f) {
1328 : innovation = -0.5f;
1329 : }
1330 :
1331 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
1332 : // the zero elements of H to reduce the number of operations.
1333 39748054 : for (unsigned i = 0; i<=stateIndexLim; i++) {
1334 : // j as the inner loop allows the compiler to hoist the KH product
1335 : // to save computation, and do the inner indexing more efficiently.
1336 905072638 : for (unsigned j = 0; j<=stateIndexLim; j++) {
1337 : ftype res = 0;
1338 866999516 : res += (Kfusion[i] * Hfusion[16]) * P[16][j];
1339 866999516 : res += (Kfusion[i] * Hfusion[17]) * P[17][j];
1340 866999516 : KHP[i][j] = res;
1341 : }
1342 : }
1343 :
1344 : // finish fusion from KHP and Kfusion then record health status
1345 1674932 : faultStatus.bad_decl = FinishFusion(innovation);
1346 : }
1347 :
1348 : /********************************************************
1349 : * MISC FUNCTIONS *
1350 : ********************************************************/
1351 :
1352 : // align the NE earth magnetic field states with the published declination
1353 7819 : void NavEKF3_core::alignMagStateDeclination()
1354 : {
1355 : // don't do this if we already have a learned magnetic field
1356 7819 : if (magFieldLearned) {
1357 0 : return;
1358 : }
1359 :
1360 : // get the magnetic declination
1361 7819 : ftype magDecAng = MagDeclination();
1362 :
1363 : // rotate the NE values so that the declination matches the published value
1364 7819 : Vector3F initMagNED = stateStruct.earth_magfield;
1365 7819 : ftype magLengthNE = initMagNED.xy().length();
1366 7819 : stateStruct.earth_magfield.x = magLengthNE * cosF(magDecAng);
1367 7819 : stateStruct.earth_magfield.y = magLengthNE * sinF(magDecAng);
1368 :
1369 7819 : if (!inhibitMagStates) {
1370 : // zero the corresponding state covariances if magnetic field state learning is active
1371 735 : ftype var_16 = P[16][16];
1372 735 : ftype var_17 = P[17][17];
1373 735 : zeroStatesVarCov(16, 17);
1374 735 : P[16][16] = var_16;
1375 735 : P[17][17] = var_17;
1376 :
1377 : // fuse the declination angle to establish covariances and prevent large swings in declination
1378 : // during initial fusion
1379 735 : FuseDeclination(0.1f);
1380 :
1381 : }
1382 : }
1383 :
1384 : // record a magnetic field state reset event
1385 4205 : void NavEKF3_core::recordMagReset()
1386 : {
1387 4205 : magStateResetRequest = false;
1388 4205 : magStateInitComplete = true;
1389 4205 : if (inFlight) {
1390 1094 : finalInflightMagInit = true;
1391 : }
1392 : // take a snap-shot of the vertical position, quaternion and yaw innovation to use as a reference
1393 : // for post alignment checks
1394 4205 : posDownAtLastMagReset = stateStruct.position.z;
1395 4205 : quatAtLastMagReset = stateStruct.quat;
1396 4205 : yawInnovAtLastMagReset = innovYaw;
1397 4205 : }
1398 :
1399 : /*
1400 : learn magnetometer biases from GPS yaw. Return true if the
1401 : resulting mag vector is close enough to the one predicted by GPS
1402 : yaw to use it for fallback
1403 : */
1404 1932 : bool NavEKF3_core::learnMagBiasFromGPS(void)
1405 : {
1406 1932 : if (!have_table_earth_field) {
1407 : // we need the earth field from WMM
1408 : return false;
1409 : }
1410 1518 : if (!inFlight) {
1411 : // don't start learning till we've started flying
1412 : return false;
1413 : }
1414 :
1415 632 : mag_elements mag_data;
1416 632 : if (!storedMag.recall(mag_data, imuDataDelayed.time_ms)) {
1417 : // no mag data to correct
1418 : return false;
1419 : }
1420 :
1421 : // combine yaw with current quaternion to get yaw corrected quaternion
1422 632 : QuaternionF quat = stateStruct.quat;
1423 632 : if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_321) {
1424 632 : Vector3F euler321;
1425 632 : quat.to_euler(euler321.x, euler321.y, euler321.z);
1426 632 : quat.from_euler(euler321.x, euler321.y, yawAngDataDelayed.yawAng);
1427 0 : } else if (yawAngDataDelayed.order == rotationOrder::TAIT_BRYAN_312) {
1428 0 : Vector3F euler312 = quat.to_vector312();
1429 0 : quat.from_vector312(euler312.x, euler312.y, yawAngDataDelayed.yawAng);
1430 : } else {
1431 : // rotation order not supported
1432 : return false;
1433 : }
1434 :
1435 : // build the expected body field from orientation and table earth field
1436 632 : Matrix3F dcm;
1437 632 : quat.rotation_matrix(dcm);
1438 632 : Vector3F expected_body_field = dcm.transposed() * table_earth_field_ga;
1439 :
1440 : // calculate error in field
1441 632 : Vector3F err = (expected_body_field - mag_data.mag) + stateStruct.body_magfield;
1442 :
1443 : // learn body frame mag biases
1444 632 : stateStruct.body_magfield -= err * EK3_GPS_MAG_LEARN_RATE;
1445 :
1446 : // check if error is below threshold. If it is then we can
1447 : // fallback to magnetometer on failure of external yaw
1448 632 : ftype err_length = err.length();
1449 :
1450 : // we allow for yaw backback to compass if we have had 50 samples
1451 : // in a row below the threshold. This corresponds to 10 seconds
1452 : // for a 5Hz GPS
1453 : const uint8_t fallback_count_threshold = 50;
1454 :
1455 632 : if (err_length > EK3_GPS_MAG_LEARN_LIMIT) {
1456 2 : gps_yaw_fallback_good_counter = 0;
1457 630 : } else if (gps_yaw_fallback_good_counter < fallback_count_threshold) {
1458 318 : gps_yaw_fallback_good_counter++;
1459 : }
1460 632 : bool ok = gps_yaw_fallback_good_counter >= fallback_count_threshold;
1461 632 : if (ok) {
1462 : // mark mag healthy to prevent a magTimeout when we start using it
1463 318 : lastHealthyMagTime_ms = imuSampleTime_ms;
1464 : }
1465 : return ok;
1466 : }
1467 :
1468 : // Reset states using yaw from EKF-GSF and velocity and position from GPS
1469 1670 : bool NavEKF3_core::EKFGSF_resetMainFilterYaw(bool emergency_reset)
1470 : {
1471 : // Don't do a reset unless permitted by the EK3_GSF_USE_MASK and EK3_GSF_RUN_MASK parameter masks
1472 1670 : if ((yawEstimator == nullptr)
1473 1670 : || !(frontend->_gsfUseMask & (1U<<core_index))) {
1474 0 : return false;
1475 : };
1476 :
1477 : // limit the number of emergency resets
1478 1670 : if (emergency_reset && (EKFGSF_yaw_reset_count >= frontend->_gsfResetMaxCount)) {
1479 : return false;
1480 : }
1481 :
1482 : ftype yawEKFGSF, yawVarianceEKFGSF;
1483 1670 : if (EKFGSF_getYaw(yawEKFGSF, yawVarianceEKFGSF)) {
1484 : // keep roll and pitch and reset yaw
1485 : rotationOrder order;
1486 4 : bestRotationOrder(order);
1487 4 : resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, order);
1488 :
1489 : // record the emergency reset event
1490 4 : EKFGSF_yaw_reset_request_ms = 0;
1491 4 : EKFGSF_yaw_reset_ms = imuSampleTime_ms;
1492 4 : EKFGSF_yaw_reset_count++;
1493 :
1494 4 : if ((yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF) ||
1495 6 : !use_compass() || (dal.compass().get_num_enabled() == 0)) {
1496 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u yaw aligned using GPS",(unsigned)imu_index);
1497 : } else {
1498 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u emergency yaw reset",(unsigned)imu_index);
1499 : }
1500 :
1501 : // Fail the magnetomer so it doesn't get used and pull the yaw away from the correct value
1502 4 : if (emergency_reset) {
1503 2 : allMagSensorsFailed = true;
1504 : }
1505 :
1506 : // record the yaw reset event
1507 4 : recordYawResetsCompleted();
1508 :
1509 : // reset velocity and position states to GPS - if yaw is fixed then the filter should start to operate correctly
1510 4 : ResetVelocity(resetDataSource::DEFAULT);
1511 4 : ResetPosition(resetDataSource::DEFAULT);
1512 :
1513 : // reset test ratios that are reported to prevent a race condition with the external state machine requesting the reset
1514 4 : velTestRatio = 0.0f;
1515 4 : posTestRatio = 0.0f;
1516 :
1517 : return true;
1518 :
1519 : }
1520 :
1521 : return false;
1522 :
1523 : }
1524 :
1525 : // returns true on success and populates yaw (in radians) and yawVariance (rad^2)
1526 1009421 : bool NavEKF3_core::EKFGSF_getYaw(ftype &yaw, ftype &yawVariance) const
1527 : {
1528 : // return immediately if no yaw estimator
1529 1009421 : if (yawEstimator == nullptr) {
1530 : return false;
1531 : }
1532 :
1533 : ftype velInnovLength;
1534 2017652 : if (yawEstimator->getYawData(yaw, yawVariance) &&
1535 1008231 : is_positive(yawVariance) &&
1536 2674763 : yawVariance < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG)) &&
1537 999733 : (assume_zero_sideslip() || (yawEstimator->getVelInnovLength(velInnovLength) && velInnovLength < frontend->maxYawEstVelInnov))) {
1538 655897 : return true;
1539 : }
1540 :
1541 : return false;
1542 : }
1543 :
1544 4237 : void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order)
1545 : {
1546 4237 : QuaternionF quatBeforeReset = stateStruct.quat;
1547 :
1548 : // check if we should use a 321 or 312 Rotation order and update the quaternion
1549 : // states using the preferred yaw definition
1550 4237 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
1551 4237 : Vector3F eulerAngles;
1552 4237 : if (order == rotationOrder::TAIT_BRYAN_321) {
1553 : // rolled more than pitched so use 321 rotation order
1554 3017 : stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
1555 3017 : stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, yaw);
1556 1220 : } else if (order == rotationOrder::TAIT_BRYAN_312) {
1557 : // pitched more than rolled so use 312 rotation order
1558 1220 : eulerAngles = stateStruct.quat.to_vector312();
1559 1220 : stateStruct.quat.from_vector312(eulerAngles.x, eulerAngles.y, yaw);
1560 : } else {
1561 : // rotation order not supported
1562 0 : return;
1563 : }
1564 :
1565 : // Update the rotation matrix
1566 4237 : stateStruct.quat.inverse().rotation_matrix(prevTnb);
1567 :
1568 : // calculate the change in the quaternion state and apply it to the output history buffer
1569 4237 : QuaternionF quat_delta = stateStruct.quat / quatBeforeReset;
1570 4237 : StoreQuatRotate(quat_delta);
1571 :
1572 : // assume tilt uncertainty split equally between roll and pitch
1573 4237 : Vector3F angleErrVarVec = Vector3F(0.5 * tiltErrorVariance, 0.5 * tiltErrorVariance, yawVariance);
1574 4237 : CovariancePrediction(&angleErrVarVec);
1575 :
1576 : // record the yaw reset event
1577 4237 : yawResetCount++;
1578 :
1579 : // record the yaw reset event
1580 4237 : recordYawResetsCompleted();
1581 : }
|
