Algorithm for Stepping Through Latitude, Longitude and Height with a Heading

Question

I have a function which reads in latitude, longitude, height, heading and step size. My function should calculate the latitude, longitude and height of the position one step size away in the direction of the heading.

At present my Algorithm is as follows:

1. Convert Latitude, Longitude and Height into Earth-Centred Earth-Fixed Coordinates.
2. Convert ECEF vector into East-North-Up coordinates.
3. Add step size to northward component of the ENU vector.
4. Rotate the ENU vector by the heading, about the Up axis.
5. Convert ENU vector back to ECEF coordinates.
6. Convert ECEF vector back to Latitude, Longitude and Height.

This works so long as the heading is 0 degrees. When the heading increases I seem to be getting changes in Latitude, Longitude and Height that are greater than should be possible with the step size.

The individual functions for the coordinate conversions and rotations are all well tested functions, so I think the source of my error is my algorithm. Is there anything Mathematically wrong with what I've done so far?

Answer 1

The solution was simpler than I had realised. There was no need for my rotation matrix.

I was adding on 1 stepsize in the north direction, this is a special case for when the heading is zero.

I changed this to add on stepsize*cos(heading) in the North direction, and stepsize*sin(heading) in the East direction, and removed step 4 from my algorithm, and it worked.