daolibai2.asv

高 斯 迭 代

M0=1.6;
 M1=0.185;
 M2=0.21;
 J1=0.0036;
 J2=0.0040;
 l1=0.2415;
 l2=0.2450;
L1=0.483;
F0=22.915;
F1=0.00705;
F2=0.00264;
G0=11.88;
g=9.8;%
M00=[M0+M1+M2,M2*L1+M1*l1,M2*l2;
    M1*l1+M2*l1,J1+M1*l1*l1+M2*L1*L1,M2*L1*l2;
    M2*l2,M1*L1*l2,J2+M2*l2*l2];
F0000=[-F0,0,0;0,-(F1+F2),F2;0,F2,-F2];
N00=[0,0,0;0,(M1*l1+M2*L1)*g,0;0,0,M2*l2*g];
G=[G0;0;0];
T0=[1 0 0;0 1 0;0 -1 1];

A=zeros(6,6);
B=zeros(6,1);
C=eye(6,6);
A(1:3,4:6)=eye(3,3);
A(4:6,1:3)=M00\N00;

B(4:6)=M00\G;
D=zeros(6,1);
eig(A)
t=0.8;
I=1.6*eye(6,6);
[K,S,E]=lqr(A,B,I,t)
T=0:0.02:10;
Ac=[A-B*K]
%Nbar=rscale(A,B,C,D,K);
U=ones(size(T));
x0=[0 1/12*pi 1/18*pi 0 0 0 ];
[Y,X]=lsim(Ac,B,C,D,U,T,x0);
subplot(2,1,1)
plot(T,X(:,2),T,X(:,3))
%K=[K,0,0,0,0];
n=length(T);
for j=1:n
    u(j,:)=K*(X(j,:))';
end
   % u=K.*X
subplot(2,1,2)
plot(T,-u)
figure