regolatore lqr per nxtway

tentativo di trovare il regolatore lqr (wiki) per il nxt di sybille mayer con scilab
attingendo a piene mani (copia-incolla) dal mitico R. Bucher


function [num,den]=tfdata(G)
num=G('num');
den=G('den');
endfunction

A=[0,1,0,0; 94.05,0,0,1.3;0,0,0,1; -544.05,0,0,-14.3]
B=[0;-0.21;0;2.32]
C=[1,0,0,0;0,0,1,0]
D=[0;0]
nxtway_SYB=syslin('c',A,B,C,D)
s=%s;
Ts=0.03
spec(A)
autovalori_di_A=spec(A)
sys=syslin('c',A,B,C,D);
sysd=dscr(sys,Ts);
[ad,bd,cd,dd]=abcd(sysd);

// pesi del controllore LQR

Q=diag([10000,1,10000,1]); // 4 per 4
R=[1]; // 1 per 1

//calcola i guadagni LQR per un sistema discreto
[n1,d1]=size(ad);
big=sysdiag(Q,R);
[w,wp]=fullrf(big ,1e-20);
C1=wp(:,1:n1);
D12=wp(:,n1+1:$);
P=syslin('d',ad,bd,C1,D12);
[k,X]=lqr(P)
k_lqr= -k;

E=spec(ad);
plqr=abs(real(log(E)/Ts))';
pmax=max(plqr);
k_lqr=-k_lqr;
preg=spec(A);
// osservatore di ordine ridotto
poli_oss=exp([real(preg(3)),real(preg(4))]*10*Ts);
T=[0,0,0,1;0,1,0,0];

// trova l'ossrvatore di ordine ridotto per A,B,C,D, con polo di osservatore
// T e' la matrice necessaria per rendere [C;T] invertibile
P=[cd;T]
invP=inv([cd;T])
AA=P * ad * invP
ny=size(cd,1)
nx=size(ad,1)
nu=size(bd,2)
A11=AA(1:ny,1:ny)
A12=AA(1:ny,ny+1:nx)
A21=AA(ny+1:nx,1:ny)
A22=AA(ny+1:nx,ny+1:nx)
L1=ppol (A22',A12',poli_oss)';
nn=nx - ny;
A_redobs=[-L1 eye(nn,nn)]*P*ad*invP*[zeros(ny,nn); eye(nn,nn)];
B_redobs=[-L1 eye(nn,nn)]*[P*bd P*ad*invP*[eye(ny,ny);L1]]*[eye(nu,nu) zeros(nu,ny);-dd, eye(ny,ny)];
C_redobs=invP*[zeros(ny,nx-ny);eye(nn,nn)];
D_redobs=invP*[zeros(ny,nu) eye(ny,ny);zeros(nx-ny,nu) L1]*[eye(nu,nu) zeros(nu,ny);-dd, eye(ny,ny)];
ao=A_redobs
bo=B_redobs
co=C_redobs
do=D_redobs

// Crea la forma compatta dell'osservatore ABCD e il guadagno K
//
//

Bu=bo(:,1);
By=bo(:,2:$);
Du=do(:,1);
Dy=do(:,2:$);
X=inv(1+k_lqr*Du);
Ac=ao - Bu*X*k_lqr*co;
Bc=[Bu*X,By-Bu*X*k_lqr*Dy]
Cc=-X*k_lqr*co;
Dc=[X,-X*k_lqr*Dy]
Greg=syslin('d',Ac,Bc,Cc,Dc)
///////////////////////////////////////////
Gregtf=ss2tf(Greg);
Gregtf( :,2)
Gregtf( :,3)
[g1n,g1d]=tfdata(Gregtf( :,2))
[g2n,g2d]=tfdata(Gregtf( :,3))


questo butta fuori come regolatore il seguente:

Greg =


Greg(1) (state-space system:)

!lss A B C D X0 dt !

Greg(2) = A matrix =

- 1.1704196 - 16.638285
0.1322204 2.048937

Greg(3) = B matrix =

0.0329866 - 559.33453 - 69.773154
- 0.0036029 41.850078 4.3785807

Greg(4) = C matrix =

- 35.525535 - 497.8127

Greg(5) = D matrix =

1. - 16487.356 - 1308.8181

Greg(6) = X0 (initial state) =

0.
0.

Greg(7) = Time domain =

d