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...