Comment faire pivoter la covariance?

Je travaille sur un EKF et j'ai une question concernant la conversion de trames de coordonnées pour les matrices de covariance. Disons que j'obtiens une mesure avec la matrice de covariance 6x6 correspondante . Cette mesure et sont donnés dans un cadre de coordonnées . J'ai besoin de transformer la mesure en un autre cadre de coordonnées, . Transformer la mesure elle-même est trivial, mais j'aurais également besoin de transformer sa covariance, n'est-ce pas? La traduction entre et ne devrait pas être pertinente, mais j'aurais quand même besoin de la faire pivoter. Si j'ai raison, comment pourrais-je procéder? Pour les covariances entre ,C C G 1 G 2 G 1 G 2 x $(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$et , ma première pensée a été d'appliquer simplement une matrice de rotation 3D, mais cela ne fonctionne que pour une sous-matrice 3x3 dans la matrice de covariance 6x6 complète. Dois-je appliquer la même rotation aux quatre blocs?$z$

La covariance est définie comme

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

où, dans votre cas, est votre vecteur d'état et est la matrice de covariance que vous avez déjà.$X \in \mathbb{R}^6$$C$

Pour l'état transformé , avec dans votre cas, cela devient$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}$

Comme mise en garde, soyez prudent avec les angles d'Euler. Ceux-ci sont habituellement non intuitifs dans leur comportement, vous ne pourrez donc peut-être pas simplement les faire pivoter avec la même matrice de rotation que vous utilisez pour la position. N'oubliez pas qu'ils sont généralement définis (dans le monde de la robotique) en termes de système de coordonnées local tandis que la position est généralement définie en termes de système de coordonnées global. Cependant, je ne me souviens pas s'ils ont besoin d'un traitement spécial.

La bibliothèque MRPT peut le faire pour vous. Vous devez utiliser un CPose3DPDFGaussianpour représenter votre pose et votre covariance, puis utiliser l' +opérateur.

Sous le capot, il représente votre covariance 6DOF comme une covariance de base quaternion 7DOF, où les calculs sont plus simples.

Explication très intuitive avec interprétation géométrique de la covariance et de sa décomposition.

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