The trouble with shooting from the hip is when you shoot your own
foot! As I think about it now, I realize that my earlier suggestion
isn't the right way to go about it. My previous post described a type
of blending operation that would really only be appropriate for a
waveform that ought to be symmetric in time about the beginning/end of
the cycle. Each index of the smoothed data is based on combining raw
data that is equidistant in time from the beginning/end. That doesn't
seem like what you really needed after all.
In fact, giving more thought to your actual robotics app, it
probably wasn't such a good idea for me to think only about position.
Your derivatives are going to matter too.
Generally, the more I think about it, the more I wonder if part of
the problem is a matter of "getting painted into a corner" by trying
to work with exactly one cycle of data. There's an implicit
assumption that one can capture one cycle, then replay it repetitively
to produce continuous motion. But if your beginning and ending data
don't match up, then perhaps your cycle isn't really representative
after all. Maybe the first step or two that a person takes when
starting to walk is less typical of his/her gait than the later steps,
and that's why the endpoints don't match.
Is there an option to capture several cycles of input motion data?
Perhaps by ignoring the first cycle or two, and median-averaging the
next 3-5, you might get raw data that's more representative and that
will hopefully make for a smaller discontinuity.
<b>Smoothing idea v2.0</b> (with lots of hand-waving and no details or
guarantees):
Find the best-fit cubic over the last 15 points plus the first 5
points. Find another over the last 5 points plus the first 15 points.
These two cubic equations now give you 2 distinct predicted values for
the 10 points nearest to the transition. One prediction is biased
more by the behavior at the end of the cycle, the other by the
behavior at the beginning.
You could now replace those 10 points with a ramped weighted
average of the two predictions. By using a cubic, your motion
derivatives should be reasonable. For example, acceleration is still
allowed to vary (linearly) over that interval; you aren't artificially
constraining yourself to constant or zero accel.
I guess you might also want to blend from actual raw data to
predicted data as well -- perhaps that could be a separate weighted
average. Something like this:
..
{raw data region}
start blend from raw to end-biased cubic
{blend region}
end blend from raw to end-biased cubic
start blend from end-biased to start-biased cubic
{blend region}
end blend from end-biased to start-biased cubic
start blend from start-biased cubic to raw
{blend region}
end blend from start-biased cubic to raw
{raw region}
..
Implementation is left as an exercise for the reader...
