从 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 和速度分量之间的协方差。

如果您遇到任何问题，调试代码并在转换过程中检查矩阵的值可能会有所帮助，以查看它们是否符合您的预期。 此外，您可以将生成的协方差矩阵与类似测量的协方差矩阵进行比较，以查看结果是否一致。

我也在学习这个。