Як обертати коваріацію?

Я працюю над EKF і маю запитання щодо перетворення кадру координат для коваріаційних матриць. Скажімо, я отримую деяке вимірювання$(x, y, z, roll, pitch, yaw)$ з відповідною коваріаційною матрицею 6x6 $C$. Це вимірювання і$C$ наведені в деякому координатному кадрі $G_1$. Мені потрібно перетворити вимірювання в інший координатний кадр,$G_2$. Перетворення самого вимірювання є тривіальним, але мені також потрібно би перетворити його коваріантність, правильно? Переклад між$G_1$ і $G_2$має бути неактуальним, але мені все одно потрібно його обертати. Якщо я маю рацію, як би це зробити? Для коваріацій між$x$, $y$, і $z$моя перша думка полягала в тому, щоб просто застосувати 3D-матрицю обертання, але це працює лише для підматриці 3x3 в межах повної коваріаційної матриці 6x6. Чи потрібно застосовувати однакове обертання до всіх чотирьох блоків?

Коваріація визначається як

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

де, у вашому випадку, $X \in \mathbb{R}^6$ - ваш стан і $C$ - матриця коваріації, яку ви вже маєте.

Для трансформованого стану $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}$

Як застереження, будьте обережні з кутами Ейлера. Вони є звичайними неінтуїтивними у своїй поведінці, тому ви, можливо, не зможете просто повернути їх тією ж матрицею обертання, яку використовуєте для позиції. Пам’ятайте, що вони зазвичай визначаються (у світі робототехніки) з точки зору локальної системи координат, тоді як положення зазвичай визначаються з точки зору глобальної системи координат. Хоча вгорі голови я не можу пригадати, чи потрібно їм спеціальне лікування.

Бібліотека MRPT може зробити це для вас. Вам потрібно використовувати a CPose3DPDFGaussianдля представлення пози і коваріації, а потім використовувати +оператора.

Під кришкою вона представляє вашу коваріацію 6DOF як базову коваріацію кватерніона 7DOF, де математика більш проста.

Дуже інтуїтивне пояснення з геометричною інтерпретацією коваріації та її розкладання.

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