starting to make the find_kinematics.m function better