วิธีการหมุนความแปรปรวนร่วม?

ฉันกำลังทำงานกับ EKF และมีคำถามเกี่ยวกับการแปลงเฟรมพิกัดสำหรับเมทริกซ์ความแปรปรวนร่วม สมมติว่าฉันได้รับการวัด$(x, y, z, roll, pitch, yaw)$ ด้วยเมทริกซ์ความแปรปรวนร่วม 6x6 ที่สอดคล้องกัน $C$. วัดนี้และ$C$ จะได้รับในกรอบพิกัดบางส่วน $G_1$. ฉันต้องเปลี่ยนการวัดเป็นเฟรมพิกัดอื่น$G_2$. การแปลงค่าการวัดนั้นเป็นเรื่องเล็กน้อย แต่ฉันก็ต้องเปลี่ยนความแปรปรวนร่วมของมันด้วยใช่ไหม? การแปลระหว่าง$G_1$ และ $G_2$ควรไม่เกี่ยวข้อง แต่ฉันยังคงต้องหมุน หากฉันถูกต้องฉันจะทำอย่างไร สำหรับพันธมิตรระหว่าง$x$, $y$และ $z$ความคิดแรกของฉันคือการใช้เมทริกซ์การหมุนสามมิติเพียงอย่างเดียว แต่ใช้ได้กับเมทริกซ์ย่อย 3x3 ภายในเมทริกซ์ความแปรปรวนร่วม 6x6 เต็มรูปแบบ ฉันจำเป็นต้องใช้การหมุนเดียวกันกับบล็อกทั้งสี่หรือไม่

ความแปรปรวนร่วมหมายถึง

$\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 ของคุณในการแปรปรวนพื้นฐาน 7DF quaternion ซึ่งคณิตศาสตร์นั้นตรงไปตรงมามากขึ้น

คำอธิบายที่เข้าใจง่ายมากพร้อมการตีความทางเรขาคณิตเพื่อความแปรปรวนร่วมและการสลายตัว

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