function paintbody_update(plt, rplt, tplt, yplt, pos, phi,the,psi,tout,xout,rp)

R = [cos(psi)*cos(the)   cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi)   cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);
     sin(psi)*cos(the)   sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi)   sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(psi);
     -sin(the)           cos(the)*sin(phi)                              cos(the)*cos(phi)];

% initialize frame 
Fo =[1  0  0;
    -1 0  0;
    0  1  0;
    0 -1  0;
    0  0  0.5]';

F = R*Fo + pos';
 
%fore
fx  = [pos(1)  F(1,1)];
fy  = [pos(2)  F(2,1)];
fz  = [pos(3)  F(3,1)];
%aft
ax  = [pos(1)  F(1,2)];
ay  = [pos(2)  F(2,2)];
az  = [pos(3)  F(3,2)];
%port
px  = [pos(1)  F(1,3)];
py  = [pos(2)  F(2,3)];
pz  = [pos(3)  F(3,3)];
%starboard
sx  = [pos(1)  F(1,4)];
sy  = [pos(2)  F(2,4)];
sz  = [pos(3)  F(3,4)];

%body Z
%zx  = [pos(1)  F(1,5)];
%zy  = [pos(2)  F(2,5)];
%zz  = [pos(3)  F(3,5)];
zax = cross(F(:,1),F(:,3))/2;
zx  = [pos(1)  zax(1)];
zy  = [pos(2)  zax(2)];
zz  = [pos(3)  zax(3)];



plt(1).XData = fx;
plt(1).YData = fy;
plt(1).ZData = fz;

plt(2).XData = ax;
plt(2).YData = ay;
plt(2).ZData = az;

plt(3).XData = px;
plt(3).YData = py;
plt(3).ZData = pz;

plt(4).XData = sx;
plt(4).YData = sy;
plt(4).ZData = sz;

plt(5).XData = zx;
plt(5).YData = zy;
plt(5).ZData = zz;

rplt(1).XData = tout;
rplt(1).YData = xout(1,:)*180/pi;
tplt(1).XData = tout;
tplt(1).YData = xout(3,:)*180/pi;
yplt(1).XData = tout;
yplt(1).YData = xout(5,:)*180/pi;

drawnow limitrate; % display updates



end