Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_core.h"
5 : #include <GCS_MAVLink/GCS.h>
6 : #include <AP_DAL/AP_DAL.h>
7 :
8 : /********************************************************
9 : * RESET FUNCTIONS *
10 : ********************************************************/
11 :
12 : // Reset XY velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
13 : // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
14 4930 : void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
15 : {
16 : // if reset source is not specified then use user defined velocity source
17 4930 : if (velResetSource == resetDataSource::DEFAULT) {
18 3310 : switch (frontend->sources.getVelXYSource(core_index)) {
19 : case AP_NavEKF_Source::SourceXY::GPS:
20 : velResetSource = resetDataSource::GPS;
21 : break;
22 : case AP_NavEKF_Source::SourceXY::BEACON:
23 : velResetSource = resetDataSource::RNGBCN;
24 : break;
25 : case AP_NavEKF_Source::SourceXY::EXTNAV:
26 : velResetSource = resetDataSource::EXTNAV;
27 : break;
28 : case AP_NavEKF_Source::SourceXY::NONE:
29 : case AP_NavEKF_Source::SourceXY::OPTFLOW:
30 : case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
31 : // unhandled sources so stick with the default
32 : break;
33 : }
34 : }
35 :
36 : // reset the corresponding covariances
37 4930 : zeroStatesVarCov(4, 5);
38 :
39 4930 : if (PV_AidingMode != AID_ABSOLUTE) {
40 3298 : stateStruct.velocity.xy().zero();
41 : // set the variances using the measurement noise parameter
42 3298 : P[5][5] = P[4][4] = sq(frontend->_gpsHorizVelNoise);
43 : } else {
44 : // reset horizontal velocity states to the GPS velocity if available
45 1632 : if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250) && (velResetSource == resetDataSource::DEFAULT || velResetSource == resetDataSource::GPS)) {
46 : // correct for antenna position
47 1608 : gps_elements gps_corrected = gpsDataNew;
48 1608 : CorrectGPSForAntennaOffset(gps_corrected);
49 1608 : stateStruct.velocity.x = gps_corrected.vel.x;
50 1608 : stateStruct.velocity.y = gps_corrected.vel.y;
51 : // set the variances using the reported GPS speed accuracy
52 1608 : P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
53 : #if EK3_FEATURE_EXTERNAL_NAV
54 1632 : } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250) && (velResetSource == resetDataSource::DEFAULT || velResetSource == resetDataSource::EXTNAV)) {
55 : // use external nav data as the 2nd preference
56 : // already corrected for sensor position
57 5 : stateStruct.velocity.x = extNavVelDelayed.vel.x;
58 5 : stateStruct.velocity.y = extNavVelDelayed.vel.y;
59 5 : P[5][5] = P[4][4] = sq(extNavVelDelayed.err);
60 : #endif // EK3_FEATURE_EXTERNAL_NAV
61 : } else {
62 19 : stateStruct.velocity.x = 0.0f;
63 19 : stateStruct.velocity.y = 0.0f;
64 : // set the variances using the likely speed range
65 19 : P[5][5] = P[4][4] = sq(25.0f);
66 : }
67 : // clear the timeout flags and counters
68 1632 : velTimeout = false;
69 1632 : lastVelPassTime_ms = imuSampleTime_ms;
70 : }
71 49754 : for (uint8_t i=0; i<imu_buffer_length; i++) {
72 44824 : storedOutput[i].velocity.x = stateStruct.velocity.x;
73 44824 : storedOutput[i].velocity.y = stateStruct.velocity.y;
74 : }
75 4930 : outputDataNew.velocity.x = stateStruct.velocity.x;
76 4930 : outputDataNew.velocity.y = stateStruct.velocity.y;
77 4930 : outputDataDelayed.velocity.x = stateStruct.velocity.x;
78 4930 : outputDataDelayed.velocity.y = stateStruct.velocity.y;
79 :
80 4930 : }
81 :
82 : // resets position states to last GPS measurement or to zero if in constant position mode
83 4997 : void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
84 : {
85 : // if reset source is not specified thenn use the user defined position source
86 4997 : if (posResetSource == resetDataSource::DEFAULT) {
87 3371 : switch (frontend->sources.getPosXYSource(core_index)) {
88 : case AP_NavEKF_Source::SourceXY::GPS:
89 : posResetSource = resetDataSource::GPS;
90 : break;
91 : case AP_NavEKF_Source::SourceXY::BEACON:
92 : posResetSource = resetDataSource::RNGBCN;
93 : break;
94 : case AP_NavEKF_Source::SourceXY::EXTNAV:
95 : posResetSource = resetDataSource::EXTNAV;
96 : break;
97 : case AP_NavEKF_Source::SourceXY::NONE:
98 : case AP_NavEKF_Source::SourceXY::OPTFLOW:
99 : case AP_NavEKF_Source::SourceXY::WHEEL_ENCODER:
100 : // invalid sources so stick with the default
101 : break;
102 : }
103 : }
104 :
105 : // Store the position before the reset so that we can record the reset delta
106 4997 : posResetNE.x = stateStruct.position.x;
107 4997 : posResetNE.y = stateStruct.position.y;
108 :
109 : // reset the corresponding covariances
110 4997 : zeroStatesVarCov(7, 8);
111 :
112 4997 : if (PV_AidingMode != AID_ABSOLUTE) {
113 : // reset all position state history to the last known position
114 3298 : stateStruct.position.x = lastKnownPositionNE.x;
115 3298 : stateStruct.position.y = lastKnownPositionNE.y;
116 : // set the variances using the position measurement noise parameter
117 3298 : P[7][7] = P[8][8] = sq(frontend->_gpsHorizPosNoise);
118 : } else {
119 : // Use GPS data as first preference if fresh data is available
120 1699 : if ((imuSampleTime_ms - lastTimeGpsReceived_ms < 250) && (posResetSource == resetDataSource::DEFAULT || posResetSource == resetDataSource::GPS)) {
121 : // correct for antenna position
122 1676 : gps_elements gps_corrected = gpsDataNew;
123 1676 : CorrectGPSForAntennaOffset(gps_corrected);
124 : // record the ID of the GPS for the data we are using for the reset
125 1676 : last_gps_idx = gps_corrected.sensor_idx;
126 : // calculate position
127 1676 : const Location gpsloc{gps_corrected.lat, gps_corrected.lng, 0, Location::AltFrame::ABSOLUTE};
128 1676 : stateStruct.position.xy() = EKF_origin.get_distance_NE_ftype(gpsloc);
129 : // compensate for offset between last GPS measurement and the EKF time horizon. Note that this is an unusual
130 : // time delta in that it can be both -ve and +ve
131 1676 : const int32_t tdiff = imuDataDelayed.time_ms - gps_corrected.time_ms;
132 1676 : stateStruct.position.xy() += gps_corrected.vel.xy()*0.001*tdiff;
133 : // set the variances using the position measurement noise parameter
134 1676 : P[7][7] = P[8][8] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
135 : #if EK3_FEATURE_BEACON_FUSION
136 1699 : } else if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250) && (posResetSource == resetDataSource::DEFAULT || posResetSource == resetDataSource::RNGBCN)) {
137 : // use the range beacon data as a second preference
138 6 : stateStruct.position.x = rngBcn.receiverPos.x;
139 6 : stateStruct.position.y = rngBcn.receiverPos.y;
140 : // set the variances from the beacon alignment filter
141 6 : P[7][7] = rngBcn.receiverPosCov[0][0];
142 6 : P[8][8] = rngBcn.receiverPosCov[1][1];
143 : #endif
144 : #if EK3_FEATURE_EXTERNAL_NAV
145 17 : } else if ((imuSampleTime_ms - extNavDataDelayed.time_ms < 250) && (posResetSource == resetDataSource::DEFAULT || posResetSource == resetDataSource::EXTNAV)) {
146 : // use external nav data as the third preference
147 5 : stateStruct.position.x = extNavDataDelayed.pos.x;
148 5 : stateStruct.position.y = extNavDataDelayed.pos.y;
149 : // set the variances as received from external nav system data
150 5 : P[7][7] = P[8][8] = sq(extNavDataDelayed.posErr);
151 : #endif // EK3_FEATURE_EXTERNAL_NAV
152 : }
153 : }
154 50424 : for (uint8_t i=0; i<imu_buffer_length; i++) {
155 45427 : storedOutput[i].position.x = stateStruct.position.x;
156 45427 : storedOutput[i].position.y = stateStruct.position.y;
157 : }
158 4997 : outputDataNew.position.x = stateStruct.position.x;
159 4997 : outputDataNew.position.y = stateStruct.position.y;
160 4997 : outputDataDelayed.position.x = stateStruct.position.x;
161 4997 : outputDataDelayed.position.y = stateStruct.position.y;
162 :
163 : // Calculate the position jump due to the reset
164 4997 : posResetNE.x = stateStruct.position.x - posResetNE.x;
165 4997 : posResetNE.y = stateStruct.position.y - posResetNE.y;
166 :
167 4997 : posNEResetCount++;
168 :
169 : // clear the timeout flags and counters
170 4997 : posTimeout = false;
171 4997 : lastGpsPosPassTime_ms = imuSampleTime_ms;
172 4997 : }
173 :
174 : #if EK3_FEATURE_POSITION_RESET
175 : // Sets the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and optionally uncertainty
176 : // The altitude element of the location is not used. If accuracy is not known should be passed as NaN.
177 : // Returns true if the set was successful
178 4 : bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
179 : {
180 4 : if ((imuSampleTime_ms - lastGpsPosPassTime_ms) < frontend->deadReckonDeclare_ms ||
181 2 : (PV_AidingMode == AID_NONE)
182 2 : || !validOrigin) {
183 : return false;
184 : }
185 :
186 : // Store the position before the reset so that we can record the reset delta
187 2 : posResetNE.x = stateStruct.position.x;
188 2 : posResetNE.y = stateStruct.position.y;
189 :
190 : // reset the corresponding covariances
191 2 : zeroStatesVarCov(7, 8);
192 :
193 : // handle unknown accuracy
194 2 : if (isnan(posAccuracy)) {
195 0 : posAccuracy = 0.0f; // will be ignored due to MAX below
196 : }
197 :
198 : // set the variances using the position measurement noise parameter
199 2 : P[7][7] = P[8][8] = sq(MAX(posAccuracy,frontend->_gpsHorizPosNoise));
200 :
201 : // Correct the position for time delay relative to fusion time horizon assuming a constant velocity
202 : // Limit time stamp to a range between current time and 5 seconds ago
203 2 : const uint32_t timeStampConstrained_ms = MAX(MIN(timestamp_ms, imuSampleTime_ms), imuSampleTime_ms - 5000);
204 2 : const int32_t delta_ms = int32_t(imuDataDelayed.time_ms - timeStampConstrained_ms);
205 2 : const ftype delaySec = 1E-3F * ftype(delta_ms);
206 2 : const Vector2F newPosNE = EKF_origin.get_distance_NE_ftype(loc) + stateStruct.velocity.xy() * delaySec;
207 2 : ResetPositionNE(newPosNE.x,newPosNE.y);
208 :
209 2 : return true;
210 : }
211 : #endif // EK3_FEATURE_POSITION_RESET
212 :
213 :
214 : // reset the stateStruct's NE position to the specified position
215 : // posResetNE is updated to hold the change in position
216 : // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
217 : // posNEResetCount is incremented to record the reset
218 22 : void NavEKF3_core::ResetPositionNE(ftype posN, ftype posE)
219 : {
220 : // Store the position before the reset so that we can record the reset delta
221 22 : const Vector3F posOrig = stateStruct.position;
222 :
223 : // Set the position states to the new position
224 22 : stateStruct.position.x = posN;
225 22 : stateStruct.position.y = posE;
226 :
227 : // Calculate the position offset due to the reset
228 22 : posResetNE.x = stateStruct.position.x - posOrig.x;
229 22 : posResetNE.y = stateStruct.position.y - posOrig.y;
230 :
231 : // Add the offset to the output observer states
232 205 : for (uint8_t i=0; i<imu_buffer_length; i++) {
233 183 : storedOutput[i].position.x += posResetNE.x;
234 183 : storedOutput[i].position.y += posResetNE.y;
235 : }
236 22 : outputDataNew.position.x += posResetNE.x;
237 22 : outputDataNew.position.y += posResetNE.y;
238 22 : outputDataDelayed.position.x += posResetNE.x;
239 22 : outputDataDelayed.position.y += posResetNE.y;
240 :
241 22 : posNEResetCount++;
242 22 : }
243 :
244 : // reset the stateStruct's D position
245 : // posResetD is updated to hold the change in position
246 : // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
247 : // posDResetCount is incremented to record the reset
248 27 : void NavEKF3_core::ResetPositionD(ftype posD)
249 : {
250 : // Store the position before the reset so that we can record the reset delta
251 27 : const ftype posDOrig = stateStruct.position.z;
252 :
253 : // write to the state vector
254 27 : stateStruct.position.z = posD;
255 :
256 : // Calculate the position jump due to the reset
257 27 : posResetD = stateStruct.position.z - posDOrig;
258 :
259 : // Add the offset to the output observer states
260 27 : outputDataNew.position.z += posResetD;
261 27 : vertCompFiltState.pos = outputDataNew.position.z;
262 27 : outputDataDelayed.position.z += posResetD;
263 252 : for (uint8_t i=0; i<imu_buffer_length; i++) {
264 225 : storedOutput[i].position.z += posResetD;
265 : }
266 :
267 27 : posDResetCount++;
268 27 : }
269 :
270 : // reset the vertical position state using the last height measurement
271 3198 : void NavEKF3_core::ResetHeight(void)
272 : {
273 : // Store the position before the reset so that we can record the reset delta
274 3198 : posResetD = stateStruct.position.z;
275 :
276 : // write to the state vector
277 3198 : stateStruct.position.z = -hgtMea;
278 3198 : outputDataNew.position.z = stateStruct.position.z;
279 3198 : outputDataDelayed.position.z = stateStruct.position.z;
280 :
281 : // reset the terrain state height
282 3198 : if (onGround) {
283 : // assume vehicle is sitting on the ground
284 3198 : terrainState = stateStruct.position.z + rngOnGnd;
285 : } else {
286 : // can make no assumption other than vehicle is not below ground level
287 0 : terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState);
288 : }
289 32258 : for (uint8_t i=0; i<imu_buffer_length; i++) {
290 29060 : storedOutput[i].position.z = stateStruct.position.z;
291 : }
292 3198 : vertCompFiltState.pos = stateStruct.position.z;
293 :
294 : // Calculate the position jump due to the reset
295 3198 : posResetD = stateStruct.position.z - posResetD;
296 :
297 3198 : posDResetCount++;
298 :
299 : // clear the timeout flags and counters
300 3198 : hgtTimeout = false;
301 3198 : lastHgtPassTime_ms = imuSampleTime_ms;
302 :
303 : // reset the corresponding covariances
304 3198 : zeroStatesVarCov(9, 9);
305 :
306 : // set the variances to the measurement variance
307 3198 : P[9][9] = posDownObsNoise;
308 :
309 : // Reset the vertical velocity state using GPS vertical velocity if we are airborne
310 : // Check that GPS vertical velocity data is available and can be used
311 3198 : if (inFlight &&
312 0 : (gpsIsInUse || badIMUdata) &&
313 0 : frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS, core_index) &&
314 3198 : gpsDataNew.have_vz &&
315 0 : (imuSampleTime_ms - gpsDataDelayed.time_ms < 500)) {
316 0 : stateStruct.velocity.z = gpsDataNew.vel.z;
317 : #if EK3_FEATURE_EXTERNAL_NAV
318 3198 : } else if (inFlight && useExtNavVel && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
319 0 : stateStruct.velocity.z = extNavVelDelayed.vel.z;
320 : #endif
321 3198 : } else if (onGround) {
322 3198 : stateStruct.velocity.z = 0.0f;
323 : }
324 32258 : for (uint8_t i=0; i<imu_buffer_length; i++) {
325 29060 : storedOutput[i].velocity.z = stateStruct.velocity.z;
326 : }
327 3198 : outputDataNew.velocity.z = stateStruct.velocity.z;
328 3198 : outputDataDelayed.velocity.z = stateStruct.velocity.z;
329 3198 : vertCompFiltState.vel = outputDataNew.velocity.z;
330 :
331 : // reset the corresponding covariances
332 3198 : zeroStatesVarCov(6, 6);
333 :
334 : // set the variances to the measurement variance
335 : #if EK3_FEATURE_EXTERNAL_NAV
336 3198 : if (useExtNavVel) {
337 7 : P[6][6] = sq(extNavVelDelayed.err);
338 : } else
339 : #endif
340 : {
341 3191 : P[6][6] = sq(frontend->_gpsVertVelNoise);
342 : }
343 3198 : vertVelVarClipCounter = 0;
344 3198 : }
345 :
346 : // Zero the EKF height datum
347 : // Return true if the height datum reset has been performed
348 5885 : bool NavEKF3_core::resetHeightDatum(void)
349 : {
350 5885 : if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER || !onGround) {
351 : // only allow resets when on the ground.
352 : // If using using rangefinder for height then never perform a
353 : // reset of the height datum
354 : return false;
355 : }
356 : // record the old height estimate
357 5283 : ftype oldHgt = -stateStruct.position.z;
358 : // reset the barometer so that it reads zero at the current height
359 5283 : dal.baro().update_calibration();
360 :
361 : // clear the baro data buffer
362 5283 : storedBaro.reset();
363 :
364 : // reset the vertical position and velocity states
365 5283 : stateStruct.position.z = 0.0f;
366 5283 : stateStruct.velocity.z = 0.0f;
367 53235 : for (uint8_t i=0; i<imu_buffer_length; i++) {
368 47952 : storedOutput[i].position.z = stateStruct.position.z;
369 47952 : storedOutput[i].velocity.z = stateStruct.velocity.z;
370 : }
371 5283 : outputDataNew.position.z = outputDataDelayed.position.z = stateStruct.position.z;
372 5283 : outputDataNew.velocity.z = outputDataDelayed.velocity.z = stateStruct.velocity.z;
373 5283 : vertCompFiltState.vel = outputDataNew.velocity.z;
374 :
375 : // baroHgtOffset is a slow first-order filter (calcFiltBaroOffset)
376 : // tracking baroDataDelayed.hgt + position.z. Post-reset baro
377 : // reads 0 and position.z is 0 so the steady-state offset is 0;
378 : // without this, hgtMea = baroDataDelayed.hgt - baroHgtOffset
379 : // would feed a non-zero observation into the EKF for the ~1 s
380 : // the filter takes to relax, producing a post-reset altitude
381 : // transient.
382 5283 : baroHgtOffset = 0.0f;
383 :
384 : // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
385 5283 : if (validOrigin) {
386 4121 : if (!gpsGoodToAlign) {
387 : // if we don't have GPS lock then we shouldn't be doing a
388 : // resetHeightDatum, but if we do then the best option is
389 : // to maintain the old error
390 319 : EKF_origin.alt += (int32_t)(100.0f * oldHgt);
391 : } else {
392 : // if we have a good GPS lock then reset to the GPS
393 : // altitude. This ensures the reported AMSL alt from
394 : // getLLH() is equal to GPS altitude, while also ensuring
395 : // that the relative alt is zero
396 3802 : EKF_origin.copy_alt_from(dal.gps().location());
397 : }
398 4121 : ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
399 : }
400 :
401 : // set the terrain state to zero (on ground). The adjustment for
402 : // frame height will get added in the later constraints
403 5283 : terrainState = 0;
404 :
405 5283 : return true;
406 : }
407 :
408 : /*
409 : correct GPS data for position offset of antenna phase centre relative to the IMU
410 : */
411 1251656 : void NavEKF3_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
412 : {
413 : // return immediately if already corrected
414 1251656 : if (gps_data.corrected) {
415 1246390 : return;
416 : }
417 1251656 : gps_data.corrected = true;
418 :
419 1251656 : const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gps_data.sensor_idx).toftype() - accelPosOffset;
420 1251656 : if (posOffsetBody.is_zero()) {
421 : return;
422 : }
423 :
424 : // TODO use a filtered angular rate with a group delay that matches the GPS delay
425 5266 : Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
426 5266 : Vector3F velOffsetBody = angRate % posOffsetBody;
427 5266 : Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
428 5266 : gps_data.vel -= velOffsetEarth;
429 :
430 5266 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
431 5266 : Location::offset_latlng(gps_data.lat, gps_data.lng, -posOffsetEarth.x, -posOffsetEarth.y);
432 5266 : gps_data.hgt += posOffsetEarth.z;
433 : }
434 :
435 : // correct external navigation earth-frame position using sensor body-frame offset
436 17877 : void NavEKF3_core::CorrectExtNavForSensorOffset(ext_nav_elements &ext_nav_data)
437 : {
438 : // return immediately if already corrected
439 17877 : if (ext_nav_data.corrected) {
440 17877 : return;
441 : }
442 17877 : ext_nav_data.corrected = true;
443 :
444 : // external nav data is against the public_origin, so convert to offset from EKF_origin
445 17877 : ext_nav_data.pos.xy() += EKF_origin.get_distance_NE_ftype(public_origin);
446 :
447 : #if HAL_VISUALODOM_ENABLED
448 17877 : const auto *visual_odom = dal.visualodom();
449 17877 : if (visual_odom == nullptr) {
450 : return;
451 : }
452 17877 : const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
453 17877 : if (posOffsetBody.is_zero()) {
454 : return;
455 : }
456 0 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
457 0 : ext_nav_data.pos.x -= posOffsetEarth.x;
458 0 : ext_nav_data.pos.y -= posOffsetEarth.y;
459 0 : ext_nav_data.pos.z -= posOffsetEarth.z;
460 : #endif
461 : }
462 :
463 : // correct external navigation earth-frame velocity using sensor body-frame offset
464 8299 : void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const
465 : {
466 : // return immediately if already corrected
467 8299 : if (ext_nav_vel_data.corrected) {
468 8299 : return;
469 : }
470 8299 : ext_nav_vel_data.corrected = true;
471 :
472 : #if HAL_VISUALODOM_ENABLED
473 8299 : const auto *visual_odom = dal.visualodom();
474 8299 : if (visual_odom == nullptr) {
475 : return;
476 : }
477 8299 : const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
478 8299 : if (posOffsetBody.is_zero()) {
479 : return;
480 : }
481 : // TODO use a filtered angular rate with a group delay that matches the sensor delay
482 0 : const Vector3F angRate = imuDataDelayed.delAng * (1.0/imuDataDelayed.delAngDT);
483 0 : ext_nav_vel_data.vel += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
484 : #endif
485 : }
486 :
487 : // calculate velocity variance helper function
488 1256671 : void NavEKF3_core::CalculateVelInnovationsAndVariances(const Vector3F &velocity, ftype noise, ftype accel_scale, Vector3F &innovations, Vector3F &variances) const
489 : {
490 : // innovations are latest estimate - latest observation
491 1256671 : innovations = stateStruct.velocity - velocity;
492 :
493 1256671 : const ftype obs_data_chk = sq(constrain_ftype(noise, 0.05, 5.0)) + sq(accel_scale * accNavMag);
494 :
495 : // calculate innovation variance. velocity states start at index 4
496 1256671 : variances.x = P[4][4] + obs_data_chk;
497 1256671 : variances.y = P[5][5] + obs_data_chk;
498 1256671 : variances.z = P[6][6] + obs_data_chk;
499 1256671 : }
500 :
501 : /********************************************************
502 : * FUSE MEASURED_DATA *
503 : ********************************************************/
504 : // select fusion of velocity, position and height measurements
505 19965274 : void NavEKF3_core::SelectVelPosFusion()
506 : {
507 : // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
508 : // If so, don't fuse measurements on this time step to reduce frame over-runs
509 : // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
510 19965274 : if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
511 1204036 : posVelFusionDelayed = true;
512 1204036 : return;
513 : } else {
514 18761238 : posVelFusionDelayed = false;
515 : }
516 :
517 : #if EK3_FEATURE_EXTERNAL_NAV
518 : // Check for data at the fusion time horizon
519 18761238 : extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
520 18761238 : if (extNavDataToFuse) {
521 17877 : CorrectExtNavForSensorOffset(extNavDataDelayed);
522 : }
523 18761238 : extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
524 18761238 : if (extNavVelToFuse) {
525 8299 : CorrectExtNavVelForSensorOffset(extNavVelDelayed);
526 :
527 : // calculate innovations and variances for reporting purposes only
528 8299 : CalculateVelInnovationsAndVariances(extNavVelDelayed.vel, extNavVelDelayed.err, frontend->extNavVelVarAccScale, extNavVelInnov, extNavVelVarInnov);
529 :
530 : // record time innovations were calculated (for timeout checks)
531 8299 : extNavVelInnovTime_ms = dal.millis();
532 : }
533 : #endif // EK3_FEATURE_EXTERNAL_NAV
534 :
535 : // Read GPS data from the sensor
536 18761238 : readGpsData();
537 18761238 : readGpsYawData();
538 :
539 : // get data that has now fallen behind the fusion time horizon
540 18761238 : gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms) && !waitingForGpsChecks;
541 :
542 18761238 : if (gpsDataToFuse) {
543 1248372 : CorrectGPSForAntennaOffset(gpsDataDelayed);
544 : // calculate innovations and variances for reporting purposes only
545 1248372 : CalculateVelInnovationsAndVariances(gpsDataDelayed.vel, frontend->_gpsHorizVelNoise.get(), frontend->gpsNEVelVarAccScale, gpsVelInnov, gpsVelVarInnov);
546 : // record time GPS data was retrieved from the buffer (for timeout checks)
547 1248372 : gpsRetrieveTime_ms = dal.millis();
548 : }
549 :
550 : // detect position source changes. Trigger position reset if position source is valid
551 18761238 : const AP_NavEKF_Source::SourceXY posxy_source = frontend->sources.getPosXYSource(core_index);
552 18761238 : if (posxy_source != posxy_source_last) {
553 84 : posxy_source_reset = (posxy_source != AP_NavEKF_Source::SourceXY::NONE);
554 84 : posxy_source_last = posxy_source;
555 : }
556 :
557 : // initialise all possible data we may fuse
558 18761238 : fusePosData = false;
559 18761238 : fuseVelData = false;
560 :
561 : // Determine if we need to fuse position and velocity data on this time step
562 18761238 : if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::GPS)) {
563 :
564 : // Don't fuse velocity data if GPS doesn't support it
565 1118207 : fuseVelData = frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS, core_index);
566 1118207 : fuseVelVertData = frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS, core_index) && useGpsVertVel;
567 1118207 : fusePosData = true;
568 : #if EK3_FEATURE_EXTERNAL_NAV
569 1118207 : extNavUsedForPos = false;
570 : #endif
571 :
572 : // copy corrected GPS data to observation vector
573 1118207 : if (fuseVelData) {
574 1117840 : velPosObs[0] = gpsDataDelayed.vel.x;
575 1117840 : velPosObs[1] = gpsDataDelayed.vel.y;
576 : }
577 1118207 : if (fuseVelVertData) {
578 1081465 : velPosObs[2] = gpsDataDelayed.vel.z;
579 : }
580 1118207 : const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
581 1118207 : const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
582 1118207 : velPosObs[3] = posxy.x;
583 1118207 : velPosObs[4] = posxy.y;
584 : #if EK3_FEATURE_EXTERNAL_NAV
585 18761238 : } else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV)) {
586 : // use external nav system for horizontal position
587 12170 : extNavUsedForPos = true;
588 12170 : fusePosData = true;
589 12170 : velPosObs[3] = extNavDataDelayed.pos.x;
590 12170 : velPosObs[4] = extNavDataDelayed.pos.y;
591 : #endif // EK3_FEATURE_EXTERNAL_NAV
592 : }
593 :
594 : #if EK3_FEATURE_EXTERNAL_NAV
595 : // fuse external navigation velocity data if available
596 : // extNavVelDelayed is already corrected for sensor position
597 18761238 : if (extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV, core_index)) {
598 6322 : fuseVelData = true;
599 6322 : velPosObs[0] = extNavVelDelayed.vel.x;
600 6322 : velPosObs[1] = extNavVelDelayed.vel.y;
601 : }
602 18761238 : if (extNavVelToFuse && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::EXTNAV, core_index)) {
603 4447 : fuseVelVertData = true;
604 4447 : velPosObs[2] = extNavVelDelayed.vel.z;
605 : }
606 : #endif
607 :
608 : // we have GPS data to fuse and a request to align the yaw using the GPS course
609 18761238 : if (gpsYawResetRequest) {
610 3652 : realignYawGPS(false);
611 : }
612 :
613 : // Select height data to be fused from the available baro, range finder and GPS sources
614 18761238 : selectHeightForFusion();
615 :
616 : // if we are using GPS, check for a change in receiver and reset position and height
617 18761238 : if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::GPS) && (gpsDataDelayed.sensor_idx != last_gps_idx || posxy_source_reset)) {
618 : // mark a source reset as consumed
619 18 : posxy_source_reset = false;
620 :
621 : // record the ID of the GPS that we are using for the reset
622 18 : last_gps_idx = gpsDataDelayed.sensor_idx;
623 :
624 : // reset the position to the GPS position
625 18 : const Location gpsloc{gpsDataDelayed.lat, gpsDataDelayed.lng, 0, Location::AltFrame::ABSOLUTE};
626 18 : const Vector2F posxy = EKF_origin.get_distance_NE_ftype(gpsloc);
627 18 : ResetPositionNE(posxy.x, posxy.y);
628 :
629 : // If we are also using GPS as the height reference, reset the height
630 18 : if (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) {
631 0 : ResetPositionD(-hgtMea);
632 : }
633 : }
634 :
635 : #if EK3_FEATURE_EXTERNAL_NAV
636 : // check for external nav position reset
637 18761238 : if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && (extNavDataDelayed.posReset || posxy_source_reset)) {
638 : // mark a source reset as consumed
639 2 : posxy_source_reset = false;
640 2 : ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
641 2 : if (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) {
642 2 : ResetPositionD(-hgtMea);
643 : }
644 : }
645 : #endif // EK3_FEATURE_EXTERNAL_NAV
646 :
647 : // If we are operating without any aiding, fuse in constant position of constant
648 : // velocity measurements to constrain tilt drift. This assumes a non-manoeuvring
649 : // vehicle. Do this to coincide with the height fusion.
650 18761238 : fusingStationaryZeroVel = false;
651 :
652 18761238 : if (fuseHgtData && PV_AidingMode == AID_NONE) {
653 542374 : if (assume_zero_sideslip() && tiltAlignComplete && motorsArmed) {
654 : // handle special case where we are launching a FW aircraft without magnetometer
655 640 : fusePosData = false;
656 640 : velPosObs[0] = 0.0f;
657 640 : velPosObs[1] = 0.0f;
658 640 : velPosObs[2] = stateStruct.velocity.z;
659 640 : bool resetVelNE = !prevMotorsArmed;
660 : // reset states to stop launch accel causing tilt error
661 640 : if (imuDataDelayed.delVel.x > 1.1f * GRAVITY_MSS * imuDataDelayed.delVelDT) {
662 140 : lastLaunchAccelTime_ms = imuSampleTime_ms;
663 140 : fuseVelData = false;
664 140 : fuseVelVertData = false;
665 : resetVelNE = true;
666 500 : } else if (lastLaunchAccelTime_ms != 0 && (imuSampleTime_ms - lastLaunchAccelTime_ms) < 10000) {
667 350 : fuseVelData = false;
668 350 : fuseVelVertData = false;
669 : resetVelNE = true;
670 : } else {
671 150 : fuseVelData = true;
672 150 : fuseVelVertData = true;
673 : }
674 150 : if (resetVelNE) {
675 490 : stateStruct.velocity.x = 0.0f;
676 490 : stateStruct.velocity.y = 0.0f;
677 : }
678 : } else {
679 541734 : fusePosData = true;
680 : // When stationary on ground, fuse zero velocity
681 : // to constrain gyro bias and Z-axis accel bias learning. XY accel biases
682 : // remain unobservable until the vehicle accelerates and are separately
683 : // inhibited by dvelBiasAxisInhibit in CovariancePrediction.
684 : // Use onGroundNotMoving to avoid fusing zero velocity when the vehicle
685 : // is being moved (e.g. on a boat or carried by hand).
686 541734 : const bool onGroundNotFlying = onGroundNotMoving;
687 541734 : if (onGroundNotFlying && tiltAlignComplete) {
688 453006 : fuseVelData = true;
689 453006 : fusingStationaryZeroVel = true;
690 453006 : velPosObs[0] = 0.0f;
691 453006 : velPosObs[1] = 0.0f;
692 453006 : velPosObs[2] = 0.0f;
693 : } else {
694 88728 : fuseVelData = false;
695 : }
696 541734 : velPosObs[3] = lastKnownPositionNE.x;
697 541734 : velPosObs[4] = lastKnownPositionNE.y;
698 : }
699 : }
700 :
701 : // When in AID_RELATIVE or AID_ABSOLUTE mode but stationary on ground without velocity
702 : // aiding, fuse synthetic zero velocity to constrain gyro bias and Z-axis accel bias
703 : // learning. XY accel biases are unobservable on the ground and are inhibited by
704 : // dvelBiasAxisInhibit. Without this, configurations like optical flow where
705 : // PV_AidingMode is AID_RELATIVE but no velocity data is available when stationary
706 : // have no velocity observations at all, causing unchecked bias drift. The timeout
707 : // check on each velocity source ensures we only inject zero velocity when no real
708 : // sensor data is being fused. Use onGroundNotMoving to avoid injecting zero velocity
709 : // when the vehicle is being moved, and takeoff_expected for armed-on-ground.
710 : // Gate behind fuseHgtData to limit fusion rate to baro rate (~10Hz) and avoid
711 : // overconstraining the filter by fusing at IMU rate.
712 18761238 : const bool onGroundNotFlying = onGroundNotMoving;
713 :
714 18761238 : if (fuseHgtData && PV_AidingMode != AID_NONE && onGroundNotFlying) {
715 : // Check if we have recent velocity aiding from any source
716 : const uint32_t velAidTimeout_ms = 1000;
717 319230 : const bool haveRecentGpsVel = (imuSampleTime_ms - lastVelPassTime_ms < velAidTimeout_ms);
718 : #if EK3_FEATURE_OPTFLOW_FUSION
719 319230 : const bool haveRecentFlowVel = (imuSampleTime_ms - prevFlowFuseTime_ms < velAidTimeout_ms);
720 : #else
721 : const bool haveRecentFlowVel = false;
722 : #endif
723 319230 : const bool haveRecentBodyVel = (imuSampleTime_ms - prevBodyVelFuseTime_ms < velAidTimeout_ms);
724 :
725 319230 : if (!haveRecentGpsVel && !haveRecentFlowVel && !haveRecentBodyVel) {
726 : // No velocity aiding available while stationary - fuse synthetic zero velocity
727 : // to constrain gyro bias and gravity-aligned accel bias
728 694 : fuseVelData = true;
729 694 : fusingStationaryZeroVel = true;
730 694 : velPosObs[0] = 0.0f;
731 694 : velPosObs[1] = 0.0f;
732 694 : velPosObs[2] = 0.0f;
733 : }
734 : }
735 :
736 : // perform fusion
737 18761238 : if (fuseVelData|| fuseVelVertData || fusePosData || fuseHgtData) {
738 3837832 : FuseVelPosNED();
739 : // clear the flags to prevent repeated fusion of the same data
740 3837832 : fuseVelData = false;
741 3837832 : fuseVelVertData = false;
742 3837832 : fuseHgtData = false;
743 3837832 : fusePosData = false;
744 : }
745 : }
746 :
747 : // fuse selected position, velocity and height measurements
748 3837832 : void NavEKF3_core::FuseVelPosNED()
749 : {
750 : // declare variables used to control access to arrays
751 3837832 : bool fuseData[6] {};
752 : uint8_t stateIndex;
753 : uint8_t obsIndex;
754 :
755 : // declare variables used by state and covariance update calculations
756 : Vector6 R_OBS; // Measurement variances used for fusion
757 : Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
758 : ftype SK;
759 :
760 : // perform sequential fusion of measurements. This assumes that the
761 : // errors in the different velocity and position components are
762 : // uncorrelated which is not true, however in the absence of covariance
763 : // data from sensors like GPS receivers; it is the only assumption we can make
764 : // so we might as well take advantage of the computational efficiencies
765 : // associated with sequential fusion
766 3837832 : if (fuseVelData || fuseVelVertData || fusePosData || fuseHgtData) {
767 : // estimate the velocity, horiz position and height measurement variances.
768 : // Use different errors if operating without external aiding using an assumed position or velocity of zero
769 3837832 : if (PV_AidingMode == AID_NONE) {
770 542386 : if (fusingStationaryZeroVel) {
771 : // Synthetic zero velocity on ground: use 1 m/s noise (variance = 1 m^2/s^2).
772 : // This is tighter than _noaidHorizNoise (default 10 m/s) used when armed,
773 : // giving stronger velocity/bias convergence while we have high confidence
774 : // the vehicle is stationary. Not as tight as a real sensor since
775 : // the measurement is an assumption, not a physical observation.
776 453006 : R_OBS[0] = sq(MIN(frontend->_noaidHorizNoise, 1.0f));
777 89380 : } else if (tiltAlignComplete && motorsArmed) {
778 : // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
779 42462 : R_OBS[0] = sq(constrain_ftype(frontend->_noaidHorizNoise, 0.5f, 50.0f));
780 : } else {
781 : // Use a smaller value to give faster initial alignment
782 46918 : R_OBS[0] = sq(0.5f);
783 : }
784 542386 : R_OBS[1] = R_OBS[0];
785 542386 : R_OBS[2] = R_OBS[0];
786 542386 : R_OBS[3] = R_OBS[0];
787 542386 : R_OBS[4] = R_OBS[0];
788 2169544 : for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
789 3295446 : } else if (fusingStationaryZeroVel) {
790 : // Synthetic zero velocity in AID_RELATIVE or AID_ABSOLUTE mode when no
791 : // velocity sensor data is available (e.g. GPS configured but not yet locked,
792 : // or optical flow with no movement). Use 1 m/s noise for velocity — same
793 : // rationale as the AID_NONE case above. Position noise uses actual sensor
794 : // characteristics when available (extNav posErr, GPS accuracy), falling back
795 : // to _gpsHorizPosNoise as a conservative default since it is the only
796 : // position noise parameter available regardless of aiding source.
797 694 : R_OBS[0] = sq(1.0f);
798 694 : R_OBS[1] = R_OBS[0];
799 694 : R_OBS[2] = R_OBS[0];
800 2776 : for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
801 : #if EK3_FEATURE_EXTERNAL_NAV
802 694 : if (extNavUsedForPos) {
803 2 : R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
804 : } else
805 : #endif
806 692 : if (gpsPosAccuracy > 0.0f) {
807 615 : R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
808 : } else {
809 77 : R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
810 : }
811 694 : R_OBS[4] = R_OBS[3];
812 : } else {
813 : #if EK3_FEATURE_EXTERNAL_NAV
814 3294752 : const bool extNavUsedForVel = extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV, core_index);
815 : if (extNavUsedForVel) {
816 6282 : R_OBS[2] = R_OBS[0] = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 50.0f));
817 : } else
818 : #endif
819 3288470 : if (gpsSpdAccuracy > 0.0f) {
820 : // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
821 3272968 : R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
822 3272968 : R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
823 : } else {
824 : // calculate additional error in GPS velocity caused by manoeuvring
825 15502 : R_OBS[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
826 15502 : R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
827 : }
828 3294752 : R_OBS[1] = R_OBS[0];
829 : // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
830 : #if EK3_FEATURE_EXTERNAL_NAV
831 3294752 : if (extNavUsedForPos) {
832 13459 : R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 100.0f));
833 : } else
834 : #endif
835 3281293 : if (gpsPosAccuracy > 0.0f) {
836 3271490 : R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
837 : } else {
838 : // calculate additional error in GPS position caused by manoeuvring
839 9803 : const ftype posErr = frontend->gpsPosVarAccScale * accNavMag;
840 9803 : R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
841 : }
842 3294752 : R_OBS[4] = R_OBS[3];
843 : // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
844 : // For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS performance
845 : // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
846 : ftype obs_data_chk;
847 : #if EK3_FEATURE_EXTERNAL_NAV
848 3294752 : if (extNavUsedForVel) {
849 6282 : obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
850 : } else
851 : #endif
852 : {
853 3288470 : obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
854 : }
855 3294752 : R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
856 : }
857 3837832 : R_OBS[5] = posDownObsNoise;
858 15351328 : for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
859 :
860 : // if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
861 : // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
862 : // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
863 3837832 : const bool fuse_gps_vz = frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS, core_index) && gpsDataDelayed.have_vz;
864 3415118 : if (fuse_gps_vz && fuseVelVertData && (frontend->sources.getPosZSource(core_index) != AP_NavEKF_Source::SourceZ::GPS)) {
865 : // calculate innovations for height and vertical GPS vel measurements
866 1080073 : const ftype hgtErr = stateStruct.position.z - velPosObs[5];
867 1080073 : const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
868 : // Check if they are the same sign and both more than 3-sigma out of bounds
869 : // Step the test threshold up in stages from 1 to 2 to 3 sigma after exiting
870 : // from a previous bad IMU event so that a subsequent error is caught more quickly.
871 1080073 : const uint32_t timeSinceLastBadIMU_ms = imuSampleTime_ms - badIMUdata_ms;
872 : float R_gain;
873 1080073 : if (timeSinceLastBadIMU_ms > (BAD_IMU_DATA_HOLD_MS * 2)) {
874 : R_gain = 9.0F;
875 2525 : } else if (timeSinceLastBadIMU_ms > ((BAD_IMU_DATA_HOLD_MS * 3) / 2)) {
876 : R_gain = 4.0F;
877 : } else {
878 : R_gain = 1.0F;
879 : }
880 1080073 : if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > R_gain * R_OBS[5]) && (sq(velDErr) >R_gain * R_OBS[2])) {
881 78 : badIMUdata_ms = imuSampleTime_ms;
882 : } else {
883 1079995 : goodIMUdata_ms = imuSampleTime_ms;
884 : }
885 1080073 : if (timeSinceLastBadIMU_ms < BAD_IMU_DATA_HOLD_MS) {
886 1380 : badIMUdata = true;
887 1380 : stateStruct.velocity.z = gpsDataDelayed.vel.z;
888 : } else {
889 1078693 : badIMUdata = false;
890 : }
891 : }
892 :
893 : // Test horizontal position measurements
894 3837832 : if (fusePosData) {
895 1672111 : innovVelPos[3] = stateStruct.position.x - velPosObs[3];
896 1672111 : innovVelPos[4] = stateStruct.position.y - velPosObs[4];
897 1672111 : varInnovVelPos[3] = P[7][7] + R_OBS_DATA_CHECKS[3];
898 1672111 : varInnovVelPos[4] = P[8][8] + R_OBS_DATA_CHECKS[4];
899 :
900 : // Apply an innovation consistency threshold test
901 : // Don't allow test to fail if not navigating and using a constant position
902 : // assumption to constrain tilt errors because innovations can become large
903 : // due to vehicle motion.
904 1672111 : ftype maxPosInnov2 = sq(MAX(0.01 * (ftype)frontend->_gpsPosInnovGate, 1.0))*(varInnovVelPos[3] + varInnovVelPos[4]);
905 :
906 1672111 : posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
907 : bool posCheckPassed = false; // boolean true if position measurements have passed innovation consistency check
908 1672111 : if (posTestRatio < 1.0f || (PV_AidingMode == AID_NONE)) {
909 : posCheckPassed = true;
910 1669976 : lastGpsPosPassTime_ms = imuSampleTime_ms;
911 2135 : } else if ((frontend->_gpsGlitchRadiusMax <= 0) && (PV_AidingMode != AID_NONE)) {
912 : // Handle the special case where the glitch radius parameter has been set to a non-positive number.
913 : // The innovation variance is increased to limit the state update to an amount corresponding
914 : // to a test ratio of 1.
915 0 : lastGpsPosPassTime_ms = imuSampleTime_ms;
916 0 : varInnovVelPos[3] *= posTestRatio;
917 0 : varInnovVelPos[4] *= posTestRatio;
918 : posCheckPassed = true;
919 : lastGpsPosPassTime_ms = imuSampleTime_ms;
920 : }
921 :
922 : // Use position data if healthy or timed out or bad IMU data
923 : // Always fuse data if bad IMU to prevent aliasing and clipping pulling the state estimate away
924 : // from the measurement un-opposed if test threshold is exceeded.
925 2135 : if (posCheckPassed || posTimeout || badIMUdata) {
926 : // if timed out or outside the specified uncertainty radius, reset to the external sensor
927 : // if velocity drift is being constrained, dont reset until gps passes quality checks
928 1670082 : const bool posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax));
929 1670082 : if ((posTimeout || posVarianceIsTooLarge) && (!velAiding || gpsGoodToAlign)) {
930 : // reset the position to the current external sensor position
931 101 : ResetPosition(resetDataSource::DEFAULT);
932 :
933 : // Don't fuse the same data we have used to reset states.
934 101 : fusePosData = false;
935 :
936 : // Reset the position variances and corresponding covariances to a value that will pass the checks
937 101 : zeroStatesVarCov(7, 8);
938 101 : P[7][7] = sq(ftype(0.5f*frontend->_gpsGlitchRadiusMax));
939 101 : P[8][8] = P[7][7];
940 :
941 : // Reset the normalised innovation to avoid failing the bad fusion tests
942 101 : posTestRatio = 0.0f;
943 :
944 : // Reset velocity if it has timed out
945 101 : if (velTimeout) {
946 32 : ResetVelocity(resetDataSource::DEFAULT);
947 :
948 : // Don't fuse the same data we have used to reset states.
949 32 : fuseVelData = false;
950 :
951 : // Reset the normalised innovation to avoid failing the bad fusion tests
952 32 : velTestRatio = 0.0f;
953 : }
954 : }
955 : } else {
956 2029 : fusePosData = false;
957 : }
958 : }
959 :
960 : // Test velocity measurements
961 3837832 : if (fuseVelData) {
962 : uint8_t imax = 2;
963 : // Don't fuse vertical velocity observations if disabled in sources or not available
964 1577958 : const bool fuse_extnav_vz = frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::EXTNAV, core_index) && useExtNavVel;
965 1577958 : if ((PV_AidingMode != AID_ABSOLUTE || !fuse_gps_vz) && !fuse_extnav_vz) {
966 : imax = 1;
967 : }
968 :
969 : // Apply an innovation consistency threshold test
970 : ftype innovVelSumSq = 0; // sum of squares of velocity innovations
971 : ftype varVelSum = 0; // sum of velocity innovation variances
972 :
973 5820150 : for (uint8_t i = 0; i<=imax; i++) {
974 4242192 : stateIndex = i + 4;
975 4242192 : const float innovation = stateStruct.velocity[i] - velPosObs[i];
976 4242192 : innovVelSumSq += sq(innovation);
977 4242192 : varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
978 4242192 : varVelSum += varInnovVelPos[i];
979 : }
980 1577958 : velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01 * (ftype)frontend->_gpsVelInnovGate, 1.0)));
981 : bool velCheckPassed = false; // boolean true if velocity measurements have passed innovation consistency checks
982 1577958 : if (velTestRatio < 1.0) {
983 : velCheckPassed = true;
984 1577070 : lastVelPassTime_ms = imuSampleTime_ms;
985 888 : } else if (frontend->_gpsGlitchRadiusMax <= 0) {
986 : // Handle the special case where the glitch radius parameter has been set to a non-positive number.
987 : // The innovation variance is increased to limit the state update to an amount corresponding
988 : // to a test ratio of 1.
989 0 : lastGpsPosPassTime_ms = imuSampleTime_ms;
990 0 : for (uint8_t i = 0; i<=imax; i++) {
991 0 : varInnovVelPos[i] *= velTestRatio;
992 : }
993 : velCheckPassed = true;
994 0 : lastVelPassTime_ms = imuSampleTime_ms;
995 : }
996 :
997 : // Use velocity data if healthy, timed out or when IMU fault has been detected
998 : // Always fuse data if bad IMU to prevent aliasing and clipping pulling the state estimate away
999 : // from the measurement un-opposed if test threshold is exceeded.
1000 888 : if (velCheckPassed || velTimeout || badIMUdata) {
1001 : // If we are doing full aiding and velocity fusion times out, reset to the external sensor velocity
1002 1577114 : if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
1003 2 : ResetVelocity(resetDataSource::DEFAULT);
1004 :
1005 : // Don't fuse the same data we have used to reset states.
1006 2 : fuseVelData = false;
1007 :
1008 : // Reset the normalised innovation to avoid failing the bad fusion tests
1009 2 : velTestRatio = 0.0f;
1010 : }
1011 : } else {
1012 844 : fuseVelData = false;
1013 : }
1014 : }
1015 :
1016 : // Test height measurements
1017 3837832 : if (fuseHgtData) {
1018 : // Calculate height innovations
1019 2809486 : innovVelPos[5] = stateStruct.position.z - velPosObs[5];
1020 2809486 : varInnovVelPos[5] = P[9][9] + R_OBS_DATA_CHECKS[5];
1021 :
1022 : // Calculate the innovation consistency test ratio
1023 2809486 : hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01 * (ftype)frontend->_hgtInnovGate, 1.0)) * varInnovVelPos[5]);
1024 :
1025 : // When on ground we accept a larger test ratio to allow the filter to handle large switch on IMU
1026 : // bias errors without rejecting the height sensor.
1027 2809486 : const bool onGroundNotNavigating = (PV_AidingMode == AID_NONE) && onGround;
1028 : const float maxTestRatio = onGroundNotNavigating ? 3.0f : 1.0f;
1029 : bool hgtCheckPassed = false; // boolean true if height measurements have passed innovation consistency check
1030 2809486 : if (hgtTestRatio < maxTestRatio) {
1031 : hgtCheckPassed = true;
1032 2809289 : lastHgtPassTime_ms = imuSampleTime_ms;
1033 197 : } else if ((frontend->_gpsGlitchRadiusMax <= 0) && !onGroundNotNavigating && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
1034 : // Handle the special case where the glitch radius parameter has been set to a non-positive number.
1035 : // The innovation variance is increased to limit the state update to an amount corresponding
1036 : // to a test ratio of 1.
1037 0 : lastGpsPosPassTime_ms = imuSampleTime_ms;
1038 0 : varInnovVelPos[5] *= hgtTestRatio;
1039 : hgtCheckPassed = true;
1040 0 : lastHgtPassTime_ms = imuSampleTime_ms;
1041 : }
1042 :
1043 : // Use height data if innovation check passed or timed out or if bad IMU data
1044 : // Always fuse data if bad IMU to prevent aliasing and clipping pulling the state estimate away
1045 : // from the measurement un-opposed if test threshold is exceeded.
1046 197 : if (hgtCheckPassed || hgtTimeout || badIMUdata) {
1047 : // Calculate a filtered value to be used by pre-flight health checks
1048 : // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
1049 2809296 : if (onGround) {
1050 861433 : ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms) * 1.0e-3;
1051 : const ftype hgtInnovFiltTC = 2.0;
1052 861433 : ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC), 0.0, 1.0);
1053 861433 : hgtInnovFiltState += (innovVelPos[5] - hgtInnovFiltState)*alpha;
1054 : } else {
1055 1947863 : hgtInnovFiltState = 0.0f;
1056 : }
1057 :
1058 2809296 : if (hgtTimeout) {
1059 13 : ResetHeight();
1060 :
1061 : // Don't fuse the same data we have used to reset states.
1062 13 : fuseHgtData = false;
1063 : }
1064 :
1065 : } else {
1066 190 : fuseHgtData = false;
1067 : }
1068 : }
1069 :
1070 : // set range for sequential fusion of velocity and position measurements depending on which data is available and its health
1071 3837832 : if (fuseVelData) {
1072 1577112 : fuseData[0] = true;
1073 1577112 : fuseData[1] = true;
1074 1577112 : if (fuseVelVertData || fusingStationaryZeroVel) {
1075 1538862 : fuseData[2] = true;
1076 : }
1077 : }
1078 3837832 : if (fusePosData) {
1079 1669981 : fuseData[3] = true;
1080 1669981 : fuseData[4] = true;
1081 : }
1082 3837832 : if (fuseHgtData) {
1083 2809283 : fuseData[5] = true;
1084 : }
1085 :
1086 : // fuse measurements sequentially
1087 26864824 : for (obsIndex=0; obsIndex<=5; obsIndex++) {
1088 23026992 : if (fuseData[obsIndex]) {
1089 10842331 : stateIndex = 4 + obsIndex;
1090 : // calculate the measurement innovation, using states from a different time coordinate if fusing height data
1091 : // adjust scaling on GPS measurement noise variances if not enough satellites
1092 10842331 : if (obsIndex <= 2) {
1093 4693086 : innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
1094 4693086 : if (frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS, core_index)) {
1095 4666404 : R_OBS[obsIndex] *= sq(gpsNoiseScaler);
1096 : }
1097 6149245 : } else if (obsIndex == 3 || obsIndex == 4) {
1098 3339962 : innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
1099 3339962 : if (frontend->sources.getPosXYSource(core_index) == AP_NavEKF_Source::SourceXY::GPS) {
1100 3308530 : R_OBS[obsIndex] *= sq(gpsNoiseScaler);
1101 : }
1102 : } else if (obsIndex == 5) {
1103 2809283 : innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
1104 2809283 : const ftype gndMaxBaroErr = MAX(frontend->_baroGndEffectDeadZone, 0.0);
1105 : const ftype gndBaroInnovFloor = -0.5;
1106 :
1107 2809283 : if ((dal.get_touchdown_expected() || dal.get_takeoff_expected()) && activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
1108 : // when baro positive pressure error due to ground effect is expected,
1109 : // floor the barometer innovation at gndBaroInnovFloor
1110 : // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
1111 : // this function looks like this:
1112 : // |/
1113 : //---------|---------
1114 : // ____/|
1115 : // / |
1116 : // / |
1117 154293 : innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
1118 : }
1119 : }
1120 :
1121 : // calculate the Kalman gain and calculate innovation variances
1122 10842331 : varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
1123 10842331 : SK = 1.0f/varInnovVelPos[obsIndex];
1124 :
1125 : uint32_t kalman_mask = (1<<10)-1; // values to calculate in Kfusion (others are set to zero)
1126 :
1127 : // inhibit delta angle bias state estimation by setting Kalman gains to zero
1128 10842331 : if (!inhibitDelAngBiasStates) {
1129 42838760 : for (uint8_t i = 10; i<=12; i++) {
1130 : // Don't try to learn gyro bias if not aiding and the axis is
1131 : // less than 45 degrees from vertical because the bias is poorly observable
1132 : bool poorObservability = false;
1133 32129070 : if (PV_AidingMode == AID_NONE) {
1134 8557791 : const uint8_t axisIndex = i - 10;
1135 8557791 : if (axisIndex == 0) {
1136 2852597 : poorObservability = fabsF(prevTnb.a.z) > M_SQRT1_2;
1137 5705194 : } else if (axisIndex == 1) {
1138 2852597 : poorObservability = fabsF(prevTnb.b.z) > M_SQRT1_2;
1139 : } else {
1140 2852597 : poorObservability = fabsF(prevTnb.c.z) > M_SQRT1_2;
1141 : }
1142 : }
1143 8557791 : if (!poorObservability) {
1144 29276519 : kalman_mask |= (1<<i);
1145 : }
1146 : }
1147 : }
1148 :
1149 : // Inhibit delta velocity bias state estimation by setting Kalman gains to zero
1150 : // Don't use 'fake' horizontal measurements used to constrain attitude drift during
1151 : // periods of non-aiding to learn bias as these can give incorrect esitmates.
1152 10842331 : const bool horizInhibit = PV_AidingMode == AID_NONE && obsIndex != 2 && obsIndex != 5;
1153 8852539 : if (!horizInhibit && !inhibitDelVelBiasStates && !badIMUdata) {
1154 35194740 : for (uint8_t i = 13; i<=15; i++) {
1155 26396055 : if (!dvelBiasAxisInhibit[i-13]) {
1156 21959657 : kalman_mask |= (1<<i);
1157 : }
1158 : }
1159 : }
1160 :
1161 : // inhibit magnetic field state estimation by setting Kalman gains to zero
1162 10842331 : if (!inhibitMagStates) {
1163 5814297 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
1164 : }
1165 :
1166 : // inhibit wind state estimation by setting Kalman gains to zero
1167 10842331 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
1168 2153591 : kalman_mask |= (1<<22) | (1<<23);
1169 : }
1170 :
1171 271058275 : for (auto i=0; i<24; i++) {
1172 : ftype res = 0;
1173 260215944 : if (kalman_mask & (1<<i)) {
1174 198852450 : res = P[i][stateIndex]*SK;
1175 : }
1176 260215944 : Kfusion[i] = res;
1177 : }
1178 :
1179 : // one element of H is 1, compiler will optimize it away
1180 : Vector24 Hfusion;
1181 : Hfusion[stateIndex] = 1;
1182 :
1183 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
1184 : // the zero elements of H to reduce the number of operations.
1185 222833301 : for (unsigned i = 0; i<=stateIndexLim; i++) {
1186 : // j as the inner loop allows the compiler to hoist the KH product
1187 : // to save computation, and do the inner indexing more efficiently.
1188 4495244878 : for (unsigned j = 0; j<=stateIndexLim; j++) {
1189 : ftype res = 0;
1190 4283253908 : res += (Kfusion[i] * Hfusion[stateIndex]) * P[stateIndex][j];
1191 4283253908 : KHP[i][j] = res;
1192 : }
1193 : }
1194 :
1195 : // finish fusion from KHP and Kfusion
1196 10842331 : const bool fault = FinishFusion(innovVelPos[obsIndex]);
1197 : // record health status
1198 10842331 : if (obsIndex == 0) {
1199 1577112 : faultStatus.bad_nvel = fault;
1200 9265219 : } else if (obsIndex == 1) {
1201 1577112 : faultStatus.bad_evel = fault;
1202 7688107 : } else if (obsIndex == 2) {
1203 1538862 : faultStatus.bad_dvel = fault;
1204 6149245 : } else if (obsIndex == 3) {
1205 1669981 : faultStatus.bad_npos = fault;
1206 4479264 : } else if (obsIndex == 4) {
1207 1669981 : faultStatus.bad_epos = fault;
1208 : } else if (obsIndex == 5) {
1209 2809283 : faultStatus.bad_dpos = fault;
1210 : }
1211 : }
1212 : }
1213 : }
1214 3837832 : }
1215 :
1216 : /********************************************************
1217 : * MISC FUNCTIONS *
1218 : ********************************************************/
1219 :
1220 : // select the height measurement to be fused from the available baro, range finder and GPS sources
1221 18761238 : void NavEKF3_core::selectHeightForFusion()
1222 : {
1223 : #if AP_RANGEFINDER_ENABLED
1224 : // Read range finder data and check for new data in the buffer
1225 : // This data is used by both height and optical flow fusion processing
1226 18761238 : readRangeFinder();
1227 18761238 : rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
1228 :
1229 : // correct range data for the body frame position offset relative to the IMU
1230 : // the corrected reading is the reading that would have been taken if the sensor was
1231 : // co-located with the IMU
1232 18761238 : const auto *_rng = dal.rangefinder();
1233 18761238 : if (_rng && rangeDataToFuse) {
1234 173855 : auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
1235 173855 : if (sensor != nullptr) {
1236 173855 : Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset;
1237 173855 : if (!posOffsetBody.is_zero()) {
1238 1555 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
1239 1555 : rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
1240 : }
1241 : }
1242 : }
1243 : #endif // AP_RANGEFINDER_ENABLED
1244 :
1245 : // read baro height data from the sensor and check for new data in the buffer
1246 18761238 : readBaroData();
1247 18761238 : baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
1248 :
1249 18761238 : bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
1250 : #if EK3_FEATURE_EXTERNAL_NAV
1251 18761238 : const bool extNavDataIsFresh = (imuSampleTime_ms - extNavMeasTime_ms < 500);
1252 : #endif
1253 : // select height source
1254 18761238 : if ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::NONE)) {
1255 : // user has specified no height sensor
1256 0 : activeHgtSource = AP_NavEKF_Source::SourceZ::NONE;
1257 : #if AP_RANGEFINDER_ENABLED
1258 18761238 : } else if ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::RANGEFINDER) && _rng && rangeFinderDataIsFresh) {
1259 : // user has specified the range finder as a primary height source
1260 0 : activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER;
1261 18761238 : } else if ((frontend->_useRngSwHgt > 0) && ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::BARO) || (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::GPS)) && _rng && rangeFinderDataIsFresh) {
1262 : // determine if we are above or below the height switch region
1263 15248 : const ftype rangeMaxUse = 1e-2 * (ftype)_rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
1264 15248 : bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
1265 15248 : bool belowLowerSwHgt = ((terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse) && (imuSampleTime_ms - gndHgtValidTime_ms < 1000);
1266 :
1267 : // If the terrain height is consistent and we are moving slowly, then it can be
1268 : // used as a height reference in combination with a range finder
1269 : // apply a hysteresis to the speed check to prevent rapid switching
1270 : bool dontTrustTerrain, trustTerrain;
1271 15248 : if (filterStatus.flags.horiz_vel) {
1272 : // We can use the velocity estimate
1273 15030 : ftype horizSpeed = stateStruct.velocity.xy().length();
1274 15030 : dontTrustTerrain = (horizSpeed > frontend->_useRngSwSpd) || !terrainHgtStable;
1275 15030 : ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
1276 15030 : trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
1277 : } else {
1278 : // We can't use the velocity estimate
1279 218 : dontTrustTerrain = !terrainHgtStable;
1280 : trustTerrain = terrainHgtStable;
1281 : }
1282 :
1283 : /*
1284 : * Switch between range finder and primary height source using height above ground and speed thresholds with
1285 : * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
1286 : * which cannot be assumed if the vehicle is moving horizontally.
1287 : */
1288 15248 : if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
1289 : // cannot trust terrain or range finder so stop using range finder height
1290 3 : if (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::BARO) {
1291 3 : activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
1292 0 : } else if (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::GPS) {
1293 0 : activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
1294 : }
1295 15245 : } else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) {
1296 : // reliable terrain and range finder so start using range finder height
1297 295 : activeHgtSource = AP_NavEKF_Source::SourceZ::RANGEFINDER;
1298 : }
1299 : #endif
1300 18745990 : } else if (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::BARO) {
1301 18667007 : activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
1302 78983 : } else if ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
1303 23633 : activeHgtSource = AP_NavEKF_Source::SourceZ::GPS;
1304 : #if EK3_FEATURE_BEACON_FUSION
1305 55350 : } else if ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::BEACON) && validOrigin && rngBcn.goodToAlign) {
1306 0 : activeHgtSource = AP_NavEKF_Source::SourceZ::BEACON;
1307 : #endif
1308 : #if EK3_FEATURE_EXTERNAL_NAV
1309 55350 : } else if ((frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::EXTNAV) && extNavDataIsFresh) {
1310 39832 : activeHgtSource = AP_NavEKF_Source::SourceZ::EXTNAV;
1311 : #endif
1312 : }
1313 :
1314 : // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
1315 18761238 : bool lostRngHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) && !rangeFinderDataIsFresh);
1316 18761238 : bool lostGpsHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000 || !gpsAccuracyGoodForAltitude));
1317 : #if EK3_FEATURE_BEACON_FUSION
1318 18761238 : bool lostRngBcnHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::BEACON) && ((imuSampleTime_ms - rngBcn.dataDelayed.time_ms) > 2000));
1319 : #endif
1320 : bool fallback_to_baro =
1321 : lostRngHgt
1322 18761238 : || lostGpsHgt
1323 : #if EK3_FEATURE_BEACON_FUSION
1324 18761238 : || lostRngBcnHgt
1325 : #endif
1326 : ;
1327 : #if EK3_FEATURE_EXTERNAL_NAV
1328 18761238 : bool lostExtNavHgt = ((activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV) && !extNavDataIsFresh);
1329 18761238 : fallback_to_baro |= lostExtNavHgt;
1330 : #endif
1331 18761238 : if (fallback_to_baro) {
1332 1 : activeHgtSource = AP_NavEKF_Source::SourceZ::BARO;
1333 : }
1334 :
1335 : // if there is new baro data to fuse, calculate filtered baro data required by other processes
1336 18761238 : if (baroDataToFuse) {
1337 : // calculate offset to baro data that enables us to switch to Baro height use during operation
1338 2806673 : if (activeHgtSource != AP_NavEKF_Source::SourceZ::BARO) {
1339 9847 : calcFiltBaroOffset();
1340 : }
1341 : // filtered baro data used to provide a reference for takeoff
1342 : // it is is reset to last height measurement on disarming in performArmingChecks()
1343 2806673 : if (!dal.get_takeoff_expected()) {
1344 : const ftype gndHgtFiltTC = 0.5;
1345 2717576 : const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3;
1346 2717576 : ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0,1.0);
1347 2717576 : meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
1348 : }
1349 : }
1350 :
1351 : // If we are not using GPS as the primary height sensor, correct EKF origin height so that
1352 : // combined local NED position height and origin height remains consistent with the GPS altitude
1353 : // This also enables the GPS height to be used as a backup height source
1354 18761238 : if (gpsDataToFuse &&
1355 1248372 : (((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) ||
1356 1217490 : ((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)))
1357 : ) {
1358 30882 : correctEkfOriginHeight();
1359 : }
1360 :
1361 : // Select the height measurement source
1362 : #if EK3_FEATURE_EXTERNAL_NAV
1363 18761238 : if (extNavDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::EXTNAV)) {
1364 9167 : hgtMea = -extNavDataDelayed.pos.z;
1365 9167 : velPosObs[5] = -hgtMea;
1366 9167 : posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
1367 9167 : fuseHgtData = true;
1368 : } else
1369 : #endif // EK3_FEATURE_EXTERNAL_NAV
1370 18752071 : if (rangeDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
1371 : // using range finder data
1372 : // correct for tilt using a flat earth model
1373 1912 : if (prevTnb.c.z >= 0.7) {
1374 : // calculate height above ground
1375 1912 : hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
1376 : // correct for terrain position relative to datum
1377 1912 : hgtMea -= terrainState;
1378 : // correct sensor so that local position height adjusts to match GPS
1379 1912 : if (frontend->_originHgtMode & (1 << 1) && frontend->_originHgtMode & (1 << 2)) {
1380 : // offset has to be applied to the measurement, not the NED origin
1381 0 : hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt);
1382 : }
1383 1912 : velPosObs[5] = -hgtMea;
1384 : // enable fusion
1385 1912 : fuseHgtData = true;
1386 : // set the observation noise
1387 1912 : posDownObsNoise = sq(constrain_ftype(frontend->_rngNoise, 0.1f, 10.0f));
1388 : // add uncertainty created by terrain gradient and vehicle tilt
1389 1912 : posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
1390 : } else {
1391 : // disable fusion if tilted too far
1392 0 : fuseHgtData = false;
1393 : }
1394 18750159 : } else if (gpsDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::GPS)) {
1395 : // using GPS data
1396 1581 : hgtMea = gpsDataDelayed.hgt;
1397 1581 : velPosObs[5] = -hgtMea;
1398 : // enable fusion
1399 1581 : fuseHgtData = true;
1400 : // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
1401 1581 : if (gpsHgtAccuracy > 0.0f) {
1402 1581 : posDownObsNoise = sq(constrain_ftype(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
1403 : } else {
1404 0 : posDownObsNoise = sq(constrain_ftype(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
1405 : }
1406 18748578 : } else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) {
1407 : // using Baro data
1408 2796826 : hgtMea = baroDataDelayed.hgt - baroHgtOffset;
1409 : // correct sensor so that local position height adjusts to match GPS
1410 2796826 : if (frontend->_originHgtMode & (1 << 0) && frontend->_originHgtMode & (1 << 2)) {
1411 31208 : hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt);
1412 : }
1413 : // enable fusion
1414 2796826 : fuseHgtData = true;
1415 : // set the observation noise
1416 2796826 : posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 0.01f, 100.0f));
1417 : // reduce weighting (increase observation noise) on baro if we are likely to be experiencing rotor wash ground interaction
1418 2796826 : if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) {
1419 154295 : posDownObsNoise *= frontend->gndEffectBaroScaler;
1420 : }
1421 2796826 : velPosObs[5] = -hgtMea;
1422 15951752 : } else if ((activeHgtSource == AP_NavEKF_Source::SourceZ::NONE && imuSampleTime_ms - lastHgtPassTime_ms > 70)) {
1423 : // fuse a constant height of 0 at 14 Hz
1424 0 : hgtMea = 0.0f;
1425 0 : fuseHgtData = true;
1426 0 : velPosObs[5] = -hgtMea;
1427 0 : if (onGround) {
1428 : // use a typical vertical positoin observation noise when not flying for faster IMU delta velocity bias estimation
1429 0 : posDownObsNoise = sq(2.0f);
1430 : } else {
1431 : // alow a larger value when flying to accomodate vertical maneouvres
1432 0 : posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 2.0f, 100.0f));
1433 : }
1434 : } else {
1435 15951752 : fuseHgtData = false;
1436 : }
1437 :
1438 : // detect changes in source and reset height
1439 18761238 : if ((activeHgtSource != prevHgtSource) && fuseHgtData) {
1440 25 : prevHgtSource = activeHgtSource;
1441 25 : ResetPositionD(-hgtMea);
1442 : }
1443 :
1444 : // If we haven't fused height data for a while or have bad IMU data, then declare the height data as being timed out
1445 : // set height timeout period based on whether we have vertical GPS velocity available to constrain drift
1446 18761238 : hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
1447 18761238 : if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms ||
1448 18756883 : (badIMUdata &&
1449 17120 : (imuSampleTime_ms - goodIMUdata_ms > BAD_IMU_DATA_TIMEOUT_MS))) {
1450 4387 : hgtTimeout = true;
1451 : } else {
1452 18756851 : hgtTimeout = false;
1453 : }
1454 18761238 : }
1455 :
1456 : #if EK3_FEATURE_BODY_ODOM
1457 : /*
1458 : * Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
1459 : * The script file used to generate these and other equations in this filter can be found here:
1460 : * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
1461 : */
1462 4168 : void NavEKF3_core::FuseBodyVel()
1463 : {
1464 : Vector24 H_VEL;
1465 4168 : Vector3F bodyVelPred;
1466 :
1467 : // Copy required states to local variable names
1468 4168 : ftype q0 = stateStruct.quat[0];
1469 4168 : ftype q1 = stateStruct.quat[1];
1470 4168 : ftype q2 = stateStruct.quat[2];
1471 4168 : ftype q3 = stateStruct.quat[3];
1472 4168 : ftype vn = stateStruct.velocity.x;
1473 4168 : ftype ve = stateStruct.velocity.y;
1474 4168 : ftype vd = stateStruct.velocity.z;
1475 :
1476 : // Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated
1477 16672 : for (uint8_t obsIndex=0; obsIndex<=2; obsIndex++) {
1478 :
1479 : // calculate relative velocity in sensor frame including the relative motion due to rotation
1480 12504 : bodyVelPred = (prevTnb * stateStruct.velocity);
1481 :
1482 : // correct sensor offset body frame position offset relative to IMU
1483 12504 : Vector3F posOffsetBody = bodyOdmDataDelayed.body_offset - accelPosOffset;
1484 :
1485 : // correct prediction for relative motion due to rotation
1486 : // note - % operator overloaded for cross product
1487 12504 : if (imuDataDelayed.delAngDT > 0.001f) {
1488 12504 : bodyVelPred += (imuDataDelayed.delAng * (1.0f / imuDataDelayed.delAngDT)) % posOffsetBody;
1489 : }
1490 :
1491 : // calculate observation jacobians and Kalman gains
1492 12504 : if (obsIndex == 0) {
1493 : // calculate X axis observation Jacobian
1494 4168 : H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
1495 4168 : H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
1496 4168 : H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
1497 4168 : H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
1498 4168 : H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
1499 4168 : H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
1500 4168 : H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
1501 : for (uint8_t index = 7; index < 24; index++) {
1502 : H_VEL[index] = 0.0f;
1503 : }
1504 :
1505 : // calculate intermediate expressions for X axis Kalman gains
1506 4168 : ftype R_VEL = sq(bodyOdmDataDelayed.velErr);
1507 4168 : ftype t2 = q0*q3*2.0f;
1508 4168 : ftype t3 = q1*q2*2.0f;
1509 4168 : ftype t4 = t2+t3;
1510 4168 : ftype t5 = q0*q0;
1511 4168 : ftype t6 = q1*q1;
1512 4168 : ftype t7 = q2*q2;
1513 4168 : ftype t8 = q3*q3;
1514 4168 : ftype t9 = t5+t6-t7-t8;
1515 4168 : ftype t10 = q0*q2*2.0f;
1516 4168 : ftype t25 = q1*q3*2.0f;
1517 4168 : ftype t11 = t10-t25;
1518 4168 : ftype t12 = q3*ve*2.0f;
1519 4168 : ftype t13 = q0*vn*2.0f;
1520 4168 : ftype t26 = q2*vd*2.0f;
1521 4168 : ftype t14 = t12+t13-t26;
1522 4168 : ftype t15 = q3*vd*2.0f;
1523 4168 : ftype t16 = q2*ve*2.0f;
1524 4168 : ftype t17 = q1*vn*2.0f;
1525 4168 : ftype t18 = t15+t16+t17;
1526 4168 : ftype t19 = q0*vd*2.0f;
1527 4168 : ftype t20 = q2*vn*2.0f;
1528 4168 : ftype t27 = q1*ve*2.0f;
1529 4168 : ftype t21 = t19+t20-t27;
1530 4168 : ftype t22 = q1*vd*2.0f;
1531 4168 : ftype t23 = q0*ve*2.0f;
1532 4168 : ftype t28 = q3*vn*2.0f;
1533 4168 : ftype t24 = t22+t23-t28;
1534 4168 : ftype t29 = P[0][0]*t14;
1535 4168 : ftype t30 = P[1][1]*t18;
1536 4168 : ftype t31 = P[4][5]*t9;
1537 4168 : ftype t32 = P[5][5]*t4;
1538 4168 : ftype t33 = P[0][5]*t14;
1539 4168 : ftype t34 = P[1][5]*t18;
1540 4168 : ftype t35 = P[3][5]*t24;
1541 4168 : ftype t79 = P[6][5]*t11;
1542 4168 : ftype t80 = P[2][5]*t21;
1543 4168 : ftype t36 = t31+t32+t33+t34+t35-t79-t80;
1544 4168 : ftype t37 = t4*t36;
1545 4168 : ftype t38 = P[4][6]*t9;
1546 4168 : ftype t39 = P[5][6]*t4;
1547 4168 : ftype t40 = P[0][6]*t14;
1548 4168 : ftype t41 = P[1][6]*t18;
1549 4168 : ftype t42 = P[3][6]*t24;
1550 4168 : ftype t81 = P[6][6]*t11;
1551 4168 : ftype t82 = P[2][6]*t21;
1552 4168 : ftype t43 = t38+t39+t40+t41+t42-t81-t82;
1553 4168 : ftype t44 = P[4][0]*t9;
1554 4168 : ftype t45 = P[5][0]*t4;
1555 4168 : ftype t46 = P[1][0]*t18;
1556 4168 : ftype t47 = P[3][0]*t24;
1557 4168 : ftype t84 = P[6][0]*t11;
1558 4168 : ftype t85 = P[2][0]*t21;
1559 4168 : ftype t48 = t29+t44+t45+t46+t47-t84-t85;
1560 4168 : ftype t49 = t14*t48;
1561 4168 : ftype t50 = P[4][1]*t9;
1562 4168 : ftype t51 = P[5][1]*t4;
1563 4168 : ftype t52 = P[0][1]*t14;
1564 4168 : ftype t53 = P[3][1]*t24;
1565 4168 : ftype t86 = P[6][1]*t11;
1566 4168 : ftype t87 = P[2][1]*t21;
1567 4168 : ftype t54 = t30+t50+t51+t52+t53-t86-t87;
1568 4168 : ftype t55 = t18*t54;
1569 4168 : ftype t56 = P[4][2]*t9;
1570 4168 : ftype t57 = P[5][2]*t4;
1571 4168 : ftype t58 = P[0][2]*t14;
1572 4168 : ftype t59 = P[1][2]*t18;
1573 4168 : ftype t60 = P[3][2]*t24;
1574 4168 : ftype t78 = P[2][2]*t21;
1575 4168 : ftype t88 = P[6][2]*t11;
1576 4168 : ftype t61 = t56+t57+t58+t59+t60-t78-t88;
1577 4168 : ftype t62 = P[4][3]*t9;
1578 4168 : ftype t63 = P[5][3]*t4;
1579 4168 : ftype t64 = P[0][3]*t14;
1580 4168 : ftype t65 = P[1][3]*t18;
1581 4168 : ftype t66 = P[3][3]*t24;
1582 4168 : ftype t90 = P[6][3]*t11;
1583 4168 : ftype t91 = P[2][3]*t21;
1584 4168 : ftype t67 = t62+t63+t64+t65+t66-t90-t91;
1585 4168 : ftype t68 = t24*t67;
1586 4168 : ftype t69 = P[4][4]*t9;
1587 4168 : ftype t70 = P[5][4]*t4;
1588 4168 : ftype t71 = P[0][4]*t14;
1589 4168 : ftype t72 = P[1][4]*t18;
1590 4168 : ftype t73 = P[3][4]*t24;
1591 4168 : ftype t92 = P[6][4]*t11;
1592 4168 : ftype t93 = P[2][4]*t21;
1593 4168 : ftype t74 = t69+t70+t71+t72+t73-t92-t93;
1594 4168 : ftype t75 = t9*t74;
1595 4168 : ftype t83 = t11*t43;
1596 4168 : ftype t89 = t21*t61;
1597 4168 : ftype t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
1598 : ftype t77;
1599 :
1600 : // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
1601 4168 : if (t76 > R_VEL) {
1602 4168 : t77 = 1.0f/t76;
1603 4168 : faultStatus.bad_xvel = false;
1604 : } else {
1605 : t76 = R_VEL;
1606 : t77 = 1.0f/R_VEL;
1607 0 : faultStatus.bad_xvel = true;
1608 0 : return;
1609 : }
1610 4168 : varInnovBodyVel[0] = t76;
1611 :
1612 : // calculate innovation for X axis observation
1613 4168 : innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x;
1614 :
1615 : // calculate Kalman gains for X-axis observation
1616 4168 : Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
1617 4168 : Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
1618 4168 : Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
1619 4168 : Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
1620 4168 : Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
1621 4168 : Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
1622 4168 : Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
1623 :
1624 : // values to calculate in Kfusion (others are set to zero, indices 0-6 ignored)
1625 : uint32_t kalman_mask = (1<<7) | (1<<8) | (1<<9);
1626 :
1627 4168 : if (!inhibitDelAngBiasStates) {
1628 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
1629 : }
1630 :
1631 4168 : if (!inhibitDelVelBiasStates && !badIMUdata) {
1632 16488 : for (uint8_t index = 0; index < 3; index++) {
1633 12366 : const uint8_t stateIndex = index + 13;
1634 12366 : if (!dvelBiasAxisInhibit[index]) {
1635 9472 : kalman_mask |= (1<<stateIndex);
1636 : }
1637 : }
1638 : }
1639 :
1640 4168 : if (!inhibitMagStates) {
1641 2150 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
1642 : }
1643 :
1644 4168 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
1645 0 : kalman_mask |= (1<<22) | (1<<23);
1646 : }
1647 :
1648 75024 : for (auto i=7; i<24; i++) { // 0-6 are already computed
1649 : ftype res = 0;
1650 70856 : if (kalman_mask & (1<<i)) {
1651 47242 : res = t77*(P[i][5]*t4+P[i][4]*t9+P[i][0]*t14-P[i][6]*t11+P[i][1]*t18-P[i][2]*t21+P[i][3]*t24);
1652 : }
1653 70856 : Kfusion[i] = res;
1654 : }
1655 8336 : } else if (obsIndex == 1) {
1656 : // calculate Y axis observation Jacobian
1657 4168 : H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
1658 4168 : H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
1659 4168 : H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
1660 4168 : H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
1661 4168 : H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
1662 4168 : H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
1663 4168 : H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
1664 : for (uint8_t index = 7; index < 24; index++) {
1665 : H_VEL[index] = 0.0f;
1666 : }
1667 :
1668 : // calculate intermediate expressions for Y axis Kalman gains
1669 4168 : ftype R_VEL = sq(bodyOdmDataDelayed.velErr);
1670 : ftype t2 = q0*q3*2.0f;
1671 : ftype t9 = q1*q2*2.0f;
1672 4168 : ftype t3 = t2-t9;
1673 : ftype t4 = q0*q0;
1674 : ftype t5 = q1*q1;
1675 : ftype t6 = q2*q2;
1676 : ftype t7 = q3*q3;
1677 : ftype t8 = t4-t5+t6-t7;
1678 : ftype t10 = q0*q1*2.0f;
1679 : ftype t11 = q2*q3*2.0f;
1680 : ftype t12 = t10+t11;
1681 : ftype t13 = q1*vd*2.0f;
1682 : ftype t14 = q0*ve*2.0f;
1683 : ftype t26 = q3*vn*2.0f;
1684 : ftype t15 = t13+t14-t26;
1685 : ftype t16 = q0*vd*2.0f;
1686 : ftype t17 = q2*vn*2.0f;
1687 : ftype t27 = q1*ve*2.0f;
1688 4168 : ftype t18 = t16+t17-t27;
1689 : ftype t19 = q3*vd*2.0f;
1690 : ftype t20 = q2*ve*2.0f;
1691 : ftype t21 = q1*vn*2.0f;
1692 : ftype t22 = t19+t20+t21;
1693 : ftype t23 = q3*ve*2.0f;
1694 : ftype t24 = q0*vn*2.0f;
1695 : ftype t28 = q2*vd*2.0f;
1696 4168 : ftype t25 = t23+t24-t28;
1697 4168 : ftype t29 = P[0][0]*t15;
1698 4168 : ftype t30 = P[1][1]*t18;
1699 4168 : ftype t31 = P[5][4]*t8;
1700 4168 : ftype t32 = P[6][4]*t12;
1701 4168 : ftype t33 = P[0][4]*t15;
1702 4168 : ftype t34 = P[1][4]*t18;
1703 4168 : ftype t35 = P[2][4]*t22;
1704 4168 : ftype t78 = P[4][4]*t3;
1705 4168 : ftype t79 = P[3][4]*t25;
1706 4168 : ftype t36 = t31+t32+t33+t34+t35-t78-t79;
1707 4168 : ftype t37 = P[5][6]*t8;
1708 4168 : ftype t38 = P[6][6]*t12;
1709 4168 : ftype t39 = P[0][6]*t15;
1710 4168 : ftype t40 = P[1][6]*t18;
1711 4168 : ftype t41 = P[2][6]*t22;
1712 4168 : ftype t81 = P[4][6]*t3;
1713 4168 : ftype t82 = P[3][6]*t25;
1714 4168 : ftype t42 = t37+t38+t39+t40+t41-t81-t82;
1715 4168 : ftype t43 = t12*t42;
1716 4168 : ftype t44 = P[5][0]*t8;
1717 4168 : ftype t45 = P[6][0]*t12;
1718 4168 : ftype t46 = P[1][0]*t18;
1719 4168 : ftype t47 = P[2][0]*t22;
1720 4168 : ftype t83 = P[4][0]*t3;
1721 4168 : ftype t84 = P[3][0]*t25;
1722 4168 : ftype t48 = t29+t44+t45+t46+t47-t83-t84;
1723 4168 : ftype t49 = t15*t48;
1724 4168 : ftype t50 = P[5][1]*t8;
1725 4168 : ftype t51 = P[6][1]*t12;
1726 4168 : ftype t52 = P[0][1]*t15;
1727 4168 : ftype t53 = P[2][1]*t22;
1728 4168 : ftype t85 = P[4][1]*t3;
1729 4168 : ftype t86 = P[3][1]*t25;
1730 4168 : ftype t54 = t30+t50+t51+t52+t53-t85-t86;
1731 4168 : ftype t55 = t18*t54;
1732 4168 : ftype t56 = P[5][2]*t8;
1733 4168 : ftype t57 = P[6][2]*t12;
1734 4168 : ftype t58 = P[0][2]*t15;
1735 4168 : ftype t59 = P[1][2]*t18;
1736 4168 : ftype t60 = P[2][2]*t22;
1737 4168 : ftype t87 = P[4][2]*t3;
1738 4168 : ftype t88 = P[3][2]*t25;
1739 4168 : ftype t61 = t56+t57+t58+t59+t60-t87-t88;
1740 4168 : ftype t62 = t22*t61;
1741 4168 : ftype t63 = P[5][3]*t8;
1742 4168 : ftype t64 = P[6][3]*t12;
1743 4168 : ftype t65 = P[0][3]*t15;
1744 4168 : ftype t66 = P[1][3]*t18;
1745 4168 : ftype t67 = P[2][3]*t22;
1746 4168 : ftype t89 = P[4][3]*t3;
1747 4168 : ftype t90 = P[3][3]*t25;
1748 4168 : ftype t68 = t63+t64+t65+t66+t67-t89-t90;
1749 4168 : ftype t69 = P[5][5]*t8;
1750 4168 : ftype t70 = P[6][5]*t12;
1751 4168 : ftype t71 = P[0][5]*t15;
1752 4168 : ftype t72 = P[1][5]*t18;
1753 4168 : ftype t73 = P[2][5]*t22;
1754 4168 : ftype t92 = P[4][5]*t3;
1755 4168 : ftype t93 = P[3][5]*t25;
1756 4168 : ftype t74 = t69+t70+t71+t72+t73-t92-t93;
1757 4168 : ftype t75 = t8*t74;
1758 4168 : ftype t80 = t3*t36;
1759 4168 : ftype t91 = t25*t68;
1760 4168 : ftype t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
1761 : ftype t77;
1762 :
1763 : // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
1764 4168 : if (t76 > R_VEL) {
1765 4168 : t77 = 1.0f/t76;
1766 4168 : faultStatus.bad_yvel = false;
1767 : } else {
1768 : t76 = R_VEL;
1769 : t77 = 1.0f/R_VEL;
1770 0 : faultStatus.bad_yvel = true;
1771 0 : return;
1772 : }
1773 4168 : varInnovBodyVel[1] = t76;
1774 :
1775 : // calculate innovation for Y axis observation
1776 4168 : innovBodyVel[1] = bodyVelPred.y - bodyOdmDataDelayed.vel.y;
1777 :
1778 : // calculate Kalman gains for Y-axis observation
1779 4168 : Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
1780 4168 : Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
1781 4168 : Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
1782 4168 : Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
1783 4168 : Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
1784 4168 : Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
1785 4168 : Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
1786 :
1787 : // values to calculate in Kfusion (others are set to zero, indices 0-6 ignored)
1788 : uint32_t kalman_mask = (1<<7) | (1<<8) | (1<<9);
1789 :
1790 4168 : if (!inhibitDelAngBiasStates) {
1791 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
1792 : }
1793 :
1794 4168 : if (!inhibitDelVelBiasStates && !badIMUdata) {
1795 16488 : for (uint8_t index = 0; index < 3; index++) {
1796 12366 : const uint8_t stateIndex = index + 13;
1797 12366 : if (!dvelBiasAxisInhibit[index]) {
1798 9472 : kalman_mask |= (1<<stateIndex);
1799 : }
1800 : }
1801 : }
1802 :
1803 4168 : if (!inhibitMagStates) {
1804 2150 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
1805 : }
1806 :
1807 4168 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
1808 0 : kalman_mask |= (1<<22) | (1<<23);
1809 : }
1810 :
1811 75024 : for (auto i=7; i<24; i++) { // 0-6 are already computed
1812 : ftype res = 0;
1813 70856 : if (kalman_mask & (1<<i)) {
1814 47242 : res = t77*(-P[i][4]*t3+P[i][5]*t8+P[i][0]*t15+P[i][6]*t12+P[i][1]*t18+P[i][2]*t22-P[i][3]*t25);
1815 : }
1816 70856 : Kfusion[i] = res;
1817 : }
1818 : } else if (obsIndex == 2) {
1819 : // calculate Z axis observation Jacobian
1820 4168 : H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
1821 4168 : H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
1822 4168 : H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
1823 4168 : H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
1824 4168 : H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
1825 4168 : H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
1826 4168 : H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
1827 : for (uint8_t index = 7; index < 24; index++) {
1828 : H_VEL[index] = 0.0f;
1829 : }
1830 :
1831 : // calculate intermediate expressions for Z axis Kalman gains
1832 4168 : ftype R_VEL = sq(bodyOdmDataDelayed.velErr);
1833 : ftype t2 = q0*q2*2.0f;
1834 : ftype t3 = q1*q3*2.0f;
1835 : ftype t4 = t2+t3;
1836 : ftype t5 = q0*q0;
1837 : ftype t6 = q1*q1;
1838 : ftype t7 = q2*q2;
1839 : ftype t8 = q3*q3;
1840 : ftype t9 = t5-t6-t7+t8;
1841 : ftype t10 = q0*q1*2.0f;
1842 : ftype t25 = q2*q3*2.0f;
1843 4168 : ftype t11 = t10-t25;
1844 : ftype t12 = q0*vd*2.0f;
1845 : ftype t13 = q2*vn*2.0f;
1846 : ftype t26 = q1*ve*2.0f;
1847 4168 : ftype t14 = t12+t13-t26;
1848 : ftype t15 = q1*vd*2.0f;
1849 : ftype t16 = q0*ve*2.0f;
1850 : ftype t27 = q3*vn*2.0f;
1851 4168 : ftype t17 = t15+t16-t27;
1852 : ftype t18 = q3*ve*2.0f;
1853 : ftype t19 = q0*vn*2.0f;
1854 : ftype t28 = q2*vd*2.0f;
1855 4168 : ftype t20 = t18+t19-t28;
1856 : ftype t21 = q3*vd*2.0f;
1857 : ftype t22 = q2*ve*2.0f;
1858 : ftype t23 = q1*vn*2.0f;
1859 : ftype t24 = t21+t22+t23;
1860 4168 : ftype t29 = P[0][0]*t14;
1861 4168 : ftype t30 = P[6][4]*t9;
1862 4168 : ftype t31 = P[4][4]*t4;
1863 4168 : ftype t32 = P[0][4]*t14;
1864 4168 : ftype t33 = P[2][4]*t20;
1865 4168 : ftype t34 = P[3][4]*t24;
1866 4168 : ftype t78 = P[5][4]*t11;
1867 4168 : ftype t79 = P[1][4]*t17;
1868 4168 : ftype t35 = t30+t31+t32+t33+t34-t78-t79;
1869 4168 : ftype t36 = t4*t35;
1870 4168 : ftype t37 = P[6][5]*t9;
1871 4168 : ftype t38 = P[4][5]*t4;
1872 4168 : ftype t39 = P[0][5]*t14;
1873 4168 : ftype t40 = P[2][5]*t20;
1874 4168 : ftype t41 = P[3][5]*t24;
1875 4168 : ftype t80 = P[5][5]*t11;
1876 4168 : ftype t81 = P[1][5]*t17;
1877 4168 : ftype t42 = t37+t38+t39+t40+t41-t80-t81;
1878 4168 : ftype t43 = P[6][0]*t9;
1879 4168 : ftype t44 = P[4][0]*t4;
1880 4168 : ftype t45 = P[2][0]*t20;
1881 4168 : ftype t46 = P[3][0]*t24;
1882 4168 : ftype t83 = P[5][0]*t11;
1883 4168 : ftype t84 = P[1][0]*t17;
1884 4168 : ftype t47 = t29+t43+t44+t45+t46-t83-t84;
1885 4168 : ftype t48 = t14*t47;
1886 4168 : ftype t49 = P[6][1]*t9;
1887 4168 : ftype t50 = P[4][1]*t4;
1888 4168 : ftype t51 = P[0][1]*t14;
1889 4168 : ftype t52 = P[2][1]*t20;
1890 4168 : ftype t53 = P[3][1]*t24;
1891 4168 : ftype t85 = P[5][1]*t11;
1892 4168 : ftype t86 = P[1][1]*t17;
1893 4168 : ftype t54 = t49+t50+t51+t52+t53-t85-t86;
1894 4168 : ftype t55 = P[6][2]*t9;
1895 4168 : ftype t56 = P[4][2]*t4;
1896 4168 : ftype t57 = P[0][2]*t14;
1897 4168 : ftype t58 = P[2][2]*t20;
1898 4168 : ftype t59 = P[3][2]*t24;
1899 4168 : ftype t88 = P[5][2]*t11;
1900 4168 : ftype t89 = P[1][2]*t17;
1901 4168 : ftype t60 = t55+t56+t57+t58+t59-t88-t89;
1902 4168 : ftype t61 = t20*t60;
1903 4168 : ftype t62 = P[6][3]*t9;
1904 4168 : ftype t63 = P[4][3]*t4;
1905 4168 : ftype t64 = P[0][3]*t14;
1906 4168 : ftype t65 = P[2][3]*t20;
1907 4168 : ftype t66 = P[3][3]*t24;
1908 4168 : ftype t90 = P[5][3]*t11;
1909 4168 : ftype t91 = P[1][3]*t17;
1910 4168 : ftype t67 = t62+t63+t64+t65+t66-t90-t91;
1911 4168 : ftype t68 = t24*t67;
1912 4168 : ftype t69 = P[6][6]*t9;
1913 4168 : ftype t70 = P[4][6]*t4;
1914 4168 : ftype t71 = P[0][6]*t14;
1915 4168 : ftype t72 = P[2][6]*t20;
1916 4168 : ftype t73 = P[3][6]*t24;
1917 4168 : ftype t92 = P[5][6]*t11;
1918 4168 : ftype t93 = P[1][6]*t17;
1919 4168 : ftype t74 = t69+t70+t71+t72+t73-t92-t93;
1920 4168 : ftype t75 = t9*t74;
1921 4168 : ftype t82 = t11*t42;
1922 4168 : ftype t87 = t17*t54;
1923 4168 : ftype t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
1924 : ftype t77;
1925 :
1926 : // calculate innovation variance for Z axis observation and protect against a badly conditioned calculation
1927 4168 : if (t76 > R_VEL) {
1928 4168 : t77 = 1.0f/t76;
1929 4168 : faultStatus.bad_zvel = false;
1930 : } else {
1931 : t76 = R_VEL;
1932 : t77 = 1.0f/R_VEL;
1933 0 : faultStatus.bad_zvel = true;
1934 0 : return;
1935 : }
1936 4168 : varInnovBodyVel[2] = t76;
1937 :
1938 : // calculate innovation for Z axis observation
1939 4168 : innovBodyVel[2] = bodyVelPred.z - bodyOdmDataDelayed.vel.z;
1940 :
1941 : // calculate Kalman gains for Z-axis observation
1942 4168 : Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
1943 4168 : Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
1944 4168 : Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
1945 4168 : Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
1946 4168 : Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
1947 4168 : Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
1948 4168 : Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
1949 :
1950 : // values to calculate in Kfusion (others are set to zero, indices 0-6 ignored)
1951 : uint32_t kalman_mask = (1<<7) | (1<<8) | (1<<9);
1952 :
1953 4168 : if (!inhibitDelAngBiasStates) {
1954 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
1955 : }
1956 :
1957 4168 : if (!inhibitDelVelBiasStates && !badIMUdata) {
1958 16488 : for (uint8_t index = 0; index < 3; index++) {
1959 12366 : const uint8_t stateIndex = index + 13;
1960 12366 : if (!dvelBiasAxisInhibit[index]) {
1961 9472 : kalman_mask |= (1<<stateIndex);
1962 : }
1963 : }
1964 : }
1965 :
1966 4168 : if (!inhibitMagStates) {
1967 2150 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
1968 : }
1969 :
1970 4168 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
1971 0 : kalman_mask |= (1<<22) | (1<<23);
1972 : }
1973 :
1974 75024 : for (auto i=7; i<24; i++) { // 0-6 are already computed
1975 : ftype res = 0;
1976 70856 : if (kalman_mask & (1<<i)) {
1977 47242 : res = t77*(P[i][4]*t4+P[i][0]*t14+P[i][6]*t9-P[i][5]*t11-P[i][1]*t17+P[i][2]*t20+P[i][3]*t24);
1978 : }
1979 70856 : Kfusion[i] = res;
1980 : }
1981 : } else {
1982 : return;
1983 : }
1984 :
1985 : // calculate the innovation consistency test ratio
1986 : // TODO add tuning parameter for gate
1987 12504 : bodyVelTestRatio[obsIndex] = sq(innovBodyVel[obsIndex]) / (sq(5.0f) * varInnovBodyVel[obsIndex]);
1988 :
1989 : // Check the innovation for consistency and don't fuse if out of bounds
1990 : // TODO also apply angular velocity magnitude check
1991 12504 : if ((bodyVelTestRatio[obsIndex]) < 1.0f) {
1992 : // record the last time observations were accepted for fusion
1993 12504 : prevBodyVelFuseTime_ms = imuSampleTime_ms;
1994 : // notify first time only
1995 12504 : if (!bodyVelFusionActive) {
1996 6 : bodyVelFusionActive = true;
1997 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing odometry",(unsigned)imu_index);
1998 : }
1999 :
2000 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
2001 : // the zero elements of H to reduce the number of operations.
2002 250440 : for (unsigned i = 0; i<=stateIndexLim; i++) {
2003 : // j as the inner loop allows the compiler to hoist the KH product
2004 : // to save computation, and do the inner indexing more efficiently.
2005 4888032 : for (unsigned j = 0; j<=stateIndexLim; j++) {
2006 : ftype res = 0;
2007 4650096 : res += (Kfusion[i] * H_VEL[0]) * P[0][j];
2008 4650096 : res += (Kfusion[i] * H_VEL[1]) * P[1][j];
2009 4650096 : res += (Kfusion[i] * H_VEL[2]) * P[2][j];
2010 4650096 : res += (Kfusion[i] * H_VEL[3]) * P[3][j];
2011 4650096 : res += (Kfusion[i] * H_VEL[4]) * P[4][j];
2012 4650096 : res += (Kfusion[i] * H_VEL[5]) * P[5][j];
2013 4650096 : res += (Kfusion[i] * H_VEL[6]) * P[6][j];
2014 4650096 : KHP[i][j] = res;
2015 : }
2016 : }
2017 :
2018 : // finish fusion from KHP and Kfusion
2019 12504 : if (FinishFusion(innovBodyVel[obsIndex])) {
2020 : // fault, record bad axis
2021 0 : if (obsIndex == 0) {
2022 0 : faultStatus.bad_xvel = true;
2023 0 : } else if (obsIndex == 1) {
2024 0 : faultStatus.bad_yvel = true;
2025 : } else if (obsIndex == 2) {
2026 0 : faultStatus.bad_zvel = true;
2027 : }
2028 : }
2029 : }
2030 : }
2031 : }
2032 : #endif // EK3_FEATURE_BODY_ODOM
2033 :
2034 : #if EK3_FEATURE_BODY_ODOM
2035 : // select fusion of body odometry measurements
2036 19965274 : void NavEKF3_core::SelectBodyOdomFusion()
2037 : {
2038 : // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
2039 : // If so, don't fuse measurements on this time step to reduce frame over-runs
2040 : // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
2041 19965274 : if (magFusePerformed && (dtIMUavg < 0.005f) && !bodyVelFusionDelayed) {
2042 1204036 : bodyVelFusionDelayed = true;
2043 1204036 : return;
2044 : } else {
2045 18761238 : bodyVelFusionDelayed = false;
2046 : }
2047 :
2048 : // Check for body odometry data (aka visual position delta) at the fusion time horizon
2049 18761238 : const bool bodyOdomDataToFuse = storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms);
2050 18761238 : if (bodyOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV, core_index)) {
2051 :
2052 : // Fuse data into the main filter
2053 4168 : FuseBodyVel();
2054 : }
2055 :
2056 : // Check for wheel encoder data at the fusion time horizon
2057 18761238 : const bool wheelOdomDataToFuse = storedWheelOdm.recall(wheelOdmDataDelayed, imuDataDelayed.time_ms);
2058 18761238 : if (wheelOdomDataToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER, core_index)) {
2059 :
2060 : // check if the delta time is too small to calculate a velocity
2061 0 : if (wheelOdmDataDelayed.delTime > EKF_TARGET_DT) {
2062 :
2063 : // get the forward velocity
2064 0 : ftype fwdSpd = wheelOdmDataDelayed.delAng * wheelOdmDataDelayed.radius * (1.0f / wheelOdmDataDelayed.delTime);
2065 :
2066 : // get the unit vector from the projection of the X axis onto the horizontal
2067 0 : Vector3F unitVec;
2068 0 : unitVec.x = prevTnb.a.x;
2069 0 : unitVec.y = prevTnb.a.y;
2070 : unitVec.z = 0.0f;
2071 0 : unitVec.normalize();
2072 :
2073 : // multiply by forward speed to get velocity vector measured by wheel encoders
2074 0 : Vector3F velNED = unitVec * fwdSpd;
2075 :
2076 : // This is a hack to enable use of the existing body frame velocity fusion method
2077 : // TODO write a dedicated observation model for wheel encoders
2078 0 : bodyOdmDataDelayed.vel = prevTnb * velNED;
2079 0 : bodyOdmDataDelayed.body_offset = wheelOdmDataDelayed.hub_offset;
2080 0 : bodyOdmDataDelayed.velErr = frontend->_wencOdmVelErr.get();
2081 :
2082 : // Fuse data into the main filter
2083 0 : FuseBodyVel();
2084 : }
2085 : }
2086 : }
2087 : #endif // EK3_FEATURE_BODY_ODOM
|
