* 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>
* fix max-hagl restriction to position/altitude control
* max hagl vel restriction in ManAcc position mode
* use interpolate func, change naming
* simplyfied vertical vel limitation
* move velocity-constraint adjustment to StickAccelXY
