Added option to trust roll and pitch from state estimate, but still estimate yaw and xyz from vicon data. Reverted quaternion parameterization in motionCaptureJointCalibration for this purpose.
fmincon instead of weirdly parameterized fminunc
fixed test