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

posNED to pos and velNED to vel and add ev vel covariances to reset

Browse Source
This commit is contained in:
kamilritz
2019-09-24 07:48:46 +10:00
committed by Paul Riseborough
parent 4f6ca3a74c
commit fd6b364c11
4 changed files with 14 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -147,7 +147,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observation is in local NED frame
innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
innovation[5] = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
R[5] = R[5] * R[5];