6x6 Covariance Matrix Conversion from ENU to ECEF-CodePudding

The

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

Then from this paper:

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?

CodePudding user response：

There are two fixes to make:

Update your rotation matrix to have zeros in the off-diagonal blocks.

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.

CodePudding user response：

This is my first time trying a bounty. I think I have done what you are looking for.. So basically it seems that there may be an issue with the implementation of the createRotationMatrixV3 function or with the input ENU covariance matrix. I took a little bit of time to mess around with the sin and cos Math functions to try and do it manually.

Here is the code that I ended up with: https://codecatch.net/post/008da886-fa1a-4995-a024-f58d883e0504

Give that a shot and let me know if I need to refine my answer