I am working on an EKF and have a question regarding coordinate frame conversion for covariance matrices. Let's say I get some measurement $(x, y, z, roll, pitch, yaw)$ with corresponding 6x6 covariance matrix $C$. This measurement and $C$ are given in some coordinate frame $G_1$. I need to transform the measurement to another coordinate frame, $G_2$. Transforming the measurement itself is trivial, but I would also need to transform its covariance, correct? The translation between $G_1$ and $G_2$ should be irrelevant, but I would still need to rotate it. If I am correct, how would I do this? For the covariances between $x$, $y$, and $z$, my first thought was to simply apply a 3D rotation matrix, but that only works for a 3x3 submatrix within the full 6x6 covariance matrix. Do I need to apply the same rotation to all four blocks?
3 Answers
Covariance is defined as
$\begin{align} C &= \mathbb{E}(XX^T) - \mathbb{E}(X)\mathbb{E}(X^T) \end{align}$
where, in your case, $X \in \mathbb{R}^6$ is your state vector and $C$ is the covariance matrix you already have.
For the transformed state $X'=R X$, with $R \in \mathbb{R}^{6\times 6}$ in your case, this becomes
$\begin{align} C' &= \mathbb{E}(X' X'^T) - \mathbb{E}(X')\mathbb{E}(X'^T) \\ &= \mathbb{E}(R X X^T R^T) - \mathbb{E}(RX)\mathbb{E}(X^T R^T) \\ &= R ~\mathbb{E}(X X^T) ~R^T - R\mathbb{E}(X)\mathbb{E}(X^T)R^T \\ &= R \big(~\mathbb{E}(X X^T) - \mathbb{E}(X)\mathbb{E}(X^T)\big)R^T \\ &= R C R^T \end{align}$
As a caveat, be careful with Euler angles. Those are usual non-intuitive in their behavior so you might not be able to simply rotate them with the same rotation matrix that you use for position. Remember that they are usually defined (in the robotics world) in terms of the local coordinate system whereas position is usually defined in terms of the global coordinate system. Off the top of my head, though, I can't remember if they need special treatment.
-
$\begingroup$ Thanks. In this case, though, $R$ is 3x3 and $C$ is 6x6. I guess part of my problem is that I'm uncertain how $R$ would affect the covariance between linear axes and rotation (or even the covariance of the Euler angles themselves), i.e., how should I augment $R$ so that it's 6x6. $\endgroup$ Commented Mar 3, 2014 at 18:14
-
1$\begingroup$ $R$ is just any arbitrary affine transformation. In your case, the top left 3x3 block and the bottom right 3x3 blocks are both the rotation matrix (if you assume that the Euler angles can be rotated the same ... see caveat in answer). The off-diagonal blocks are zeros. $\endgroup$– ryan0270Commented Mar 3, 2014 at 21:23
The MRPT library can do this for you. You need to use a CPose3DPDFGaussian to represent your pose and covariance then use the + operator.
Under the hood it represents your 6DOF covariance as a 7DOF quaternion base covariance, where the math is more straightforward.
-
$\begingroup$ Would be beneficial to show the maths as well as a library that does it for you. $\endgroup$– chutsuCommented Apr 10, 2019 at 10:04
Very intuitive explanation with geometric interpretation for covariance and its decomposition.
http://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/
-
$\begingroup$ Hello and welcome to Robotics! Thanks for your answer but we prefer answers to be self contained where possible. Links tend to rot so answers which rely on a link can be rendered useless if the linked to content disappears. If you add more context from the link, it is more likely that people will find your answer useful. $\endgroup$– mactroCommented Feb 24, 2017 at 9:17