6x6 Covariance Matrix Conversion from ENU to ECEF

Question

I am currently working with a RADAR that outputs a 6x6 covariance matrix with every track in the following format:

Col1	Col2	Col3	Col4	Col5	Col6
(EP)(EP)	(EP)(NP)	(EP)(UP)	(EP)(EV)	(EP)(NV)	(EP)(UV)
(NP)(EP)	(NP)(NP)	(NP)(UP)	(NP)(EV)	(NP)(NV)	(NP)(UV)
(UP)(EP)	(UP)(NP)	(UP)(UP)	(UP)(EV)	(UP)(NV)	(UP)(UV)
(EV)(EP)	(EV)(NP)	(EV)(UP)	(EV)(EV)	(EV)(NV)	(EV)(UV)
(NV)(EP)	(NV)(NP)	(NV)(UP)	(NV)(EV)	(NV)(NV)	(NV)(UV)
(UV)(EP)	(UV)(NP)	(UV)(UP)	(UV)(EV)	(UV)(NV)	(UV)(UV)

Where, EP = East Postion, NP = North Postion,UP = Up Position, EV = East Velocity, NV = North Velocity, and UV = UP Velocity. Let [EP][EP]=Cov(EP,EP)=Var(EP) and so on

In my research I have found this: https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates

This gives exactly what I need for a 3x3 ENU to ECEF position only covariance transformation. My first assumption is that I would simply duplicate the Rotational Matrix (R) like so:

Where lambda = longitude of the radar and phi = latitude of radar.

Then from this paper: https://www.ngs.noaa.gov/CORS/Articles/SolerChin1985.pdf

Where Summation WGS72 is actually just the ENU 6x6 covariance matrix I am receiving.

Implementing in Java I am getting the following:

    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);

    }

However, the matrix I am outputting doesn't look correct as I am seeing the same values multiple times whereas the original doesn't have the same values at all besides symmetric corresponding blocks. Am I doing this correctly?

Answer 1

There are two fixes to make:

1. Update your rotation matrix to have zeros in the off-diagonal blocks.
2. Calculate C' = RCR^T, where C' is the ECEF covariance matrix and C is ENU covariance matrix, and R is the rotation matrix from ENU to ECEF for your position-velocity combination.

Reasoning for Fix #1

The position and velocity are like two different data points to be rotated and the rotation matrix should reflect their independence. To do that, take your original 3x3 rotation matrix for ENU -> ECEF:

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

And use it to build a 6x6 rotation matrix (let '0' = 3x3 matrix of 0s)

R = |R 0|
    |0 R|

Symbolic calculation of a rotation:

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 |

The result is a 6x1 vector that is basically the stacking of one independently rotated (e,n,u) on top of another.

Reasoning for Fix #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

That shows that when a set of measurements is rotated, its original variance-covariance matrix can be transformed into the new coordinate system as a function of the rotation matrix.