Kinematic Filtering for human and robot Trajectories

Kinematic Filtering computes the smoothest trajectory representing the noisy input trajectory

Low-pass filtering especially adequate for signals / trajectories representing smooth natural behaviors such as hand movements and handwriting trajectories.
Basic usage:
[x, y, ... ]=filter_JA(trj_ns);
Unlike FIR and IIR filtering, kinematic filtering allows to define any desired boundary conditions for the output trajectory.

The output is guaranteed to minimize the error with respect to the inputs and those constraints.

===== Algorithm [2] ========
Solving boundary value problem for the Euler-Lagrange Equations of the jerk-accuracy functional.
[x, y, ... ]=filter_JA(trj_ns);
filter_JA computes the smoothest representation [1] of the noisy trajectory trj_ns ([Nxd]). For d=2, the output [x,y] is the Cartesian representation of the trajectory and its five derivatives.

Imposing boundary conditions:
[x, y]=filter_JA(trj,lambda,tt,endpoints,vel,acc,direction,method)

The variable lambda is a Lagrange multiplier. For lambda=0 the output trajectory is based on the minimum jerk model [1], ie ignoring the noisy input and generating the smoothest trajectory wrt to the boundary conditions.
For lambda >> 0 the output trajectory is equal to the noisy input.

==== Citation ======================
If you use this code, please cite this paper:
Y. Meirovitch, D. Bennequin and T. Flash "Geometrical Invariance and Smoothness Maximization for Task-Space Movement Generation", 2016.
IEEE-Transaction on Robotics, 2016.

==== Examples ====================

Example 1
Adaptively changing Lambda and boundary conditions
Run: demoBoundaryCondition for a demo
demoBoundaryCondition()

Example 2
For first use, call
kinematicFiltering()

==== References ====================
[1] J. Neurosciece. Smoothness is defined based on Mean Squared Derivative model. For n=3 this is known as the minimum jerk model: Flash and Hogan, 1985.
[2] I-EEE T-RO. Geometrical Invariance and Smoothness Maximization for Task-Space Movement Generation, Meirovitch et al., 2016.