Position Calculation using GPS Bricklet

[andycruce]

Does anyone have any experience converting the GPS Lat, Long, Alt to local Tangent plane positions.  I want to collect GPS data during aerobatic maneuvers and combine it with IMU data to get an accurate picture of the plane's 6dof positon during the maneuver.  Will eventually display the results to the pilot for self training purposes.  I am using the following equations:

 

a = equatorial radius = 6378137, e = eccentricity = .08181818

 

Re = a/squrt(1-e^2sin(lat)^2)  Where e^2 = e*e

 

Using these values I calculate the ECEF coordinates:

Xe = (Re+h)*cos(lat)*cos(long)

Ye = (Re+h)*cos(lat)*cos(long)

Ze = (Re*(1-e^2)+h)sin(lat)

 

I use these to calculate a base point for the tangent plane at my current location.  Xe0, Ye0, Ze0.  Then as I move I calculate the current Xe, Ye and Ze and subtract the two values to get the distance of the current point to the base point, Xedelta. Yedelta, Zedelta.  These are in still ECEF coordinates.

 

Once I have done that I transform the "delta" point using an ECEF to Tangent Plane transformation matrix

 

                          -sin(lat)cos(long)  -sin(lat)sin(long)  cos(lat)

      Transform Matrix = -sin(long)            cos(long)              0

                          -cos(lat)cos(long)    -cos(lat)sin(long)  -sin(lat)

 

  The results don't make a lot of sense.  After making drive around a circle in out community back to the starting point the x position is -19, the y position is 145 and the z position is -16 in meters.  These are all outside the GPS estimated EPE of bout 5m.  Does anyone have come code that performs these calculations or has any idea of what I am doing wrong.

 

Thanks for any help,

 

Andy Cruce