yjlqgm.m

本例采用最优控制的方法,利用二次型性能指标,保证小车所滑过的距离和整个过程所花费的能量达到最优值.程序包括initial.m,yjlqgctr.m,cartoon.m,simlqgyj.slk等文件

%参数
h=1.5;k=5;dt=0.01;
l=10;r=0.5;
M=2.0;m=0.1;g=9.81;

%系统初始值
zf=[0 0 1 0]';
z0=[0 0 3 0]';
%创建系统
c1=M*l;c2=m*l;c3=m*g;c4=(M+m)*g;
A=[0 1 0 0;c4/c1 0 0 0;0 0 0 1;-c3/M 0 0 0];
B=[0 -1/c1 0 1/M]';
C=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
D=[0 0 0 0]';
H=[0 -1/M*l 0 0]';
%G=[0 1 0 1]';
sys1=ss(A,B,C,D)
A1=ctrb(A,B)
rank(A1)
A2=obsv(A,C)
rank(A2)

%最优
q=[0.1 0 0 0;
    0 0.1 0 0;
    0 0 0.1 0;
    0 0 0 5];
r=0.01;
kx=lqry(sys1,q,r)
%kalman
%wx=sart(1/dt)*randn(2,length(t);
sys2=ss(A,[B H],C,[D D]);
[est l p]=kalman(sys2,1,100*eye(4));
reg=lqgreg(est,kx);
sys=feedback(sys1,reg,+1);
[A1,B1,C1,D1]=ssdata(sys);
z=[z0;z0];
for i=1:4000
    u=1;
    dz=A1*z+B1*u;
    z=z+dz*dt;
    y1=C1*z+D1*u;
    y(:,i)=y1;
end
plot(y(1:4,:))