Function hatchbed_common::localization::rotateCovariance

• Defined in File covariance_util.h

Function Documentation

Covariance hatchbed_common::localization::rotateCovariance(const Covariance &cov_in, const tf2::Quaternion &q)

Rotates a 6x6 covariance matrix by a rotation, applying it to both the translational and rotational components.

C_out = J * C_in * J^T, where J = diag(R, R) (block diagonal with rotation matrix R)