From fe4ed819b186d56c7c66e196c4717ddc4ab99415 Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Thu, 6 Apr 2017 11:44:05 -0500 Subject: Implement quaternion covariance Pq = J*Pa*J' where J is the Jacobian of the euler to quaternion function --- src/state.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/state.cpp') diff --git a/src/state.cpp b/src/state.cpp index 8b69ca2..0bae8e7 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -1024,9 +1024,9 @@ State::qhat ( const Quaterniond &q ) void -State::quaternion_covariance ( ) +State::quaternion_covariance ( const Matrix &covq ) { - P.block<4,4>(6,6) = 1e-12*Matrix::Identity(); + P.block<4,4>(6,6) = covq; return ; } /* ----- end of method State::quaternion_covariance ----- */ -- cgit v1.1