Bagaimana cara memutar kovarians?

Saya sedang mengerjakan EKF dan memiliki pertanyaan tentang konversi bingkai koordinat untuk matriks kovarians. Katakanlah saya mendapatkan beberapa pengukuran dengan sesuai 6x6 kovarians matriks . Pengukuran dan ini diberikan dalam beberapa kerangka koordinat . Saya perlu mengubah pengukuran ke bingkai koordinat lain, . Mengubah pengukuran itu sendiri sepele, tetapi saya juga perlu mengubah kovariansnya, benar? Terjemahan antara dan seharusnya tidak relevan, tetapi saya masih perlu memutarnya. Jika saya benar, bagaimana saya melakukan ini? Untuk kovarian antara ,$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$, dan , pikiran pertama saya hanyalah menerapkan matriks rotasi 3D, tetapi itu hanya berfungsi untuk submatrix 3x3 dalam matriks kovarian 6x6 penuh. Apakah saya perlu menerapkan rotasi yang sama untuk keempat blok?$z$

Kovarian didefinisikan sebagai

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

di mana, dalam kasus Anda, adalah vektor keadaan Anda dan adalah matriks kovarians yang sudah Anda miliki.$X \in \mathbb{R}^6$$C$

Untuk keadaan transformasi , dengan dalam kasus Anda, ini menjadi$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}$

Sebagai peringatan, berhati-hatilah dengan sudut Euler. Itu biasanya tidak intuitif dalam perilaku mereka sehingga Anda mungkin tidak dapat hanya memutar mereka dengan matriks rotasi yang sama yang Anda gunakan untuk posisi. Ingat bahwa mereka biasanya didefinisikan (dalam dunia robotika) dalam hal sistem koordinat lokal sedangkan posisi biasanya didefinisikan dalam hal sistem koordinat global. Tapi di atas kepala saya, saya tidak ingat apakah mereka membutuhkan perawatan khusus.

The perpustakaan MRPT dapat melakukan ini untuk Anda. Anda harus menggunakan a CPose3DPDFGaussianuntuk mewakili pose dan kovarian Anda, lalu gunakan +operator.

Di bawah tenda itu mewakili kovarians 6DOF Anda sebagai kovarian dasar angka 7DOF, di mana matematika lebih mudah.

Penjelasan yang sangat intuitif dengan interpretasi geometris untuk kovarians dan penguraiannya.

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