[Hans Blom, 970324]

Here is the code of the MCT controller that is the result of my

earlier analysis. It realizes its control objective (x=r) perfectly,

and its reconstruction of the disturbance d is as good as can be. The

only complaint could be that it is TOO good: the control sequence u

-- although exactly such that x is made equal to r -- oscillates. But

that is necessary: with any other control sequence, control would be

worse. Moreover, we didn't specify that such "nervous" behavior was

not allowed.

On time indices: it is usual to suppose that the calculation of u

takes negligible time compared to dt. Thus the program assumes that,

immediately after x(t) has been observed, u(t) will be applied. If

the computation of u takes more time, the computation becomes more

complex because an additional delay must be dealt with.

Greetings,

Hans

program theodolite_model_1;

const

J = 1.0; {theodolite characteristic}

var

x, {perceived position}

v, {perceived velocity}

u, {controller's output}

r, {reference level}

d, {disturbance estimate}

p, {prediction of next observation}

dt, {time increment; var so it can be changed}

t: real; {time; for display/loop control purposes}

procedure observe;

{this procedure implements the theodolite motion equations; it

generates the NEXT observation(s) x(t+dt) and v(t+dt), given u(t)

and d(t); thus it also shifts the time by dt; note that aa, vv and

xx are LOCAL variables, not visible outside this function; declaring

them as "const" saves their value from run to run; aa, vv and xx are

initially known to be zero}

function disturbance: real;

{this function implements the disturbance, which is known only

to procedure observe, the theodolite's motion simulator}

begin

if (t < 4.0) or (t > 5.0) then

disturbance := 0.0

else

disturbance := 10.0

end;

const

aa: real = 0.0;

vv: real = 0.0;

xx: real = 0.0;

begin

aa := (u + disturbance) / J;

xx := xx + vv*dt + 0.5*aa*dt*dt;

vv := vv + aa*dt;

t := t + dt;

x := xx; v := vv {the controller may use these}

end;

function reference (t: real): real;

{this procedure specifies the reference level r(t) for x(t)}

begin

if t < 2.0 then

reference := 0.0

else

if t < 6.0 then

reference := (t - 2.0)

else

reference := 4.0

end;

procedure control;

{this function implements the controller; it knows the motion equa-

tions and J, is allowed to observe x(t) and v(t), and delivers u(t)}

begin

u := 2.0 * J * (reference (t+dt) - x - v*dt)/(dt*dt) - d;

end;

begin

dt := 0.1; {or any other value}

t := 0.0; {start at t=0}

r := 0.0; {initially zero}

x := 0.0; {initially zero}

v := 0.0; {initially zero}

d := 0.0; {estimate of disturbance}

repeat

writeln (t:12:3, r:12:6, x:12:6, v:12:6, d:12:6, u:12:6);

r := reference (t+dt); {specify desired x}

control; {controller computes u}

{predict next observation}

p := x + v*dt + 0.5 * (u + d) * (dt*dt) / J;

{time shifts by dt here}

observe; {observe x (and v?)}

{update disturbance estimate}

d := d + 2.0 * J * (x-p) / (dt*dt)

until t > 9.0

end.