Repository navigation
Conversation
Optical flow fusion leaves some states out of its Kalman gain. Their covariance with the other states still has to be corrected, and K*H*P holds that correction in their column only, so symmetrising the update applies half of it and can leave P no longer positive definite. After that the check against negative variances refuses fusions with no fault raised: sensors stop being used while the filter reports itself healthy. Give each masked state's row of K*H*P its column, so the update is the Joseph form for the gain applied. The masked states' own estimates and variances are unchanged. This fixes a regression from ArduPilot#33498, which made the Z gyro bias one of those states while no yaw is fused. Until then the only masked state with live cross-covariances was the accel bias during bad IMU data, which needs GPS vertical velocity, so a flow-only vehicle never had one. The other masked states are inhibited outright and decorrelated every step, which this leaves as it was. Replay of a flow-only Copter with no compass, launched by hand: the first flow fusion, 3 cm above the hand with the filter unaided, broke positive definiteness, and 669 fusions were refused before the vehicle came down, 2460 by the end of the log. With this change none were, and P stayed positive definite throughout. In SITL, an LDL^T probe after each flow fusion in FlowGyroZBiasNoYawReference counted 32, 373 and 173 updates leaving P indefinite in its three cases before, 0, 8 and 2 after.
Global Size Summary (Total Flash delta in bytes)Built from 47d399e rebased onto 5b36047
|
|
Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting. Reviewed at head No problems found: the row copy makes the flow covariance update match the Joseph form for states with zero gain, and both the algebra and SITL probes support it. The change (OptFlowFusion.cpp:360, OptFlowFusion.cpp:722-730) was checked against the full covariance equation over random covariances, in two extracted-code harnesses (one with a control that fails without the copy), and with a SITL positive-definiteness probe. Indefinite flow updates on core 0 dropped from 28/348/303 to 0/8/2. The flow autotests pass at head. Not independently confirmed: the replay of the flight log from #34932, and the cause of the red sitltest-quadplane-tests1c CI job. That bucket runs no optical flow tests and looks like a known flaky job. |
Summary
Fixes #34932. Optical flow fusion zeroes the gain of the states it leaves out, the Z gyro bias among them while no yaw is fused (#33498), but applied only half of their covariance correction. That can leave P no longer positive definite, after which EKF3 refuses later fusions without raising a fault: a hand-launched flow copter stopped fusing baro and climbed in altitude hold with the stick fully down. This applies the full, Joseph form, correction for those states.
Classification & Testing (check all that apply and add your own)
Replay of the hand-launch log from #34932 through master's EKF: before, P stops being positive definite at the first flow fusion and 669 fusions are refused before the vehicle comes down, 2460 by the end of the log. After, none are refused and P stays positive definite throughout. On the build that flew, the same change keeps every baro height fusion (222 of 225 were refused) and keeps the attitude level after the crash instead of drifting 2.5 deg/s on frozen gyro biases.
SITL, FlowGyroZBiasNoYawReference with an LDL^T probe of P after every flow fusion, the measurement AP-Review made on #33498. Flow updates that left P indefinite, core 0:
The 8 match the 9 AP-Review measured before #33498; the 2 are rounding at 1e-19 of the largest variance. The test passes on both with the same max |GZ| (0 to 0.01 deg/s), so flow still teaches the Z gyro bias nothing. FlowAidingRestartsWithoutYawFusion, OpticalFlow, OpticalFlowLimits, OpticalFlowLocation, OpticalFlowCalibration, EK3_AccelBiasZeroVelOptFlow, ModeFlowHold and LoiterFlowBrakeOvershoot pass. No autotest is added: none reproduces the flight's failure, for the reasons in #34932, and a passing one would not discriminate.
Description
FuseOptFlow()builds KHP for every state. A state thatkalman_maskleaves out has a zero row there, while its column holds the correction to its covariance with the other states, andFinishFusion()averages KHP with its transpose, so those cross-covariances got half. Copying each masked state's column into its row gives the Joseph form for the gain actually used, which is the standard Schmidt (consider state) update. The masked state's own estimate and variance are unchanged, as before. This fixes a regression from #33498, so it is not behind an option.test.Copter.Replaypasses.Detail for reviewers, not needed to review the change in outline:
ConstrainVariances()orCovariancePrediction(), so for them this changes nothing. The accel biases during bad IMU data keep theirs, butbadIMUdatais only set while GPS vertical velocity is fused; they now get the same consistent update.FuseVelPosNED()'s AID_NONEpoorObservabilityandhorizInhibitmasks, the range beacon height masks and wind-only airspeed fusion. Those observations are weak or the states decorrelated, so they are left for a separate change rather than moving this fix intoFinishFusion(), where it would also change Plane airspeed and beacon behaviour.