Projection Problem in Kalman Filter for Navigation

Question

I am currently working on a simple and small Kalman-Filter for GPS-Navigation. I am getting from my GPS-Sensor the current location, course angle and speed. So the Kalman-Filter should fuse the current measurement and the linear movement beginning from the previous location assuming constant speed and course-angle.

My problem is to select a useful space where the Kalman-Filter is able to perform in a good way.

Local coordinate system approach:

If I choose a local coordinate system (north [meter], east [meter]) with the previous location at origin I will be able to predict the new location easily but how to convert the new measurement (latitude/longitude) into my local coordinate system using the wgs-84 ellipsoid? and how to convert my new predicted in my local coordinate system to latitude/longitude also using the wgs-84 ellipsoid?

So I need two functions:

f:=(lat_ref, lng_ref, lat, lng) -> (x,y)

g:=(lat_ref, lng_ref, x, y) -> (lat, lng) (this could by also done using Vincenty)

Global coordinate system approach:

I found the Vincenty-Algorithm which calculates the new location from a reference location, distance and course-angle on any ellipsoid. This algorithm works fine but I dont see how to use this algorithm inside a kalman-filter which works in a global coordinate system.

Are there any ideas or suggestions how to solve one of my problems?

Answer 1

I've worked through many scientific papers and found the answer on wikipedia

The main trick is to convert latitude/longitude/height (llh) to earth-centered-earth-fixed (ecef) and then to my local coordinate system earth/north/up (enu) and vice versa.

I found some other interesting links to this topic.

• direct and faster conversion from llh to enu in matlab
• ecef2llh and llh2ecef in matlab