Come ruotare la covarianza?

Sto lavorando su un EKF e ho una domanda sulla conversione dei frame di coordinate per le matrici di covarianza. Diciamo ho qualche misura con corrispondenti 6x6 matrice di covarianza C . Questa misura e C sono riportate in alcuni frame di coordinate G 1 . Devo trasformare la misurazione in un altro frame di coordinate, G $(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$. Trasformare la misurazione stessa è banale, ma avrei anche bisogno di trasformare la sua covarianza, giusto? La traduzione tra e G 2 dovrebbe essere irrilevante, ma avrei comunque bisogno di ruotarla. Se avessi ragione, come avrei fatto? Per le covarianze tra x , y , e z , il mio primo pensiero era semplicemente applicare una matrice di rotazione 3D, ma che funziona solo per una sottomatrice 3x3 all'interno della matrice di covarianza completa 6x6. Devo applicare la stessa rotazione a tutti e quattro i blocchi?$G_1$$G_2$$x$$y$$z$

La covarianza è definita come

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

dove, nel tuo caso, $X \in \mathbb{R}^6$ è il tuo vettore di stato e $C$ è la matrice di covarianza che hai già.

Per lo stato trasformato $X'=R X$ , con $R \in \mathbb{R}^{6\times 6}$ nel tuo caso, questo diventa

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

Come avvertimento, fai attenzione con gli angoli di Eulero. Questi sono normalmente non intuitivi nel loro comportamento, quindi potresti non essere in grado di ruotarli semplicemente con la stessa matrice di rotazione che usi per la posizione. Ricorda che di solito sono definiti (nel mondo della robotica) in termini di sistema di coordinate locali, mentre la posizione è generalmente definita in termini di sistema di coordinate globale. Fuori dalla mia testa, però, non riesco a ricordare se hanno bisogno di un trattamento speciale.

La libreria MRPT può farlo per te. È necessario utilizzare a CPose3DPDFGaussianper rappresentare la posa e la covarianza, quindi utilizzare l' +operatore.

Sotto il cofano rappresenta la tua covarianza 6DOF come una covarianza di base quaternione 7DOF, dove la matematica è più semplice.

Spiegazione molto intuitiva con interpretazione geometrica per la covarianza e la sua decomposizione.

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