uapre=2*[rand(1,150001);rand(1,150001);rand(1,150001);rand(1,150001);rand(1,150001);rand(1,150001)];
T=1e-4;
A =[0 1 0 0 0 0;0 0 0 0 0 0;0 0 0 1 0 0;0 0 0 0 0 0;0 0 0 0 0 1; 0 0 0 0 0 0];
%Ô­B¾ØÕó
B=[0 0 0;1 0 0;0 0 0;0 1 0;0 0 0;0 0 1];
L=[1 1 0 0 0 0;0 0 1 1 0 0;0 0 0 0 1 1];

g=[0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
-0.2883 0.2883 -0.8654 0.8654 0 0.0025
-0.0857 -0.0857 -0.5263 -0.5263 0.0098 -0.0073
-0.0959 0.0959 0.2079 -0.2079 -0.0045 -0.0051];

J=[554486 0 -23002
0 1136949 0
-23002 0 1376852];

Q=eye(6);
r=1;
P = are(A,r^(-1)*B*B',Q);
K1=-r^(-1)*B'*P;
barA1=A+B*K1;

Q=eye(6);
r=50;
P = are(A,r^(-1)*B*B',Q);
K2=-r^(-1)*B'*P;
barA2=A+B*K2;

%global sumfuninv
sumfun=zeros(3,3);
ddt=T/1000;
for i=0:ddt:T-ddt
    sumfun=sumfun+L*expm(-A*i)*B*B'*expm(-A'*i)*L';
end
sumfuninv=inv(sumfun)/ddt;
cond(sumfuninv)


sim('Step2_control_normal_K1');
sim('Step3_control_without_vc_K1');
sim('Step4_control_compensate_K1');
sim('Step5_control_normal_K2');
sim('Step6_control_without_vc_K2');
sim('Step7_control_compensate_K2');
save data
Step8_plot