fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use Joseph stabilized update in direct state observations

Browse Source
This commit is contained in:
bresch
2024-02-26 12:32:59 -05:00
committed by Daniel Agar
parent 9d9766c6cf
commit d501d8e1d4
5 changed files with 36 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/position_fusion.cpp
+3 -3
View File
@@ -80,8 +80,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
{
// x & y
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
@@ -97,7 +97,7 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
{
// z
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)
&& fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;