Compute the covariance of $R_2 R_1^T$ where $R_2$ and $R_1$ are rotation matrices with Gaussian uncertainty

112 Views Asked by At

I have estimates of two 3x3 rotation matrices $R_1$, $R_2$ expressed in terms of their expected values $R_{1\mu}$ and $R_{2\mu}$ and covariances $\Sigma_1$, $\Sigma_2$. The latter are expressed in the Lie algebra for SO(3). That is:

$$ R_1 = R_{1\mu}\exp(\omega_1) ~~~~~~~ \omega_1 ~\tilde~ \ N(0,\Sigma_1) $$

$$ R_2 = R_{2\mu}\exp(\omega_2) ~~~~~~~ \omega_2 ~\tilde~ \ N(0,\Sigma_2) $$

where $\omega_1$ and $\omega_2$ are independent random variables, exp is the exponential map from so(3) to SO(3), and $\Sigma_1$, $\Sigma_2$ are 3x3 covariance matrices.

I want to compute an expectation and covariance for $R_{\Delta} = R_2 R_1^T$, expressed again as an expected rotation matrix and 3x3 covariance in the Lie algebra for SO(3). I do not expect that this quantity is actually Gaussian-distributed so I am happy with an approximation based on an appropriate linearization.

Note that I am not guaranteed that $R_{\Delta}$ is close to the identity.

1

There are 1 best solutions below

0
On

We can compute the resulting covariance matrix by using first order uncertainty propagation (applying the chain rule when necesary):

$$ \Sigma_\Delta = J^{R_2 R_1^T}_{R_2} \Sigma_2 (J^{R_2 R_1^T}_{R_2})^T + J^{R_2 R_1^T}_{R_1^T} J^{R_1^T}_{R_1} \Sigma_1 (J^{R_1^T}_{R_1})^T (J^{R_2 R_1^T}_{R_1^T})^T $$ where:

  • $J^{R_2 R_1^T}_{R_2} = R_1$
  • $J^{R_2 R_1^T}_{R_1^T} = I$
  • $J^{R_1^T}_{R_1} = -R_1$

(see eqs. (140)-(142) of Solà's paper).

In conclusion (unless I've made some mistake):

$$ \Sigma_\Delta = R_1 (\Sigma_2 + \Sigma_1) R_1^T $$

You probably want to test it against a monte carlo simulation to check it's correct.