Как повернуть ковариацию?

11

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

kalman-filter

— TheWumpus
источник

8

Ковариация определяется как

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

В качестве предостережения, будьте осторожны с углами Эйлера. Они обычно не интуитивны в своем поведении, поэтому вы не сможете просто повернуть их с той же матрицей вращения, которую используете для положения. Помните, что они обычно определяются (в мире робототехники) в терминах локальной системы координат, тогда как положение обычно определяется в терминах глобальной системы координат. Впрочем, я не помню, нуждаются ли они в особом уходе.

— ryan0270
источник

Спасибо. В этом случае, однако,

3x3 и

6x6. Я полагаю, что часть моей проблемы заключается в том, что я не уверен, как

повлияет на ковариацию между линейными осями и вращением (или даже на ковариацию самих углов Эйлера), т. Е. Как я должен увеличить

так, чтобы она составляла 6x6.

R

$R$

C

$C$

R

$R$

R

$R$

— TheWumpus

1

R

$R$

1

Библиотека MRPT может сделать это для вас. Вы должны использовать a CPose3DPDFGaussianдля представления вашей позы и ковариации, а затем использовать +оператор.

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

— Брайс Ребзамен
источник

Было бы полезно показать математику, а также библиотеку, которая делает это для вас.

— Чуцу

0

Очень интуитивное объяснение с геометрической интерпретацией для ковариации и ее разложения.

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

— Нитиш Гупта
источник

Привет и добро пожаловать в робототехнику! Спасибо за ваш ответ, но мы предпочитаем, чтобы ответы были автономными, где это возможно. Ссылки имеют тенденцию гнить, поэтому ответы, основанные на ссылке, могут оказаться бесполезными, если ссылка на контент исчезнет. Если вы добавите больше контекста по ссылке, более вероятно, что люди найдут ваш ответ полезным.

— 17