mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-27 15:00:04 +08:00
3ebc6d6a40
* fix(ekf2): report HAGL variance for dist_bottom_var dist_bottom_var was published as P(terrain, terrain) only. When the range finder is the EKF height reference (EKF2_HGT_REF=2), the terrain state is reset to 0 and not directly observed, so this variance does not represent the actual uncertainty of dist_bottom (HAGL). Use ComputeHaglInnovVar(P, 0) to report the proper HAGL variance, which accounts for terrain state, vertical position state, and their covariance. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> * fix(ekf2): clamp HAGL variance to non-negative H*P*H^T should be >= 0 for a valid covariance, but float drift or mildly non-PSD P can produce a tiny negative result. Clamp at the accessor so consumers of lpos.dist_bottom_var never see a negative variance. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com> --------- Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>