Como girar a covariância?

Estou trabalhando em um EKF e tenho uma pergunta sobre conversão de quadros de coordenadas para matrizes de covariância. Digamos que eu recebo alguma medida com a matriz de covariância 6x6 . Essa medida e são fornecidas em algum quadro de coordenadas . Eu preciso transformar a medida em outro quadro de coordenadas, . Transformar a medida em si é trivial, mas eu também precisaria transformar sua covariância, correto? A tradução entre e deve ser irrelevante, mas eu ainda seria necessário para a rodar. Se eu estiver correto, como eu faria isso? Para as covariâncias entre ,$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$, , meu primeiro pensamento foi simplesmente aplicar uma matriz de rotação 3D, mas isso só funciona para uma submatriz 3x3 dentro da matriz de covariância 6x6 completa. Preciso aplicar a mesma rotação nos quatro blocos?$z$

Covariância é definida como

$\begin{align} C &= \mathbb{E}(XX^T) - \mathbb{E}(X)\mathbb{E}(X^T) \end{align}$

onde, no seu caso, é seu vetor de estado e é a matriz de covariância que você já possui.$X \in \mathbb{R}^6$$C$

Para o estado transformado , com no seu caso, isso se torna$X'=R X$$R \in \mathbb{R}^{6\times 6}$

$\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}$

Como uma ressalva, tenha cuidado com os ângulos de Euler. Esses são comportamentos não-intuitivos comuns, portanto você pode não ser capaz de simplesmente girá-los com a mesma matriz de rotação usada para a posição. Lembre-se de que eles geralmente são definidos (no mundo da robótica) em termos do sistema de coordenadas local, enquanto a posição geralmente é definida em termos do sistema de coordenadas global. No topo da minha cabeça, no entanto, não me lembro se eles precisam de tratamento especial.

A biblioteca MRPT pode fazer isso por você. Você precisa usar a CPose3DPDFGaussianpara representar sua pose e covariância e depois usar o +operador.

Sob o capô, ele representa sua covariância 6DOF como uma covariância de base de quaternário 7DOF, onde a matemática é mais direta.

Explicação muito intuitiva com interpretação geométrica para covariância e sua decomposição.

http://www.visiondummy.com/2014/04/geometric-interpretation-covariance-matrix/