Finished: getPositon and getVelocity. Fixed/finished findAccel: Did summation with manual loops instead of Armadillo functions.