Làm thế nào để xoay hiệp phương sai?

Tôi đang làm việc trên EKF và có một câu hỏi liên quan đến chuyển đổi khung tọa độ cho ma trận hiệp phương sai. Hãy nói rằng tôi nhận được một số đo tương ứng với 6x6 hiệp phương sai ma trận . Phép đo và này được đưa ra trong một số khung tọa độ . Tôi cần chuyển đổi phép đo sang khung tọa độ khác, . Chuyển đổi phép đo chính nó là tầm thường, nhưng tôi cũng sẽ cần phải chuyển đổi hiệp phương sai của nó, đúng không? Bản dịch giữa và không liên quan, nhưng tôi vẫn cần xoay nó. Nếu tôi đúng, tôi sẽ làm điều này như thế nào? Cho hiệp phương sai giữa ,$(x, y, z, roll, pitch, yaw)$$C$$C$$G_1$$G_2$$G_1$$G_2$$x$$y$và , suy nghĩ đầu tiên của tôi chỉ đơn giản là áp dụng ma trận xoay 3D, nhưng nó chỉ hoạt động với một hàm con 3x3 trong ma trận hiệp phương sai 6x6 đầy đủ. Tôi có cần áp dụng cùng một vòng quay cho cả bốn khối không?$z$

Hiệp phương sai được định nghĩa là

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

trong trường hợp của bạn, là vectơ trạng thái của bạn và là ma trận hiệp phương sai mà bạn đã có.$X \in \mathbb{R}^6$$C$

Đối với trạng thái được chuyển đổi , với trong trường hợp của bạn, điều này trở thành$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}$

Như một lời cảnh báo, hãy cẩn thận với các góc Euler. Chúng thường không trực quan trong hành vi của chúng, do đó bạn có thể không thể xoay chúng với cùng một ma trận xoay mà bạn sử dụng cho vị trí. Hãy nhớ rằng chúng thường được định nghĩa (trong thế giới robot) về mặt hệ tọa độ cục bộ trong khi vị trí thường được xác định theo hệ thống tọa độ toàn cầu. Mặc dù vậy, tôi không thể nhớ họ có cần điều trị đặc biệt không.

Các thư viện MRPT thể làm điều này cho bạn. Bạn cần sử dụng a CPose3DPDFGaussianđể thể hiện tư thế và hiệp phương sai sau đó sử dụng +toán tử.

Dưới mui xe, nó đại diện cho hiệp phương sai 6DOF của bạn như một hiệp phương sai cơ sở bậc bốn 7DOF, trong đó toán học đơn giản hơn.

Giải thích rất trực quan với giải thích hình học cho hiệp phương sai và phân rã của nó.

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