Determine variables for angle control from kinematics (RKI)

Dependencies:   Matrix