Projection Problem in Kalman Filter for Navigation

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 开发者_如何学Con 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?

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

Check out FWTOOLS and in particular cs2cs and proj... They can handle the coord system transformations from projected to geographic coordinate systems. It's important to remember to consider any datum shifts.