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

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

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

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