Jak zmienić kowariancję?

Pracuję nad EKF i mam pytanie dotyczące konwersji ramki współrzędnych dla macierzy kowariancji. Załóżmy, że trochę pomiar z odpowiednimi 6x6 macierzy kowariancji . Ten pomiar i podano w niektórych ramkach współrzędnych . Muszę przekształcić pomiar na inną ramkę współrzędnych, . Przekształcenie samego pomiaru jest trywialne, ale musiałbym również przekształcić jego kowariancję, prawda? Translacja pomiędzy i powinny być bez znaczenia, ale nadal musiałby go obrócić. Jeśli mam rację, jak mam to zrobić? Dla kowariancji między ,$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$, i , moją pierwszą myślą było po prostu zastosowanie macierzy obrotu 3D, ale działa to tylko w przypadku submatrixy 3x3 w pełnej macierzy kowariancji 6x6. Czy muszę zastosować ten sam obrót do wszystkich czterech bloków?$z$

Kowariancja jest zdefiniowana jako

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

gdzie w twoim przypadku jest wektorem stanu, a jest macierzą kowariancji, którą już masz.$X \in \mathbb{R}^6$$C$

Dla stanu przekształconego , z w twoim przypadku, staje się$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}$

Jako ostrzeżenie, uważaj na kąty Eulera. Są one zwykle nieintuicyjne w swoim zachowaniu, więc możesz nie być w stanie po prostu obrócić ich za pomocą tej samej macierzy obrotu, której używasz dla pozycji. Pamiętaj, że są one zazwyczaj definiowane (w świecie robotyki) w kategoriach lokalnego układu współrzędnych, podczas gdy położenie jest zwykle definiowane w kategoriach globalnego układu współrzędnych. Jednak z głowy nie pamiętam, czy wymagają specjalnego traktowania.

Biblioteka MRPT może zrobić to za Ciebie. Musisz użyć a, CPose3DPDFGaussianaby przedstawić swoją pozę i kowariancję, a następnie użyj +operatora.

Pod maską przedstawia kowariancję 6DOF jako kowariancję bazy czwartorzędu 7DOF, gdzie matematyka jest bardziej prosta.

Bardzo intuicyjne wyjaśnienie z interpretacją geometryczną kowariancji i jej rozkładu.

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