getStateDerivatives.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the joint velocities, accelerations, and jerk
% based of the experimental joint angles using 5th degree GCV splines.
%
% (struct) -> (struct)
% Returns state derivatives


function inputs = getStateDerivatives(inputs)
points = length(inputs.experimentalTime);
interval = inputs.experimentalTime(2) - inputs.experimentalTime(1);
[N, Np, Npp] = BSplineMatrices(5, 10, points, interval);
Nodes = N\inputs.experimentalJointAngles;
inputs.experimentalJointVelocities = Np * Nodes;
inputs.experimentalJointAccelerations = Npp * Nodes;
inputs.experimentalJointJerks = calcBSplineDerivative( ...
    inputs.experimentalTime, inputs.experimentalJointAccelerations, ...
    2, 10);
end