EKF: Fix bug causing incorrect covariance initialisation

The covariance initialisation should not be performed before the quaternion states are initialised.
This commit is contained in:
Paul Riseborough
2020-02-05 15:20:34 +01:00
committed by Lorenz Meier
parent ed260c4f1d
commit 21054a4236
4 changed files with 7 additions and 3 deletions
+1
View File
@@ -1480,6 +1480,7 @@ Vector3f Ekf::calcRotVecVariances()
}
// initialise the quaternion covariances using rotation vector variances
// do not call before quaternion states are initialised
void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
{
// calculate an equivalent rotation vector from the quaternion