¿Cómo rotar la covarianza?

11

Estoy trabajando en un EKF y tengo una pregunta sobre la conversión de marcos de coordenadas para matrices de covarianza. Digamos que obtengo alguna medida con la matriz de covarianza 6x6 correspondiente . Esta medida y se dan en algún marco de coordenadas . Necesito transformar la medición a otro marco de coordenadas, . Transformar la medición en sí es trivial, pero también necesitaría transformar su covarianza, ¿correcto? La traducción entre y debería ser irrelevante, pero aún necesitaría rotarla. Si estoy en lo correcto, ¿cómo haría esto? Para las covarianzas entre , $(x, y, z, roll, pitch, yaw)$ $C$ $C$ $G_1$ $G_2$ $G_1$ $G_2$ $x$ $y$ , y , mi primer pensamiento fue simplemente aplicar una matriz de rotación 3D, pero eso solo funciona para una submatriz de 3x3 dentro de la matriz de covarianza de 6x6 completa. ¿Necesito aplicar la misma rotación a los cuatro bloques? $z$

kalman-filter

— TheWumpus
fuente

8

La covarianza se define como

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

donde, en su caso, es su vector de estado y es la matriz de covarianza que ya tiene. $X \in \mathbb{R}^6$ $C$

Para el estado transformado , con en su caso, esto se convierte en $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 advertencia, tenga cuidado con los ángulos de Euler. Esos son habituales no intuitivos en su comportamiento, por lo que es posible que no pueda simplemente rotarlos con la misma matriz de rotación que utiliza para la posición. Recuerde que generalmente se definen (en el mundo de la robótica) en términos del sistema de coordenadas local, mientras que la posición generalmente se define en términos del sistema de coordenadas global. Sin embargo, no recuerdo si necesitan un tratamiento especial.

— ryan0270
fuente

Gracias. En este caso, sin embargo, es 3x3 y es 6x6. Supongo que parte de mi problema es que no estoy seguro de cómo afectaría la covarianza entre los ejes lineales y la rotación (o incluso la covarianza de los ángulos de Euler), es decir, cómo debo aumentar para que sea 6x6.

R

$R$

C

$C$

R

$R$

R

$R$

— TheWumpus

1

R

$R$ es simplemente cualquier transformación afín arbitraria. En su caso, el bloque 3x3 superior izquierdo y los bloques 3x3 inferior derecho son la matriz de rotación (si supone que los ángulos de Euler pueden rotarse de la misma manera ... vea la advertencia en la respuesta). Los bloques fuera de la diagonal son ceros.

— ryan0270

1

La biblioteca MRPT puede hacer esto por usted. Debe usar a CPose3DPDFGaussianpara representar su pose y covarianza y luego usar el +operador.

Debajo del capó representa su covarianza 6DOF como una covarianza base cuaternión 7DOF, donde las matemáticas son más sencillas.

— brice rebsamen
fuente

Sería beneficioso mostrar las matemáticas y una biblioteca que lo haga por usted.

— chutsu

0

Explicación muy intuitiva con interpretación geométrica para la covarianza y su descomposición.

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

— Nitish Gupta
fuente

Hola y bienvenidos a Robotics! Gracias por su respuesta, pero preferimos que las respuestas sean autónomas siempre que sea posible. Los enlaces tienden a pudrirse, por lo que las respuestas que dependen de un enlace pueden volverse inútiles si el enlace al contenido desaparece. Si agrega más contexto desde el enlace, es más probable que las personas encuentren útil su respuesta.

— mactro