
function st = update( v, pv );

%
% st = update( v, pv )
% Updates the status vector v.
% pv is the parameter vector.
%

p1 = v(1,1);
v1 = v(1,2);
a1 = v(1,3);
p2 = v(1,4);
v2 = v(1,5);
a2 = v(1,6);
ts = pv(1,1);
rt = pv(1,2);
maa = pv(1,3);
mia = pv(1,4);
mav = pv(1,5);
miv = pv(1,6);
bias = pv(1,7);
trl = pv(1,8);

v1 = v1 + a1*ts;
v2 = v2 + a2*ts;
p1 = p1 + ts*v1 + ts*ts*a1/2;
p2 = p2 + ts*v2 + ts*ts*a2/2;

if v1>mav, v1=mav; end
if v2>mav, v2=mav; end
if v1<miv, v1=miv; end
if v2<miv, v2=miv; end

if p1>trl, p1 = p1-trl; end
if p2>trl, p2 = p2-trl; end

st = [ p1 v1 a1 p2 v2 a2 ];

