Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 :
5 : #include "AP_NavEKF3_core.h"
6 :
7 : #if EK3_FEATURE_OPTFLOW_FUSION
8 :
9 : #include <GCS_MAVLink/GCS.h>
10 : #include <AP_DAL/AP_DAL.h>
11 :
12 : /********************************************************
13 : * RESET FUNCTIONS *
14 : ********************************************************/
15 :
16 : /********************************************************
17 : * FUSE MEASURED_DATA *
18 : ********************************************************/
19 :
20 : // select fusion of optical flow measurements
21 19965274 : void NavEKF3_core::SelectFlowFusion()
22 : {
23 : // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
24 : // If so, don't fuse measurements on this time step to reduce frame over-runs
25 : // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
26 19965274 : if (magFusePerformed && dtIMUavg < 0.005f && !optFlowFusionDelayed) {
27 1204036 : optFlowFusionDelayed = true;
28 1204036 : return;
29 : } else {
30 18761238 : optFlowFusionDelayed = false;
31 : }
32 :
33 18761238 : of_elements ofDataDelayed; // OF data at the fusion time horizon
34 :
35 : // Check for data at the fusion time horizon
36 18761238 : const bool flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
37 :
38 : // Perform Data Checks
39 : // Check if the optical flow data is still valid
40 18761238 : flowDataValid = ((imuSampleTime_ms - flowValidMeaTime_ms) < 1000);
41 : // check is the terrain offset estimate is still valid - if we are using range finder as the main height reference, the ground is assumed to be at 0
42 18761238 : gndOffsetValid = ((imuSampleTime_ms - gndHgtValidTime_ms) < 5000) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER);
43 : // Perform tilt check
44 18761238 : bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
45 : // Constrain measurements to zero if takeoff is not detected and the height above ground
46 : // is insufficient to achieve acceptable focus. This allows the vehicle to be picked up
47 : // and carried to test optical flow operation
48 18761238 : if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
49 7637644 : ofDataDelayed.flowRadXYcomp.zero();
50 7637644 : ofDataDelayed.flowRadXY.zero();
51 7637644 : flowDataValid = true;
52 : }
53 :
54 : // if have valid flow or range measurements, fuse data into a 1-state EKF to estimate terrain height
55 18761238 : if (((flowDataToFuse && (frontend->_flowUse == FLOW_USE_TERRAIN)) || rangeDataToFuse) && tiltOK) {
56 : // Estimate the terrain offset (runs a one state EKF)
57 173035 : EstimateTerrainOffset(ofDataDelayed);
58 : }
59 :
60 : #if EK3_FEATURE_OPTFLOW_AGL_KF
61 : // Update the IMU-aided AGL KF every IMU step when enabled, regardless of flow/RF data presence.
62 18761238 : if (frontend->option_is_enabled(NavEKF3::Option::AglKfForOptflow)) {
63 0 : UpdateAglKf();
64 : }
65 : #endif
66 :
67 : // Fuse optical flow data into the main filter
68 18761238 : if (flowDataToFuse && tiltOK) {
69 19700 : const bool fuse_optflow = (frontend->_flowUse == FLOW_USE_NAV) && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW, core_index);
70 : // Set the flow noise used by the fusion processes
71 19700 : R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
72 : // Fuse the optical flow X and Y axis data into the main filter sequentially
73 19700 : FuseOptFlow(ofDataDelayed, fuse_optflow);
74 : }
75 : }
76 :
77 : /*
78 : Estimation of terrain offset using a single state EKF
79 : The filter can fuse motion compensated optical flow rates and range finder measurements
80 : Equations generated using https://github.com/PX4/ecl/tree/master/EKF/matlab/scripts/Terrain%20Estimator
81 : */
82 173035 : void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
83 : {
84 : // horizontal velocity squared
85 173035 : ftype velHorizSq = sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y);
86 :
87 : // don't fuse flow data if LOS rate is misaligned, without GPS, or insufficient velocity, as it is poorly observable
88 : // don't fuse flow data if it exceeds validity limits
89 : // don't update terrain offset if ground is being used as the zero height datum in the main filter
90 173035 : bool cantFuseFlowData = ((frontend->_flowUse != FLOW_USE_TERRAIN)
91 27808 : || !gpsIsInUse
92 27370 : || PV_AidingMode == AID_RELATIVE
93 27370 : || velHorizSq < 25.0f
94 189385 : || (MAX(ofDataDelayed.flowRadXY[0],ofDataDelayed.flowRadXY[1]) > frontend->_maxFlowRate));
95 :
96 173035 : if ((!rangeDataToFuse && cantFuseFlowData) || (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER)) {
97 : // skip update
98 1912 : inhibitGndState = true;
99 : } else {
100 171123 : inhibitGndState = false;
101 :
102 : // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
103 : // limit distance to prevent intialisation after bad gps causing bad numerical conditioning
104 171123 : ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
105 171123 : distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
106 171123 : prevPosN = stateStruct.position[0];
107 171123 : prevPosE = stateStruct.position[1];
108 :
109 : // in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copter's vertical velocity
110 171123 : ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
111 171123 : ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
112 171123 : Popt += Pincrement;
113 171123 : timeAtLastAuxEKF_ms = imuSampleTime_ms;
114 :
115 : // fuse range finder data
116 171123 : if (rangeDataToFuse) {
117 : // reset terrain state if rangefinder data not fused for 5 seconds
118 171123 : if (imuSampleTime_ms - gndHgtValidTime_ms > 5000) {
119 132 : terrainState = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd) + stateStruct.position.z;
120 : }
121 :
122 : // predict range
123 171123 : ftype predRngMeas = MAX((terrainState - stateStruct.position[2]),rngOnGnd) / prevTnb.c.z;
124 : // Copy required states to local variable names
125 171123 : ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
126 171123 : ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
127 171123 : ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
128 171123 : ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
129 :
130 : // Set range finder measurement noise variance. TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
131 171123 : ftype R_RNG = frontend->_rngNoise.get();
132 :
133 : // calculate Kalman gain
134 171123 : ftype SK_RNG = sq(q0) - sq(q1) - sq(q2) + sq(q3);
135 171123 : ftype K_RNG = Popt/(SK_RNG*(R_RNG + Popt/sq(SK_RNG)));
136 :
137 : // Calculate the innovation variance for data logging
138 171123 : varInnovRng = (R_RNG + Popt/sq(SK_RNG));
139 :
140 : // constrain terrain height to be below the vehicle
141 171123 : terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
142 :
143 : // Calculate the measurement innovation
144 171123 : innovRng = predRngMeas - rangeDataDelayed.rng;
145 :
146 : // calculate the innovation consistency test ratio
147 171123 : auxRngTestRatio = sq(innovRng) / (sq(MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f)) * varInnovRng);
148 :
149 : // Check the innovation test ratio and don't fuse if too large
150 171123 : if (auxRngTestRatio < 1.0f) {
151 : // correct the state
152 167425 : terrainState -= K_RNG * innovRng;
153 :
154 : // constrain the state
155 167425 : terrainState = MAX(terrainState, stateStruct.position[2] + rngOnGnd);
156 :
157 : // correct the covariance
158 167425 : Popt = Popt - sq(Popt)/(SK_RNG*(R_RNG + Popt/sq(SK_RNG))*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
159 :
160 : // prevent the state variance from becoming negative
161 167425 : Popt = MAX(Popt,0.0f);
162 :
163 : // record the time we last updated the terrain offset state
164 167425 : gndHgtValidTime_ms = imuSampleTime_ms;
165 : }
166 : }
167 :
168 171123 : if (!cantFuseFlowData) {
169 :
170 : Vector3F relVelSensor; // velocity of sensor relative to ground in sensor axes
171 16350 : Vector2F losPred; // predicted optical flow angular rate measurement
172 16350 : ftype q0 = stateStruct.quat[0]; // quaternion at optical flow measurement time
173 16350 : ftype q1 = stateStruct.quat[1]; // quaternion at optical flow measurement time
174 16350 : ftype q2 = stateStruct.quat[2]; // quaternion at optical flow measurement time
175 16350 : ftype q3 = stateStruct.quat[3]; // quaternion at optical flow measurement time
176 : ftype K_OPT;
177 : ftype H_OPT;
178 : Vector2F auxFlowObsInnovVar;
179 :
180 : // predict range to centre of image
181 16350 : ftype flowRngPred = MAX((terrainState - stateStruct.position.z),rngOnGnd) / prevTnb.c.z;
182 :
183 : // constrain terrain height to be below the vehicle
184 16350 : terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
185 :
186 : // calculate relative velocity in sensor frame
187 16350 : relVelSensor = prevTnb*stateStruct.velocity;
188 :
189 : // divide velocity by range, subtract body rates and apply scale factor to
190 : // get predicted sensed angular optical rates relative to X and Y sensor axes
191 16350 : losPred.x = relVelSensor.y / flowRngPred;
192 16350 : losPred.y = - relVelSensor.x / flowRngPred;
193 :
194 : // calculate innovations
195 16350 : auxFlowObsInnov = losPred - ofDataDelayed.flowRadXYcomp;
196 :
197 : // calculate observation jacobians
198 16350 : ftype t2 = q0*q0;
199 16350 : ftype t3 = q1*q1;
200 16350 : ftype t4 = q2*q2;
201 16350 : ftype t5 = q3*q3;
202 16350 : ftype t6 = stateStruct.position.z - terrainState;
203 16350 : ftype t7 = 1.0f / (t6*t6);
204 16350 : ftype t8 = q0*q3*2.0f;
205 16350 : ftype t9 = t2-t3-t4+t5;
206 :
207 : // prevent the state variances from becoming badly conditioned
208 16350 : Popt = MAX(Popt,1E-6f);
209 :
210 : // calculate observation noise variance from parameter
211 16350 : ftype flow_noise_variance = sq(MAX(frontend->_flowNoise, 0.05f));
212 :
213 : // Fuse Y axis data
214 :
215 : // Calculate observation partial derivative
216 16350 : H_OPT = t7*t9*(-stateStruct.velocity.z*(q0*q2*2.0-q1*q3*2.0)+stateStruct.velocity.x*(t2+t3-t4-t5)+stateStruct.velocity.y*(t8+q1*q2*2.0));
217 :
218 : // calculate innovation variance
219 16350 : auxFlowObsInnovVar.y = H_OPT * Popt * H_OPT + flow_noise_variance;
220 :
221 : // calculate Kalman gain
222 16350 : K_OPT = Popt * H_OPT / auxFlowObsInnovVar.y;
223 :
224 : // calculate the innovation consistency test ratio
225 16350 : auxFlowTestRatio.y = sq(auxFlowObsInnov.y) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.y);
226 :
227 : // don't fuse if optical flow data is outside valid range
228 16350 : if (auxFlowTestRatio.y < 1.0f) {
229 :
230 : // correct the state
231 15078 : terrainState -= K_OPT * auxFlowObsInnov.y;
232 :
233 : // constrain the state
234 15078 : terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
235 :
236 : // update intermediate variables used when fusing the X axis
237 15078 : t6 = stateStruct.position.z - terrainState;
238 15078 : t7 = 1.0f / (t6*t6);
239 :
240 : // correct the covariance
241 15078 : Popt = Popt - K_OPT * H_OPT * Popt;
242 :
243 : // prevent the state variances from becoming badly conditioned
244 15078 : Popt = MAX(Popt,1E-6f);
245 :
246 : // record the time we last updated the terrain offset state
247 15078 : gndHgtValidTime_ms = imuSampleTime_ms;
248 : }
249 :
250 : // fuse X axis data
251 16350 : H_OPT = -t7*t9*(stateStruct.velocity.z*(q0*q1*2.0+q2*q3*2.0)+stateStruct.velocity.y*(t2-t3+t4-t5)-stateStruct.velocity.x*(t8-q1*q2*2.0));
252 :
253 : // calculate innovation variances
254 16350 : auxFlowObsInnovVar.x = H_OPT * Popt * H_OPT + flow_noise_variance;
255 :
256 : // calculate Kalman gain
257 16350 : K_OPT = Popt * H_OPT / auxFlowObsInnovVar.x;
258 :
259 : // calculate the innovation consistency test ratio
260 16350 : auxFlowTestRatio.x = sq(auxFlowObsInnov.x) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * auxFlowObsInnovVar.x);
261 :
262 : // don't fuse if optical flow data is outside valid range
263 16350 : if (auxFlowTestRatio.x < 1.0f) {
264 :
265 : // correct the state
266 16350 : terrainState -= K_OPT * auxFlowObsInnov.x;
267 :
268 : // constrain the state
269 16350 : terrainState = MAX(terrainState, stateStruct.position.z + rngOnGnd);
270 :
271 : // correct the covariance
272 16350 : Popt = Popt - K_OPT * H_OPT * Popt;
273 :
274 : // prevent the state variances from becoming badly conditioned
275 16350 : Popt = MAX(Popt,1E-6f);
276 : }
277 : }
278 : }
279 173035 : }
280 :
281 : /*
282 : * Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox.
283 : * The script file used to generate these and other equations in this filter can be found here:
284 : * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
285 : * Requires a valid terrain height estimate.
286 : *
287 : * really_fuse should be true to actually fuse into the main filter, false to only calculate variances
288 : */
289 19700 : void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fuse)
290 : {
291 : Vector24 H_LOS;
292 : Vector2 losPred;
293 :
294 : // Copy required states to local variable names
295 19700 : ftype q0 = stateStruct.quat[0];
296 19700 : ftype q1 = stateStruct.quat[1];
297 19700 : ftype q2 = stateStruct.quat[2];
298 19700 : ftype q3 = stateStruct.quat[3];
299 19700 : ftype vn = stateStruct.velocity.x;
300 19700 : ftype ve = stateStruct.velocity.y;
301 19700 : ftype vd = stateStruct.velocity.z;
302 19700 : ftype pd = stateStruct.position.z;
303 :
304 : // Default is the terrain estimator AGL (terrainState - pd, where pd is the main filter's vertical position)
305 : // constrain height above ground to be above range measured on ground
306 19700 : ftype heightAboveGndEst = MAX((terrainState - pd), rngOnGnd);
307 :
308 : #if EK3_FEATURE_OPTFLOW_SRTM
309 : // if ground offset (aka terrainState) is not valid, use SRTM altitude
310 19700 : terrain_srtm_alt_valid = ((imuSampleTime_ms - terrain_srtm_alt_ms) < 5000);
311 19700 : if (!gndOffsetValid && terrain_srtm_alt_valid) {
312 0 : heightAboveGndEst = MAX((terrain_srtm_alt - pd), rngOnGnd);
313 : }
314 : #endif
315 :
316 : #if EK3_FEATURE_OPTFLOW_AGL_KF
317 : // AGL KF override: use the IMU-aided AGL KF estimate when enabled and valid,
318 : // instead of terrainState-pd which can drift when the main filter's vertical position
319 : // state is unreliable (e.g. poor altitude source, sensor outage, or ground effect).
320 19700 : if (frontend->option_is_enabled(NavEKF3::Option::AglKfForOptflow) && aglKfValid) {
321 0 : heightAboveGndEst = MAX(aglKfH, rngOnGnd);
322 : }
323 : #endif
324 :
325 : // calculate range from ground plain to centre of sensor fov assuming flat earth
326 19700 : ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
327 :
328 : // correct range for flow sensor offset body frame position offset
329 : // the corrected value is the predicted range from the sensor focal point to the
330 : // centre of the image on the ground assuming flat terrain
331 19700 : Vector3F posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
332 19700 : if (!posOffsetBody.is_zero()) {
333 0 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
334 0 : range -= posOffsetEarth.z / prevTnb.c.z;
335 : }
336 :
337 : #if APM_BUILD_TYPE(APM_BUILD_Rover)
338 : // override with user specified height (if given, for rover)
339 0 : if (ofDataDelayed.heightOverride > 0) {
340 0 : range = ofDataDelayed.heightOverride;
341 : }
342 : #endif
343 :
344 : // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
345 59100 : for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
346 :
347 : // calculate relative velocity in sensor frame including the relative motion due to rotation
348 39400 : const Vector3F relVelSensor = (prevTnb * stateStruct.velocity) + (ofDataDelayed.bodyRadXYZ % posOffsetBody);
349 :
350 : // divide velocity by range to get predicted angular LOS rates relative to X and Y axes
351 39400 : losPred[0] = relVelSensor.y/range;
352 39400 : losPred[1] = -relVelSensor.x/range;
353 :
354 : // calculate observation jacobians and Kalman gains
355 39400 : memset(&H_LOS[0], 0, sizeof(H_LOS));
356 39400 : if (obsIndex == 0) {
357 : // calculate X axis observation Jacobian
358 19700 : ftype t2 = 1.0f / range;
359 19700 : H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
360 19700 : H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
361 19700 : H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
362 19700 : H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
363 19700 : H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
364 19700 : H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
365 19700 : H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
366 :
367 : // calculate intermediate variables for the X observation innovation variance and Kalman gains
368 19700 : ftype t3 = q1*vd*2.0f;
369 19700 : ftype t4 = q0*ve*2.0f;
370 19700 : ftype t11 = q3*vn*2.0f;
371 19700 : ftype t5 = t3+t4-t11;
372 19700 : ftype t6 = q0*q3*2.0f;
373 19700 : ftype t29 = q1*q2*2.0f;
374 19700 : ftype t7 = t6-t29;
375 : ftype t8 = q0*q1*2.0f;
376 : ftype t9 = q2*q3*2.0f;
377 : ftype t10 = t8+t9;
378 19700 : ftype t12 = P[0][0]*t2*t5;
379 19700 : ftype t13 = q0*vd*2.0f;
380 19700 : ftype t14 = q2*vn*2.0f;
381 19700 : ftype t28 = q1*ve*2.0f;
382 19700 : ftype t15 = t13+t14-t28;
383 19700 : ftype t16 = q3*vd*2.0f;
384 19700 : ftype t17 = q2*ve*2.0f;
385 19700 : ftype t18 = q1*vn*2.0f;
386 19700 : ftype t19 = t16+t17+t18;
387 19700 : ftype t20 = q3*ve*2.0f;
388 19700 : ftype t21 = q0*vn*2.0f;
389 19700 : ftype t30 = q2*vd*2.0f;
390 19700 : ftype t22 = t20+t21-t30;
391 19700 : ftype t23 = q0*q0;
392 19700 : ftype t24 = q1*q1;
393 19700 : ftype t25 = q2*q2;
394 19700 : ftype t26 = q3*q3;
395 19700 : ftype t27 = t23-t24+t25-t26;
396 19700 : ftype t31 = P[1][1]*t2*t15;
397 19700 : ftype t32 = P[6][0]*t2*t10;
398 19700 : ftype t33 = P[1][0]*t2*t15;
399 19700 : ftype t34 = P[2][0]*t2*t19;
400 19700 : ftype t35 = P[5][0]*t2*t27;
401 19700 : ftype t79 = P[4][0]*t2*t7;
402 19700 : ftype t80 = P[3][0]*t2*t22;
403 19700 : ftype t36 = t12+t32+t33+t34+t35-t79-t80;
404 19700 : ftype t37 = t2*t5*t36;
405 19700 : ftype t38 = P[6][1]*t2*t10;
406 19700 : ftype t39 = P[0][1]*t2*t5;
407 19700 : ftype t40 = P[2][1]*t2*t19;
408 19700 : ftype t41 = P[5][1]*t2*t27;
409 19700 : ftype t81 = P[4][1]*t2*t7;
410 19700 : ftype t82 = P[3][1]*t2*t22;
411 19700 : ftype t42 = t31+t38+t39+t40+t41-t81-t82;
412 19700 : ftype t43 = t2*t15*t42;
413 19700 : ftype t44 = P[6][2]*t2*t10;
414 19700 : ftype t45 = P[0][2]*t2*t5;
415 19700 : ftype t46 = P[1][2]*t2*t15;
416 19700 : ftype t47 = P[2][2]*t2*t19;
417 19700 : ftype t48 = P[5][2]*t2*t27;
418 19700 : ftype t83 = P[4][2]*t2*t7;
419 19700 : ftype t84 = P[3][2]*t2*t22;
420 19700 : ftype t49 = t44+t45+t46+t47+t48-t83-t84;
421 19700 : ftype t50 = t2*t19*t49;
422 19700 : ftype t51 = P[6][3]*t2*t10;
423 19700 : ftype t52 = P[0][3]*t2*t5;
424 19700 : ftype t53 = P[1][3]*t2*t15;
425 19700 : ftype t54 = P[2][3]*t2*t19;
426 19700 : ftype t55 = P[5][3]*t2*t27;
427 19700 : ftype t85 = P[4][3]*t2*t7;
428 19700 : ftype t86 = P[3][3]*t2*t22;
429 19700 : ftype t56 = t51+t52+t53+t54+t55-t85-t86;
430 19700 : ftype t57 = P[6][5]*t2*t10;
431 19700 : ftype t58 = P[0][5]*t2*t5;
432 19700 : ftype t59 = P[1][5]*t2*t15;
433 19700 : ftype t60 = P[2][5]*t2*t19;
434 19700 : ftype t61 = P[5][5]*t2*t27;
435 19700 : ftype t88 = P[4][5]*t2*t7;
436 19700 : ftype t89 = P[3][5]*t2*t22;
437 19700 : ftype t62 = t57+t58+t59+t60+t61-t88-t89;
438 19700 : ftype t63 = t2*t27*t62;
439 19700 : ftype t64 = P[6][4]*t2*t10;
440 19700 : ftype t65 = P[0][4]*t2*t5;
441 19700 : ftype t66 = P[1][4]*t2*t15;
442 19700 : ftype t67 = P[2][4]*t2*t19;
443 19700 : ftype t68 = P[5][4]*t2*t27;
444 19700 : ftype t90 = P[4][4]*t2*t7;
445 19700 : ftype t91 = P[3][4]*t2*t22;
446 19700 : ftype t69 = t64+t65+t66+t67+t68-t90-t91;
447 19700 : ftype t70 = P[6][6]*t2*t10;
448 19700 : ftype t71 = P[0][6]*t2*t5;
449 19700 : ftype t72 = P[1][6]*t2*t15;
450 19700 : ftype t73 = P[2][6]*t2*t19;
451 19700 : ftype t74 = P[5][6]*t2*t27;
452 19700 : ftype t93 = P[4][6]*t2*t7;
453 19700 : ftype t94 = P[3][6]*t2*t22;
454 19700 : ftype t75 = t70+t71+t72+t73+t74-t93-t94;
455 19700 : ftype t76 = t2*t10*t75;
456 19700 : ftype t87 = t2*t22*t56;
457 19700 : ftype t92 = t2*t7*t69;
458 19700 : ftype t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
459 : ftype t78;
460 :
461 : // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
462 19700 : if (t77 > R_LOS) {
463 19700 : t78 = 1.0f/t77;
464 19700 : faultStatus.bad_xflow = false;
465 : } else {
466 : t77 = R_LOS;
467 : t78 = 1.0f/R_LOS;
468 0 : faultStatus.bad_xflow = true;
469 0 : return;
470 : }
471 19700 : flowVarInnov[0] = t77;
472 :
473 : // calculate innovation for X axis observation
474 : // flowInnovTime_ms will be updated when Y-axis innovations are calculated
475 19700 : flowInnov[0] = losPred[0] - ofDataDelayed.flowRadXYcomp.x;
476 :
477 : // calculate Kalman gains for X-axis observation
478 19700 : Kfusion[0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
479 19700 : Kfusion[1] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
480 19700 : Kfusion[2] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
481 19700 : Kfusion[3] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
482 19700 : Kfusion[4] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
483 19700 : Kfusion[5] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
484 19700 : Kfusion[6] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
485 :
486 : // values to calculate in Kfusion (others are set to zero, indices 0-6 ignored)
487 : uint32_t kalman_mask = (1<<7) | (1<<8) | (1<<9);
488 :
489 19700 : if (!inhibitDelAngBiasStates) {
490 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
491 : }
492 :
493 19700 : if (!inhibitDelVelBiasStates && !badIMUdata) {
494 78184 : for (uint8_t index = 0; index < 3; index++) {
495 58638 : const uint8_t stateIndex = index + 13;
496 58638 : if (!dvelBiasAxisInhibit[index]) {
497 47295 : kalman_mask |= (1<<stateIndex);
498 : }
499 : }
500 : }
501 :
502 19700 : if (!inhibitMagStates) {
503 11586 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
504 : }
505 :
506 19700 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
507 0 : kalman_mask |= (1<<22) | (1<<23);
508 : }
509 :
510 354600 : for (auto i=7; i<24; i++) { // 0-6 are already computed
511 : ftype res = 0;
512 334900 : if (kalman_mask & (1<<i)) {
513 234549 : res = t78*(P[i][0]*t2*t5-P[i][4]*t2*t7+P[i][1]*t2*t15+P[i][6]*t2*t10+P[i][2]*t2*t19-P[i][3]*t2*t22+P[i][5]*t2*t27);
514 : }
515 334900 : Kfusion[i] = res;
516 : }
517 : } else {
518 :
519 : // calculate Y axis observation Jacobian
520 19700 : ftype t2 = 1.0f / range;
521 19700 : H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
522 19700 : H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
523 19700 : H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
524 19700 : H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
525 19700 : H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
526 19700 : H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
527 19700 : H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
528 :
529 : // calculate intermediate variables for the Y observation innovation variance and Kalman gains
530 : ftype t3 = q3*ve*2.0f;
531 : ftype t4 = q0*vn*2.0f;
532 : ftype t11 = q2*vd*2.0f;
533 19700 : ftype t5 = t3+t4-t11;
534 : ftype t6 = q0*q3*2.0f;
535 : ftype t7 = q1*q2*2.0f;
536 : ftype t8 = t6+t7;
537 : ftype t9 = q0*q2*2.0f;
538 : ftype t28 = q1*q3*2.0f;
539 : ftype t10 = t9-t28;
540 19700 : ftype t12 = P[0][0]*t2*t5;
541 : ftype t13 = q3*vd*2.0f;
542 : ftype t14 = q2*ve*2.0f;
543 : ftype t15 = q1*vn*2.0f;
544 : ftype t16 = t13+t14+t15;
545 : ftype t17 = q0*vd*2.0f;
546 : ftype t18 = q2*vn*2.0f;
547 : ftype t29 = q1*ve*2.0f;
548 19700 : ftype t19 = t17+t18-t29;
549 : ftype t20 = q1*vd*2.0f;
550 : ftype t21 = q0*ve*2.0f;
551 : ftype t30 = q3*vn*2.0f;
552 : ftype t22 = t20+t21-t30;
553 : ftype t23 = q0*q0;
554 : ftype t24 = q1*q1;
555 : ftype t25 = q2*q2;
556 : ftype t26 = q3*q3;
557 : ftype t27 = t23+t24-t25-t26;
558 19700 : ftype t31 = P[1][1]*t2*t16;
559 19700 : ftype t32 = P[5][0]*t2*t8;
560 19700 : ftype t33 = P[1][0]*t2*t16;
561 19700 : ftype t34 = P[3][0]*t2*t22;
562 19700 : ftype t35 = P[4][0]*t2*t27;
563 19700 : ftype t80 = P[6][0]*t2*t10;
564 19700 : ftype t81 = P[2][0]*t2*t19;
565 19700 : ftype t36 = t12+t32+t33+t34+t35-t80-t81;
566 19700 : ftype t37 = t2*t5*t36;
567 19700 : ftype t38 = P[5][1]*t2*t8;
568 19700 : ftype t39 = P[0][1]*t2*t5;
569 19700 : ftype t40 = P[3][1]*t2*t22;
570 19700 : ftype t41 = P[4][1]*t2*t27;
571 19700 : ftype t82 = P[6][1]*t2*t10;
572 19700 : ftype t83 = P[2][1]*t2*t19;
573 19700 : ftype t42 = t31+t38+t39+t40+t41-t82-t83;
574 19700 : ftype t43 = t2*t16*t42;
575 19700 : ftype t44 = P[5][2]*t2*t8;
576 19700 : ftype t45 = P[0][2]*t2*t5;
577 19700 : ftype t46 = P[1][2]*t2*t16;
578 19700 : ftype t47 = P[3][2]*t2*t22;
579 19700 : ftype t48 = P[4][2]*t2*t27;
580 19700 : ftype t79 = P[2][2]*t2*t19;
581 19700 : ftype t84 = P[6][2]*t2*t10;
582 19700 : ftype t49 = t44+t45+t46+t47+t48-t79-t84;
583 19700 : ftype t50 = P[5][3]*t2*t8;
584 19700 : ftype t51 = P[0][3]*t2*t5;
585 19700 : ftype t52 = P[1][3]*t2*t16;
586 19700 : ftype t53 = P[3][3]*t2*t22;
587 19700 : ftype t54 = P[4][3]*t2*t27;
588 19700 : ftype t86 = P[6][3]*t2*t10;
589 19700 : ftype t87 = P[2][3]*t2*t19;
590 19700 : ftype t55 = t50+t51+t52+t53+t54-t86-t87;
591 19700 : ftype t56 = t2*t22*t55;
592 19700 : ftype t57 = P[5][4]*t2*t8;
593 19700 : ftype t58 = P[0][4]*t2*t5;
594 19700 : ftype t59 = P[1][4]*t2*t16;
595 19700 : ftype t60 = P[3][4]*t2*t22;
596 19700 : ftype t61 = P[4][4]*t2*t27;
597 19700 : ftype t88 = P[6][4]*t2*t10;
598 19700 : ftype t89 = P[2][4]*t2*t19;
599 19700 : ftype t62 = t57+t58+t59+t60+t61-t88-t89;
600 19700 : ftype t63 = t2*t27*t62;
601 19700 : ftype t64 = P[5][5]*t2*t8;
602 19700 : ftype t65 = P[0][5]*t2*t5;
603 19700 : ftype t66 = P[1][5]*t2*t16;
604 19700 : ftype t67 = P[3][5]*t2*t22;
605 19700 : ftype t68 = P[4][5]*t2*t27;
606 19700 : ftype t90 = P[6][5]*t2*t10;
607 19700 : ftype t91 = P[2][5]*t2*t19;
608 19700 : ftype t69 = t64+t65+t66+t67+t68-t90-t91;
609 19700 : ftype t70 = t2*t8*t69;
610 19700 : ftype t71 = P[5][6]*t2*t8;
611 19700 : ftype t72 = P[0][6]*t2*t5;
612 19700 : ftype t73 = P[1][6]*t2*t16;
613 19700 : ftype t74 = P[3][6]*t2*t22;
614 19700 : ftype t75 = P[4][6]*t2*t27;
615 19700 : ftype t92 = P[6][6]*t2*t10;
616 19700 : ftype t93 = P[2][6]*t2*t19;
617 19700 : ftype t76 = t71+t72+t73+t74+t75-t92-t93;
618 19700 : ftype t85 = t2*t19*t49;
619 19700 : ftype t94 = t2*t10*t76;
620 19700 : ftype t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
621 : ftype t78;
622 :
623 : // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
624 19700 : if (t77 > R_LOS) {
625 19700 : t78 = 1.0f/t77;
626 19700 : faultStatus.bad_yflow = false;
627 : } else {
628 : t77 = R_LOS;
629 : t78 = 1.0f/R_LOS;
630 0 : faultStatus.bad_yflow = true;
631 0 : return;
632 : }
633 19700 : flowVarInnov[1] = t77;
634 :
635 : // calculate innovation for Y observation
636 19700 : flowInnov[1] = losPred[1] - ofDataDelayed.flowRadXYcomp.y;
637 19700 : flowInnovTime_ms = dal.millis();
638 :
639 : // calculate Kalman gains for the Y-axis observation
640 19700 : Kfusion[0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
641 19700 : Kfusion[1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
642 19700 : Kfusion[2] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
643 19700 : Kfusion[3] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
644 19700 : Kfusion[4] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
645 19700 : Kfusion[5] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
646 19700 : Kfusion[6] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
647 :
648 : // values to calculate in Kfusion (others are set to zero, indices 0-6 ignored)
649 : uint32_t kalman_mask = (1<<7) | (1<<8) | (1<<9);
650 :
651 19700 : if (!inhibitDelAngBiasStates) {
652 : kalman_mask |= (1<<10) | (1<<11) | (1<<12);
653 : }
654 :
655 19700 : if (!inhibitDelVelBiasStates && !badIMUdata) {
656 78184 : for (uint8_t index = 0; index < 3; index++) {
657 58638 : const uint8_t stateIndex = index + 13;
658 58638 : if (!dvelBiasAxisInhibit[index]) {
659 47295 : kalman_mask |= (1<<stateIndex);
660 : }
661 : }
662 : }
663 :
664 19700 : if (!inhibitMagStates) {
665 11586 : kalman_mask |= (1<<16) | (1<<17) | (1<<18) | (1<<19) | (1<<20) | (1<<21);
666 : }
667 :
668 19700 : if (!inhibitWindStates && !treatWindStatesAsTruth) {
669 0 : kalman_mask |= (1<<22) | (1<<23);
670 : }
671 :
672 354600 : for (auto i=7; i<24; i++) { // 0-6 are already computed
673 : ftype res = 0;
674 334900 : if (kalman_mask & (1<<i)) {
675 234549 : res = -t78*(P[i][0]*t2*t5+P[i][5]*t2*t8-P[i][6]*t2*t10+P[i][1]*t2*t16-P[i][2]*t2*t19+P[i][3]*t2*t22+P[i][4]*t2*t27);
676 : }
677 334900 : Kfusion[i] = res;
678 : }
679 : }
680 :
681 : // calculate the innovation consistency test ratio
682 39400 : flowTestRatio[obsIndex] = sq(flowInnov[obsIndex]) / (sq(MAX(0.01f * (ftype)frontend->_flowInnovGate, 1.0f)) * flowVarInnov[obsIndex]);
683 :
684 : // Check the innovation for consistency and don't fuse if out of bounds or flow is too fast to be reliable
685 39400 : if (really_fuse && (flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
686 : // record the last time observations were accepted for fusion
687 15138 : prevFlowFuseTime_ms = imuSampleTime_ms;
688 : // notify first time only
689 15138 : if (!flowFusionActive) {
690 18 : flowFusionActive = true;
691 18 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);
692 : }
693 :
694 : // correct the covariance P = (I - K*H)*P = P - K*H*P. take advantage of
695 : // the zero elements of H to reduce the number of operations.
696 286194 : for (unsigned i = 0; i<=stateIndexLim; i++) {
697 : // j as the inner loop allows the compiler to hoist the KH product
698 : // to save computation, and do the inner indexing more efficiently.
699 5250096 : for (unsigned j = 0; j<=stateIndexLim; j++) {
700 : ftype res = 0;
701 4979040 : res += (Kfusion[i] * H_LOS[0]) * P[0][j];
702 4979040 : res += (Kfusion[i] * H_LOS[1]) * P[1][j];
703 4979040 : res += (Kfusion[i] * H_LOS[2]) * P[2][j];
704 4979040 : res += (Kfusion[i] * H_LOS[3]) * P[3][j];
705 4979040 : res += (Kfusion[i] * H_LOS[4]) * P[4][j];
706 4979040 : res += (Kfusion[i] * H_LOS[5]) * P[5][j];
707 4979040 : res += (Kfusion[i] * H_LOS[6]) * P[6][j];
708 4979040 : KHP[i][j] = res;
709 : }
710 : }
711 :
712 : // finish fusion from KHP and Kfusion
713 15138 : if (FinishFusion(flowInnov[obsIndex])) {
714 : // fault, record bad axis
715 0 : if (obsIndex == 0) {
716 0 : faultStatus.bad_xflow = true;
717 : } else if (obsIndex == 1) {
718 0 : faultStatus.bad_yflow = true;
719 : }
720 :
721 : }
722 : }
723 : }
724 :
725 : // store optical flow rates for use in external calibration
726 19700 : flowCalSample.timestamp_ms = imuSampleTime_ms;
727 19700 : flowCalSample.flowRate.x = ofDataDelayed.flowRadXY.x;
728 19700 : flowCalSample.flowRate.y = ofDataDelayed.flowRadXY.y;
729 19700 : flowCalSample.bodyRate.x = ofDataDelayed.bodyRadXYZ.x;
730 19700 : flowCalSample.bodyRate.y = ofDataDelayed.bodyRadXYZ.y;
731 19700 : flowCalSample.losPred.x = losPred[0];
732 19700 : flowCalSample.losPred.y = losPred[1];
733 : }
734 :
735 : // retrieve latest corrected optical flow samples (used for calibration)
736 11051 : bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
737 : {
738 11051 : if (flowCalSample.timestamp_ms != 0) {
739 11051 : timestamp_ms = flowCalSample.timestamp_ms;
740 11051 : flowRate = flowCalSample.flowRate;
741 11051 : bodyRate = flowCalSample.bodyRate;
742 11051 : losPred = flowCalSample.losPred;
743 11051 : return true;
744 : }
745 : return false;
746 : }
747 :
748 : /********************************************************
749 : * MISC FUNCTIONS *
750 : ********************************************************/
751 :
752 : #if EK3_FEATURE_OPTFLOW_AGL_KF
753 : /*
754 : * 2-state IMU-aided AGL Kalman filter
755 : *
756 : * State: x = [h_agl (m), v_agl (m/s)]' (AGL is "up")
757 : * Transition: F = [[1, imuDt], [0, 1]]
758 : * Input: u = -velDotNED.z * imuDt (gravity-included accel)
759 : * Noise: Qvel = sq(EK3_ACC_P_NSE * imuDt)
760 : * Qhgt = sq(EK3_TERR_GRAD) * horizDist²
761 : *
762 : * Prediction: x(k+1) = F*x(k) + [0, u]'
763 : * P(k+1) = F*P*F' + Q
764 : *
765 : * Observation: z = rng * prevTnb.c.z, H = [1, 0], R = sq(EK3_RNG_M_NSE)
766 : * Update: innov = z - H*x, innovVar = H*P*H' + R
767 : * K = P*H' / innovVar
768 : * x += K * innov
769 : * P = (I-KH)*P*(I-KH)' + K*R*K'
770 : */
771 0 : void NavEKF3_core::UpdateAglKf()
772 : {
773 0 : const ftype imuDt = imuDataDelayed.delVelDT;
774 : const uint32_t aglKfRngTimeout_ms = 5000; // mark filter invalid / hard-reset after this gap without RF fusion
775 :
776 : // h_agl(k+1) = h_agl(k) + v_agl(k)*imuDt
777 0 : aglKfH += aglKfV * imuDt;
778 :
779 : // v_agl(k+1) = v_agl(k) - velDotNED.z*imuDt
780 : // velDotNED.z is NED-down acceleration (positive = downward, includes gravity).
781 : // Negate: downward acceleration reduces AGL rate.
782 0 : aglKfV -= velDotNED.z * imuDt;
783 :
784 : // First-order decay of v_agl toward zero when RF is absent (tau = 2 s).
785 : // Without range measurements v_agl is unobservable; accumulated IMU bias
786 : // error will cause it to drift, pulling h_agl to the floor during
787 : // subsequent climbs. The decay limits that drift.
788 : // At the aglKfRngTimeout_ms validity timeout (5 s), |v| is at most
789 : // exp(-5/2) ~ 8% of its value at last RF fusion, so the hard reset finds v near zero.
790 0 : if (!rangeDataToFuse) {
791 : const ftype tauV = 2.0f;
792 0 : aglKfV *= expf(-imuDt / tauV);
793 : }
794 :
795 : // AGL cannot go below the on-ground sensor reading
796 0 : aglKfH = MAX(aglKfH, rngOnGnd);
797 :
798 : // ----- Covariance prediction: P = F*P*F' + Q -----
799 : //
800 : // F = [[1, imuDt], state-transition matrix
801 : // [0, 1]]
802 : //
803 : // Process noise Q:
804 : //
805 : // Qhgt — terrain-induced AGL uncertainty during forward flight.
806 : // As the vehicle moves horizontally by dist over terrain with unknown gradient "g",
807 : // the true AGL changes by ~g * dist. Since "g" is unknown, we treat it as zero-mean
808 : // with std-dev terrGradMax, giving variance: Qhgt = terrGradMax² * horizDist²
809 : // horizDist² = (vx²+vy²)*imuDt² is the squared horizontal distance travelled this step.
810 : // Capped at 1 m² so a single large-velocity step can't blow up the covariance.
811 : // The intended effect is that P[0][0] grows quickly during fast horizontal flight over rough terrain,
812 : // allowing the RF measurement to pull h_agl back when the next reading arrives.
813 : //
814 : // Qvel — unmodelled vertical accelerations (IMU noise, vibration, model error).
815 : // Uses the same accNoise parameter as CovariancePrediction (sq(imuDt*accNoise)),
816 : // so the velocity uncertainty budget is consistent with the main EKF.
817 : // The intended effect is that P[1][1] grows every step when RF is absent, reflecting accumulating IMU integration error in v_agl.
818 : //
819 0 : const ftype horizDistSq = MIN(sq(stateStruct.velocity.x * imuDt)
820 0 : + sq(stateStruct.velocity.y * imuDt), 1.0f); // cap at 1 m²
821 0 : const ftype Qvel = sq(frontend->_accNoise * imuDt); // matches CovariancePrediction: sq(imuDt*accNoise)
822 0 : const ftype Qhgt = sq(frontend->_terrGradMax) * horizDistSq;
823 :
824 : // Capture before overwrite (P is symmetric, so P[0][1] == P[1][0])
825 0 : const ftype P00 = aglKfP[0][0];
826 0 : const ftype P01 = aglKfP[0][1]; // == P[1][0]
827 0 : const ftype P11 = aglKfP[1][1];
828 :
829 : // Expanded F*P*F' + Q:
830 0 : aglKfP[0][0] = P00 + imuDt * (P01 + P01) + sq(imuDt) * P11 + Qhgt;
831 0 : aglKfP[0][1] = aglKfP[1][0] = P01 + imuDt * P11;
832 0 : aglKfP[1][1] = P11 + Qvel;
833 :
834 : // Cap covariance to prevent runaway during prolonged RF absence
835 0 : aglKfP[0][0] = MIN(aglKfP[0][0], 100.0f); // 10 m std-dev cap
836 0 : aglKfP[1][1] = MIN(aglKfP[1][1], 100.0f); // 10 m/s std-dev cap
837 :
838 : // mark invalid if RF has been absent too long
839 0 : if (!rangeDataToFuse) {
840 0 : if (imuSampleTime_ms - lastAglRngFuseTime_ms > aglKfRngTimeout_ms) {
841 0 : aglKfValid = false;
842 : }
843 0 : return;
844 : }
845 :
846 : // Only fuse when vehicle tilt is within acceptable limits
847 0 : if (prevTnb.c.z < frontend->DCM33FlowMin) {
848 : return;
849 : }
850 :
851 : // After the timeout of IMU-only propagation, vertical velocity drift makes the
852 : // prediction unreliable. Re-initialise directly from the rangefinder.
853 0 : if (imuSampleTime_ms - lastAglRngFuseTime_ms > aglKfRngTimeout_ms) {
854 : // Tilt-corrected AGL directly from rangefinder reading
855 0 : aglKfH = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
856 0 : aglKfV = 0.0f; // assume stationary on reset
857 0 : aglKfP[0][0] = sq(frontend->_rngNoise); // initialise h uncertainty to RF noise
858 0 : aglKfP[0][1] = aglKfP[1][0] = 0.0f;
859 0 : aglKfP[1][1] = 1.0f; // 1 m/s velocity uncertainty after reset
860 0 : lastAglRngFuseTime_ms = imuSampleTime_ms;
861 0 : aglKfValid = true;
862 0 : return; // skip measurement update this cycle (just used the reading for reset)
863 : }
864 :
865 : // Measurement update — fuse tilt-corrected rangefinder reading
866 : //
867 : // Observation model: z = h_agl, H = [1, 0]
868 : // z_meas = rng * cos(tilt) = rng * prevTnb.c.z
869 : //
870 0 : const ftype hgtMeas = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
871 :
872 : // Measurement noise variance R (reuse EK3_RNG_M_NSE)
873 0 : const ftype measNoiseVar = sq(frontend->_rngNoise);
874 :
875 : // Innovation and innovation covariance
876 : // hgtInnov = hgtMeas - H*x = hgtMeas - h_agl
877 : // innovVar = H*P*H' + R = P[0][0] + R
878 0 : const ftype hgtInnov = hgtMeas - aglKfH;
879 0 : const ftype innovVar = aglKfP[0][0] + measNoiseVar;
880 :
881 : // reject outliers (RF glitches, specular reflections, etc.)
882 : // gate is expressed as a multiplier on the 1-sigma bound.
883 0 : const ftype innovGate = MAX(0.01f * (ftype)frontend->_rngInnovGate, 1.0f);
884 0 : if (sq(hgtInnov) > sq(innovGate) * innovVar) {
885 : // Innovation too large, likely a glitch. Inflate both height and velocity
886 : // uncertainty so the next valid reading can correct both states more aggressively.
887 0 : aglKfP[0][0] = MIN(aglKfP[0][0] * 2.0f, 100.0f);
888 0 : aglKfP[1][1] = MIN(aglKfP[1][1] * 2.0f, 100.0f);
889 0 : return;
890 : }
891 :
892 : // Kalman gain: K = P*H' / innovVar = [P[0][0]/innovVar, P[1][0]/innovVar]'
893 : // (H = [1, 0], so P*H' = first column of P)
894 0 : const ftype Kh = aglKfP[0][0] / innovVar;
895 0 : const ftype Kv = aglKfP[1][0] / innovVar;
896 :
897 : // State update: x += K * hgtInnov
898 0 : aglKfH += Kh * hgtInnov;
899 0 : aglKfV += Kv * hgtInnov;
900 0 : aglKfH = MAX(aglKfH, rngOnGnd); // enforce physical constraint after update
901 :
902 : // Covariance update P = (I-KH)*P*(I-KH)' + K*R*K'
903 : // With H = [1, 0], (I-KH) = [[1-Kh, 0], [-Kv, 1]]:
904 : // P[0][0] = (1-Kh)²*Phh + Kh²*R
905 : // P[0][1] = (1-Kh)*(Phv - Kv*Phh) + Kh*Kv*R
906 : // P[1][1] = Pvv - 2*Kv*Phv + Kv²*innovVar
907 0 : const ftype oneMinusKh = 1.0f - Kh;
908 0 : const ftype Phh = aglKfP[0][0];
909 0 : const ftype Phv = aglKfP[0][1];
910 0 : const ftype Pvv = aglKfP[1][1];
911 :
912 0 : aglKfP[0][0] = MAX(sq(oneMinusKh) * Phh + sq(Kh) * measNoiseVar, 0.0f);
913 0 : aglKfP[0][1] = aglKfP[1][0] = oneMinusKh * (Phv - Kv * Phh) + Kh * Kv * measNoiseVar;
914 0 : aglKfP[1][1] = MAX(Pvv - 2.0f * Kv * Phv + sq(Kv) * innovVar, 0.0f);
915 :
916 0 : lastAglRngFuseTime_ms = imuSampleTime_ms;
917 0 : aglKfValid = true;
918 : }
919 :
920 : #endif // EK3_FEATURE_OPTFLOW_AGL_KF
921 :
922 : #endif // EK3_FEATURE_OPTFLOW_FUSION
|
