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.

Answer 2

What you're asking is a general interpolation problem. My guess is your actual problem isn't due to the curve-fitting algorithm being used, but rather your application of it to all discrete values recorded by the system instead of the relevant set of values.

Let's decompose your problem. You're currently drawing a point in spherically-mapped 3D space, adjusting for linear and curved paths. If we discretize the operations performed by an object with six degrees of freedom (roll, pitch, and yaw), the only operations you're particularly interested in are linear paths and curved paths accounting for pitch and yaw in any direction. Accounting for acceleration and deceleration also possible given understanding of basic physics.

Dealing with the spherical mapping is easy. Simply unwrap your points relative to their position on a plane, adjusting for latitude, longitude, and altitude. This should allow you to flatten data that would otherwise exist along a curved path, though this may not strictly be necessary for the solutions to your problem (see below).

Linear interpolation is easy. Given an arbitrary number of points backwards in time that fit a line within n error as determined by your system,* construct the line and compute the distance in time between each point. From here, attempt to fit the time points to one of two cases: constant velocity or constant acceleration.

Curve interpolation is a little more difficult, but still plausible. For cases of pitch, yaw, or combined pitch+yaw, construct a plane containing an arbitrary number of points backwards in time, within m error for curved readouts from your system.* From these data, construct a planar curve and once again account for constant velocity or acceleration along the curve.

You can do better than this by attempting to predict the expected operations of a plane in flight as part of a decision tree or neural network relative to the flight path. I'll leave that as an exercise for the reader.

Best of luck designing your system.

--

* Both error readouts are expected to be from GPS data, given the description of the problem. Accounting and adjusting for errors in these data is a separate interesting problem.