Come ruotare la covarianza?


11

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 2(x,y,z,roll,pitch,yaw)CCG1G2. 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?G1G2xyz

Risposte:


8

La covarianza è definita come

C=E(XXT)E(X)E(XT)

dove, nel tuo caso, XR6 è il tuo vettore di stato e C è la matrice di covarianza che hai già.

Per lo stato trasformato X=RX , con RR6×6 nel tuo caso, questo diventa

C=E(XXT)E(X)E(XT)=E(RXXTRT)E(RX)E(XTRT)=R E(XXT) RTRE(X)E(XT)RT=R( E(XXT)E(X)E(XT))RT=RCRT

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.


RCRR

1
R

1

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.


Sarebbe utile mostrare la matematica e una biblioteca che lo fa per te.
Chutsu,

0

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

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


Ciao e benvenuto in Robotica! Grazie per la tua risposta, ma preferiamo che le risposte siano autosufficienti ove possibile. I collegamenti tendono a marcire, quindi le risposte che si basano su un collegamento possono essere rese inutili se il collegamento al contenuto scompare. Se aggiungi più contesto dal link, è più probabile che le persone troveranno la tua risposta utile.
mactro,
Utilizzando il nostro sito, riconosci di aver letto e compreso le nostre Informativa sui cookie e Informativa sulla privacy.
Licensed under cc by-sa 3.0 with attribution required.