function [x,y,phi] = q2xi(q1,q2,q3,t)

%[L,rw,rth] = omni_params;
%L = .146;  % (meters) THIS VALUE HAS CHANGED!
L = .088; %.09; % <- New configuration has wheels at about 155mm from one another,
rw=.032;

phi = -(1/3)*(rw/L)*(q1+q2+q3); % holonomic constraint (on body rotation)
phi_av = .5*(phi(1:end-1)+phi(2:end)); % to match size of "dq1"

eta = [-120 0 120]*pi/180; % angle to each wheel, wrt COB (geometry)
dt = diff(t);
dq1 = diff(q1)./dt;
dq2 = diff(q2)./dt;
dq3 = diff(q3)./dt;
dx = (2/3)*rw*(sin(phi_av+eta(1)).*dq1 + ...
    sin(phi_av+eta(2)).*dq2 + ...
    sin(phi_av+eta(3)).*dq3);
dy = -(2/3)*rw*(cos(phi_av+eta(1)).*dq1 + ...
    cos(phi_av+eta(2)).*dq2 + ...
    cos(phi_av+eta(3)).*dq3);
x = 0*phi; y=0*phi; % pre-allocate size
x(2:end) = cumsum(dt.*dx);
y(2:end) = cumsum(dt.*dy);

%Jalt(3,:) = -(1/3)*(rw/L);
% Again, COB is 2/3 of the way from ICR to wheel contact point:
%Jalt(2,:) = (2/3)*(-rw)*cos(phi_b+eta);
%Jalt(1,:) = (2/3)*(rw)*sin(phi_b+eta);

