diff options
| author | Martin Miller | 2017-04-06 11:44:05 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-04-06 11:44:05 -0500 |
| commit | fe4ed819b186d56c7c66e196c4717ddc4ab99415 (patch) | |
| tree | cc1d726066f2bd3401dcde27a98407e51887a397 /src/state.cpp | |
| parent | 0a4594884adcdef15abe861fc64d74b7511c6055 (diff) | |
| download | refslam-fe4ed819b186d56c7c66e196c4717ddc4ab99415.zip refslam-fe4ed819b186d56c7c66e196c4717ddc4ab99415.tar.gz | |
Implement quaternion covariance
Pq = J*Pa*J'
where J is the Jacobian of the euler to quaternion function
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
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<double,4,4> &covq ) { - P.block<4,4>(6,6) = 1e-12*Matrix<double,4,4>::Identity(); + P.block<4,4>(6,6) = covq; return ; } /* ----- end of method State::quaternion_covariance ----- */ |