Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Nav_EKF3: varInnovBodyVel assigned the innovation covariance, rather than inverse

Browse Source
This commit is contained in:
Daniel Magree 2019-10-29 10:59:28 -04:00 committed by Peter Barker
parent 318d467ecc
commit b3ff81648b
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1108,7 +1108,7 @@ void NavEKF3_core::FuseBodyVel()
faultStatus.bad_xvel = true;
return;
}
varInnovBodyVel[0] = t77;
varInnovBodyVel[0] = t76;
// calculate innovation for X axis observation
innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x;
@ -1280,7 +1280,7 @@ void NavEKF3_core::FuseBodyVel()
faultStatus.bad_yvel = true;
return;
}
varInnovBodyVel[1] = t77;
varInnovBodyVel[1] = t76;
// calculate innovation for Y axis observation
innovBodyVel[1] = bodyVelPred.y - bodyOdmDataDelayed.vel.y;
@ -1452,7 +1452,7 @@ void NavEKF3_core::FuseBodyVel()
faultStatus.bad_zvel = true;
return;
}
varInnovBodyVel[2] = t77;
varInnovBodyVel[2] = t76;
// calculate innovation for Z axis observation
innovBodyVel[2] = bodyVelPred.z - bodyOdmDataDelayed.vel.z;