yjlqgctr1.m

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

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

%系统初始值
sf=1;
s0=3;
z0=[0 0 s0 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 0 1 0];
D=[0 0]';
H=[0 -1/M*l 0 0]';
sys1=ss(A,B,C,D);

%判断系统是否可控可观
R1=ctrb(A,B);
n1=rank(R1);
if (n1==4)
    disp('  系统可控！   ');
else
    disp('  系统不可控！   ');
end
    
R2=obsv(A,C);
n2=rank(R2);
if (n2==4)
    disp('  系统可测！   ');
else
    disp('  系统不可测！   ');
end

%最优控制器
q=[ 0.1 0 ;
    0   5];
r=0.01;
kx=lqry(sys1,q,r);

%kalman 滤波器
sys2=ss(A,[B H],C,[D D]);
[est l p]=kalman(sys2,1,0.0001*eye(2));

%LQG控制器
reg=lqgreg(est,kx);
[A1,B1,C1,D1]=ssdata(reg);