Comment faire pivoter la covariance?

11

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 , $(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$

kalman-filter

— TheWumpus
source

8

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.

— ryan0270
source

Merci. Dans ce cas, cependant, est 3x3 et est 6x6. Je suppose qu'une partie de mon problème est que je ne suis pas sûr de la façon dont affecterait la covariance entre les axes linéaires et la rotation (ou même la covariance des angles d'Euler eux-mêmes), c'est-à-dire, comment dois-je augmenter pour qu'il soit 6x6.

R

$R$

C

$C$

R

$R$

R

$R$

— TheWumpus

1

R

$R$ est juste n'importe quelle transformation affine arbitraire. Dans votre cas, le bloc 3x3 en haut à gauche et les blocs 3x3 en bas à droite sont tous deux la matrice de rotation (si vous supposez que les angles d'Euler peuvent être tournés de la même manière ... voir la mise en garde en réponse). Les blocs hors diagonale sont des zéros.

— ryan0270

1

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.

— brice rebsamen
source

Serait bénéfique de montrer les mathématiques ainsi qu'une bibliothèque qui le fait pour vous.

— chutsu

0

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/

— Nitish Gupta
source

Bonjour et bienvenue en robotique! Merci pour votre réponse, mais nous préférons que les réponses soient autonomes dans la mesure du possible. Les liens ont tendance à pourrir, de sorte que les réponses qui reposent sur un lien peuvent être rendues inutiles si le lien vers le contenu disparaît. Si vous ajoutez plus de contexte à partir du lien, il est plus probable que les gens trouveront votre réponse utile.

— mactro