Given series aircraft attitude readings produce a smoother version of flown route

Question

Problem statement: I'm working on a drone application where the goal is to produce "simon-says"-like functionality. The user manually pilots the drone on a route of their own choosing. My application tracks the drone during this user-flown route gathering the craft's yaw, pitch, roll, latitude, and longitude. The application then creates a series automated maneuvers the aircraft can follow to reproduce the same approximate flight route, but with any aberrations which arise from manual user control removed. The application of this is for videography: the end result should be a video flown according to the user's intentions, but much smoother.

Input data: data is being captured approximately 3 times per second. Here are four consecutive data points of a route where the craft is "yawing"/turning gradually in a constant curve to the left:

{
    gimbalPitch = 0;
    height = "3.1";
    index = 6;
    latitude = "37.77024559727658";
    longitude = "-122.4449859973015";
    pitch = "-14.5";
    roll = 0;
    timestamp = "1441401218.772954";
    yaw = "-1.4";
}
{
    gimbalPitch = 0;
    height = "3.1";
    index = 7;
    latitude = "37.77024812789138";
    longitude = "-122.4449860306775";
    pitch = "-13.7";
    roll = "0.1";
    timestamp = "1441401219.189793";
    yaw = "-3.7";
}
{
    gimbalPitch = 0;
    height = "3.1";
    index = 8;
    latitude = "37.77025291280704";
    longitude = "-122.4449861874498";
    pitch = "-13.3";
    roll = "0.2";
    timestamp = "1441401219.598018";
    yaw = "-7.4";
}
{
    gimbalPitch = 0;
    height = "3.1";
    index = 9;
    latitude = "37.77025778210473";
    longitude = "-122.4449864696067";
    pitch = "-13.1";
    roll = "0.1";
    timestamp = "1441401220.006745";
    yaw = "-10.6";
}

And so on..

Desired output: A series of {roll (craft's left/right tilt), pitch (craft's forward/back tilt), throttle (vertical speed), yaw (horizontal orientation)} commands that can be sent to the craft in real time which match the desired curve. Note that these values match the values captured in each data point of input.

Questions: I'm a bit at a loss here. I "don't know what I don't know" about what methods are at my disposal.

What is the right way to tackle this? A route might contain a S-curve maneuver where the craft first turns ("yaws") one way and then yaws the other way. So simply averaging the yaw deltas between data points won't work.

The term "inflection point" pops into my head. Is part of the challenge here going to be breaking a route into smaller pieces which can be described by simple averages of the deltas between points? How would I find the proper inflection points to use?

Perhaps I need to break the route into sub-maneuvers where each sub-maneuver can be described by a function? I could then use the sub-maneuver's function to derive a roll/pitch/throttle/yaw? What is this type of "function deriving" called?

Thanks for any help you guys can offer. I'm mostly needing a point in the right direction vs. being hand-held through a solution. Any insight is extremely appreciated!

Answer 1

One thing you might try is to first interpolate using splines, then smooth it out with a low-pass filter.

Answer 2

In my experience, this type of data is inherently noisy. Therefore, it has to be smoothed.

I have used cubic splines of various forms in the past. For this, I would recommend least-squares cubic splines, since you do not need the smoothed curves to pass through the original data points.

Many years ago I wrote a package (in fortran, of course) which did adaptive least squares cubic spline fitting. The knots were automatically adjusted. Sometimes it worked great, sometimes not so great. But, it worled.

A question: How soon after the data is captured to you need to generate the smoothed curve? It is harder if the new curve has to be generated immediately, as opposed to generating it and viewing it in a simulation so it can be tweaked.