Jak zmienić kowariancję?

11

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$

kalman-filter

— TheWumpus
źródło

8

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.

— ryan0270
źródło

Dzięki. W tym przypadku jednak wynosi 3x3, a to 6x6. Wydaje mi się, że część mojego problemu polega na tym, że nie jestem pewien, w jaki sposób wpłynęłoby na kowariancję między osiami liniowymi a obrotem (a nawet na kowariancję samych kątów Eulera), tj. Jak powinienem zwiększyć , aby było 6x6.

R

$R$

C

$C$

R

$R$

R

$R$

— TheWumpus

1

R

$R$ jest po prostu dowolną transformacją afiniczną. W twoim przypadku zarówno lewy górny blok 3x3, jak i prawy dolny blok 3x3 są macierzami obrotu (jeśli założymy, że kąty Eulera można obracać tak samo ... patrz zastrzeżenie w odpowiedzi). Bloki po przekątnej są zerami.

— ryan0270

1

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.

— brice rebsamen
źródło

Korzystne byłoby pokazanie matematyki oraz biblioteki, która robi to za Ciebie.

— chutsu

0

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

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

— Nitish Gupta
źródło

Witaj w Robotyka! Dziękuję za odpowiedź, ale wolimy, aby odpowiedzi były samodzielne tam, gdzie to możliwe. Łącza mają tendencję do gnicia, więc odpowiedzi oparte na łączu mogą stać się bezużyteczne, jeśli łącze do treści zniknie. Jeśli dodasz więcej kontekstu z linku, bardziej prawdopodobne jest, że inni uznają twoją odpowiedź za przydatną.

— mactro