# Yaw-pitch-roll equivalent covariance matrix of SE(3) manifold covariance

Given $$\mathbf{X} = \exp(\xi^{\wedge})\hat{\mathbf{X}} \in SE(3)$$ where $$\xi \sim (0, \mathbf{P})$$ and $$\hat{\mathbf{X}}$$ is the mean.

here $$\exp : \mathfrak{se}(3) \to SE(3)$$ and $$(\cdot)^{\wedge} : \mathbb{R}^{6} \to \mathfrak{se}(3)$$ thus, we have

$$\xi^{\wedge} = \begin{bmatrix} [\xi^{\mathbf{R}}]_{\times} & \xi^{\mathbf{t}} \\ \mathbf{0} & 0\end{bmatrix}$$ and I have that $$\mathbf{X} = \begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} & 1\end{bmatrix}$$

What I want is to find the equivalent covariance of $$\mathbf{P}$$ corresponding to the mapping from

$$\begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} & 1\end{bmatrix} \to \begin{bmatrix} x \\ y \\ z \\ \phi \\ \chi \\ \psi\end{bmatrix}$$ where $$(\phi, \chi, \psi)$$ is yaw-pitch-roll.

In the MRPT library https://www.mrpt.org/ documentation described in here http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf. In Chapter 2.5.2 they describe almost the same equivalence transformation, however, as I understand it is from an uncertainty representation in the Lie algebra and not from $$\mathbb{R}^{6}$$.

They use the approximation given $$x\sim N(\bar{x}, \Sigma_{x})$$:

$$\bar{y} = f(\bar{x})$$

$$\Sigma_{y} = \frac{\partial f(x)}{\partial x}|_{x=\bar{x}}\Sigma_{x}\frac{\partial f(x)}{\partial x}|_{x=\bar{x}}^{T}$$