fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 14:33:33 +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/velocity_fusion.cpp
+5 -5
View File
@@ -83,8 +83,8 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
{
// vx, vy
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
@@ -100,9 +100,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
{
// vx, vy, vz
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2)
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1)
&& fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;