c# - Curve fitting points in 3D space

Question

Trying to find functions that will assist us to draw a 3D line through a series of points.

For each point we know: Date&Time, Latitude, Longitude, Altitude, Speed and Heading. Data might be recorded every 10 seconds and we would like to be able to guestimate the points in between and increase granularity to 1 second. Thus creating a virtual flight path in 3D space.

I have found a number of curve fitting algorithms that will approximate a line through a series of points but they do not guarantee that the points are intersected. They also do not take into account speed and heading to determine the most likely path taken by the object to reach the next point.

Answer 1

From a physics viewpoint:

You have to assume something about the acceleration in your intermediate points to get the interpolation.

If your physical system is relatively well-behaved (as a car or a plane), as opposed to for example a bouncing ball, you may go ahead supposing an acceleration varying linearly with time between your points.

The vector equation for a constant varying accelerated movement is:

 x''[t] = a t + b

where all magnitudes except t are vectors.

For each segment you already know v(t=t0) x(t=t0) tfinal and x(tfinal) v(tfinal)

By solving the differential equation you get:

Eq 1:
x[t_] := (3 b t^2 Tf + a t^3 Tf - 3 b t Tf^2 - a t Tf^3 - 6 t X0 + 6 Tf X0 + 6 t Xf)/(6 Tf)  

And imposing the initial and final contraints for position and velocity you get:

Eqs 2:

 a -> (6 (Tf^2 V0 - 2 T0 Tf Vf + Tf^2 Vf - 2 T0 X0 + 2 Tf X0 + 
        2 T0 Xf - 2 Tf Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))

 b -> (2 (-2 Tf^3 V0 + 3 T0^2 Tf Vf - Tf^3 Vf + 3 T0^2 X0 - 
        3 Tf^2 X0 - 3 T0^2 Xf + 3 Tf^2 Xf))/(Tf^2 (3 T0^2 - 4 T0 Tf + Tf^2))}}

So inserting the values for eqs 2 into eq 1 you get the temporal interpolation for your points, based on the initial and final position and velocities.

HTH!

Edit

A few examples with abrupt velocity change in two dimensions (in 3D is exactly the same). If the initial and final speeds are similar, you'll get "straighter" paths.

Suppose:

X0 = {0, 0}; Xf = {1, 1};
T0 = 0;      Tf = 1;  

If

V0 = {0, 1}; Vf = {-1, 3};

V0 = {0, 1}; Vf = {-1, 5};

V0 = {0, 1}; Vf = {1, 3};

Here is an animation where you may see the speed changing from V0 = {0, 1} to Vf = {1, 5}:

Here you may see an accelerating body in 3D with positions taken at equal intervals:

Edit

A full problem:

For convenience, I'll work in Cartesian coordinates. If you want to convert from lat/log/alt to Cartesian just do:

x = rho sin(theta) cos(phi)
y = rho sin(theta) sin(phi)
z = rho cos(theta)

Where phi is the longitude, theta is the latitude, and rho is your altitude plus the radius of the Earth.

So suppose we start our segment at:

 t=0 with coordinates (0,0,0) and velocity (1,0,0)

and end at

 t=10 with coordinates (10,10,10) and velocity (0,0,1)  

I clearly made a change in the origin of coordinates to set the origin at my start point. That is just for getting nice round numbers ...

So we replace those numbers in the formulas for a and b and get:

a = {-(3/50), -(3/25), -(3/50)}  b = {1/5, 3/5, 2/5}  

With those we go to eq 1, and the position of the object is given by:

p[t] = {1/60 (60 t + 6 t^2 - (3 t^3)/5), 
        1/60 (18 t^2 - (6 t^3)/5), 
        1/60 (12 t^2 - (3 t^3)/5)}

And that is it. You get the position from 1 to 10 secs replacing t by its valus in the equation above.
The animation runs:

Edit 2

If you don't want to mess with the vertical acceleration (perhaps because your "speedometer" doesn't read it), you could just assign a constant speed to the z axis (there is a very minor error for considering it parallel to the Rho axis), equal to (Zfinal - Zinit)/(Tf-T0), and then solve the problem in the plane forgetting the altitude.