Wie kann man die Kovarianz drehen?

10

Ich arbeite an einem EKF und habe eine Frage zur Koordinatenrahmenkonvertierung für Kovarianzmatrizen. Nehmen wir an, ich eine Messung erhalten mit entsprechenden 6x6 Kovarianzmatrix . Diese Messung und sind in einem Koordinatenrahmen . Ich muss die Messung in einen anderen Koordinatenrahmen umwandeln, . Das Transformieren der Messung selbst ist trivial, aber ich müsste auch ihre Kovarianz transformieren, richtig? Die Übersetzung zwischen und sollte irrelevant sein, aber ich müsste sie trotzdem drehen. Wie würde ich das machen, wenn ich richtig liege? Für die Kovarianzen zwischen , $(x, y, z, roll, pitch, yaw)$ $C$ $C$ $G_1$ $G_2$ $G_1$ $G_2$ $x$ $y$ und , mein erster Gedanke war, einfach eine 3D-Rotationsmatrix anzuwenden, aber das funktioniert nur für eine 3x3-Submatrix innerhalb der vollständigen 6x6-Kovarianzmatrix. Muss ich auf alle vier Blöcke dieselbe Drehung anwenden? $z$

kalman-filter TheWumpus
quelle

7

Kovarianz ist definiert als

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

Dabei ist in Ihrem Fall $X \in \mathbb{R}^6$ Ihr Zustandsvektor und $C$ die bereits vorhandene Kovarianzmatrix.

Für den transformierten Zustand $X'=R X$ , in Ihrem Fall mit $R \in \mathbb{R}^{6\times 6}$ , wird dies

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

Seien Sie vorsichtig mit Euler-Winkeln. Diese sind in ihrem Verhalten normalerweise nicht intuitiv, sodass Sie sie möglicherweise nicht einfach mit derselben Rotationsmatrix drehen können, die Sie für die Position verwenden. Denken Sie daran, dass sie normalerweise (in der Robotikwelt) als lokales Koordinatensystem definiert werden, während die Position normalerweise als globales Koordinatensystem definiert wird. Ich kann mich jedoch nicht erinnern, ob sie eine besondere Behandlung benötigen.

ryan0270
quelle

Vielen Dank. In diesem Fall ist

jedoch 3 × 3 und

6 × 6. Ich denke, ein Teil meines Problems besteht darin, dass ich mir nicht sicher bin, wie

die Kovarianz zwischen linearen Achsen und Rotation (oder sogar die Kovarianz der Euler-Winkel selbst) beeinflussen würde, dh wie ich

so erhöhen soll , dass es 6x6 ist.

R

$R$

C

$C$

R

$R$

R

$R$

TheWumpus

1

ist nur eine beliebige affine Transformation. In Ihrem Fall sind der obere linke 3x3-Block und der untere rechte 3x3-Block beide die Rotationsmatrix (wenn Sie davon ausgehen, dass die Euler-Winkel gleich gedreht werden können ... siehe Einschränkung in der Antwort). Die nicht diagonalen Blöcke sind Nullen.

R

$R$

Ryan0270

1

Die MRPT-Bibliothek kann dies für Sie erledigen. Sie müssen a verwenden CPose3DPDFGaussian, um Ihre Pose und Kovarianz darzustellen, und dann den +Operator verwenden.

Unter der Haube repräsentiert es Ihre 6DOF-Kovarianz als 7DOF-Quaternion-Basiskovarianz, wobei die Mathematik einfacher ist.

brice rebsamen
quelle

Es wäre von Vorteil, die Mathematik sowie eine Bibliothek zu zeigen, die dies für Sie erledigt.

Chutsu

0

Sehr intuitive Erklärung mit geometrischer Interpretation für Kovarianz und deren Zerlegung.

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

Nitish Gupta
quelle

Hallo und willkommen bei Robotics! Vielen Dank für Ihre Antwort, aber wir bevorzugen Antworten, die nach Möglichkeit in sich geschlossen sind. Links neigen dazu zu verrotten, sodass Antworten, die auf einem Link beruhen, unbrauchbar werden können, wenn der mit Inhalten verknüpfte Link verschwindet. Wenn Sie über den Link mehr Kontext hinzufügen, ist es wahrscheinlicher, dass die Leute Ihre Antwort nützlich finden.

Mactro

Wie kann man die Kovarianz drehen?

Antworten: