calcGCPJointKinematics.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the new kinematic curves from the experimental
% data and deviation values.
%
% jointKinematicsBSplines is a struct with 2 fields (position, velocity)
% as comes out of makeJointKinematicsBSplines(). The output
% struct has matching fields.
%
% (2D Array of double, struct, 2D Array of double) -> (struct)
% Calculate new joint kinematics curves from data and deviations curves


function [modeledJointPositions, modeledJointVelocities] = ...
    calcGCPJointKinematics(experimentalJointPositions, ...
    jointKinematicsBSplines, deviationNodes)
nodes = jointKinematicsBSplines.position \ experimentalJointPositions';
fittedNodes = nodes .* deviationNodes;

modeledJointPositions = jointKinematicsBSplines.position * fittedNodes;
modeledJointVelocities = jointKinematicsBSplines.velocity * fittedNodes;
modeledJointPositions = modeledJointPositions';
modeledJointVelocities = modeledJointVelocities';
end