從 ENU 到 ECEF 的 6x6 協方差矩陣轉換

Question

我目前正在使用 RADAR 輸出一個 6x6 協方差矩陣，每個軌道的格式如下：

列1	列2	列3	列4	列5	列6
(EP)(EP)	(EP)(NP)	(EP)(上)	(EP)(EV)	(EP)(內華達州)	(EP)(紫外線)
(NP)(EP)	(NP)(NP)	(NP)(向上)	(NP)(EV)	(NP)(NV)	(NP)(紫外線)
(上)(EP)	(向上)(NP)	（上）（上）	(向上)(EV)	(上)(內華達)	(向上)(紫外線)
(EV)(EP)	(EV)(NP)	(EV)(向上)	(EV)(EV)	(EV)(內華達州)	(EV)(紫外線)
(內華達州)(EP)	(NV)(NP)	(內華達)(上)	(內華達)(電動汽車)	(內華達州)(內華達州)	(夜視)(紫外)
(紫外線)(EP)	(紫外線)(納米粒子)	(紫外線)(向上)	(紫外線)(EV)	(紫外線)(夜視)	(紫外線)(紫外線)

其中， EP = 東位置，NP = 北位置，UP = 上 Position，EV = 東速度，NV = 北速度，UV = 上速度。 讓 [EP][EP]=Cov(EP,EP)=Var(EP) 等等

在我的研究中，我發現了這個： https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates

這正好給出了我需要的 3x3 ENU 到 ECEF position協方差轉換。 我的第一個假設是我會像這樣簡單地復制旋轉矩陣(R) ：

其中lambda = 雷達的經度和 phi = 雷達的緯度。

然后來自這篇論文： https://www.ngs.noaa.gov/CORS/Articles/SolerChin1985.pdf

Summation WGS72 實際上只是我收到的 ENU 6x6 協方差矩陣。

在 Java 中實施，我得到以下信息：

    public static void enu2ecefCov(GMatrix ecefCov, GMatrix enuCov, LLA refLLA) {
        GMatrix R = new GMatrix(6, 6);
        GMatrix Rt = new GMatrix(6, 6);
        GMatrix tmp = new GMatrix(6, 6);

        createRotationMatrixV3(R, refLLA);

        Rt.transpose(R);
        tmp.mul(enuCov, R);
        ecefCov.mul(Rt, tmp);

    }

但是，我輸出的矩陣看起來不正確，因為我多次看到相同的值，而原始矩陣除了對稱的對應塊外根本沒有相同的值。 我這樣做正確嗎？

Answer 1

有兩個修復：

1. 更新您的旋轉矩陣，使非對角線塊中有零。
2. 計算 C' = RCR ^T ，其中 C' 是 ECEF 協方差矩陣，C 是 ENU 協方差矩陣，R 是您的位置-速度組合從 ENU 到 ECEF 的旋轉矩陣。

修復 #1 的推理

position 和速度就像要旋轉的兩個不同數據點，旋轉矩陣應反映它們的獨立性。 為此，請使用 ENU -> ECEF 的原始 3x3 旋轉矩陣：

R = | -sin(λ), -cos(λ)*sin(φ), cos(λ)*cos(φ) | 
    |  cos(λ), -sin(λ)*sin(φ), sin(λ)*cos(φ) |
    |       0,         cos(φ),        sin(φ) |

並用它來構建一個 6x6 旋轉矩陣（讓'0'= 0s的3x3矩陣）

R = |R 0|
    |0 R|

旋轉的符號計算：

R = | R11 R12 R13   0   0   0 |
    | R21 R22 R23   0   0   0 |
    | R31 R32 R33   0   0   0 | 
    |   0   0   0 R11 R12 R13 |
    |   0   0   0 R21 R22 R23 |
    |   0   0   0 R31 R32 R33 |

x = | e1 |
    | n1 |
    | u1 |
    | e2 |
    | n2 |
    | u2 |

x' = Rx = | R11*e1 + R12*n1 + R13*u1 +   0*e2 +   0*n2 +   0*u2 |
          | R21*e1 + R22*n1 + R23*u1 +   0*e2 +   0*n2 +   0*u2 |
          | R31*e1 + R32*n1 + R33*u1 +   0*e2 +   0*n2 +   0*u2 |
          |   0*e1 +   0*n1 +   0*u1 + R11*e2 + R12*n2 + R13*u2 |
          |   0*e1 +   0*n1 +   0*u1 + R21*e2 + R22*n2 + R23*u2 |
          |   0*e1 +   0*n1 +   0*u1 + R31*e2 + R32*n2 + R33*u2 |

結果是一個 6x1 向量，基本上是一個獨立旋轉的 (e,n,u) 在另一個之上的堆疊。

修復 #2 的推理

let X = | EP_1  EP_2 .. EP_n|  (i.e. a set of measurements)
        | NP_1  NP_2 .. NP_n|
        | UP_1  UP_2 .. UP_n|
        | EV_1  EV_2 .. EV_n|
        | NV_1  NV_2 .. NV_n|
        | UV_1  UV_2 .. UV_n|

let Xt = tranpose(X)
let C = variance-covariance matrix

C = E(XXt) - E(X)E(Xt)

let X' = RX = rotated measurements
let C' = variance-covariance matrix of rotated measurements

C' = E(X'X't) - E(X')E(X't)
   = E(RXXtRt) - E(RX)E(XtRt)     remember (AB)t = BtAt
   = RE(XXt)Rt - RE(X)E(Xt)Rt
   = R(E(XXt) - E(X)E(Xt))Rt
C' = RCRt

這表明當一組測量旋轉時，其原始的方差-協方差矩陣可以變換到新的坐標系中作為旋轉矩陣的function。

Answer 2

這是我第一次嘗試賞金。 我想我已經完成了您要查找的內容。所以基本上， createRotationMatrixV3 function 的實施或輸入 ENU 協方差矩陣似乎可能存在問題。 我花了一點時間來弄亂 sin 和 cos 數學函數，嘗試手動完成。

這是我最終得到的代碼：https://codecatch.net/post/008da886-fa1a-4995-a024-f58d883e0504

試一試，讓我知道是否需要完善我的答案

Answer 3

實現看起來是正確的，但協方差矩陣在從 ENU 轉換為 ECEF 后可能看起來不一樣，因為它取決於參考系的方向和測量誤差的性質。

通常，協方差矩陣的值表示隨機向量的不同元素之間的協方差。 ECEF 框架中轉換后的協方差矩陣應反映 ECEF position 和速度分量之間的協方差。

如果您遇到任何問題，調試代碼並在轉換過程中檢查矩陣的值可能會有所幫助，以查看它們是否符合您的預期。 此外，您可以將生成的協方差矩陣與類似測量的協方差矩陣進行比較，以查看結果是否一致。

我也在學習這個。