%
% UFORM = itou(IFORM).  This function converts from INTERNAL form, given by
% a 3x3 matrix which represents position and orientation, to USER form,
% given by vector [x y theta]'.  Position is in meters, with angle in
% degrees.
%

function uform = itou(iform)

DEG = 180/pi;   % Conversion factor from radians -> degrees

c_theta = iform(1,1);
s_theta = iform(2,1);

theta = atan2(s_theta,c_theta);     % Compute the angle using atan2, which
x = iform(1,3);                     % will return -pi<theta<pi, and get
y = iform(2,3);                     % the last column for position

uform = [x y theta*DEG]';   % Assemble the result vector, with angle in degrees.