This code built. Also seperated the calibration matlab code from the code calculating movement.