共分散を回転させる方法は？

私はEKFに取り組んでおり、共分散行列の座標フレーム変換について質問があります。レッツは、私はいくつかの測定を得ると言う対応する6×6共分散行列C。この測定値とCは、座標フレームG 1で示されます。測定値を別の座標フレームG 2に変換する必要があります$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$。測定自体の変換は簡単ですが、その共分散も変換する必要がありますよね？とG 2の間の平行移動は関係ないはずですが、それでも回転させる必要があります。私が正しい場合、どうすればいいですか？x、y、zの間の共分散について、最初に考えたのは3D回転行列を単に適用することでしたが、これは完全な6x6共分散行列内の3x3サブ行列に対してのみ機能します。4つのブロックすべてに同じ回転を適用する必要がありますか？$G_1$$G_2$$x$$y$$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ライブラリは、あなたのためにこれを行うことができます。CPose3DPDFGaussianポーズと共分散を表すにはa を使用し、次に+演算子を使用する必要があります。

内部的には、6DOF共分散を7DOFクォータニオンベース共分散として表します。ここでは、計算がより簡単です。

共分散とその分解の幾何学的解釈による非常に直感的な説明。

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