%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%???Googol?Linear?2?stage?Inverted?Pendulum?LQR?Control??%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear?all;
format?long;
k12=159.504;k13=-38.4356;k14=0.784646;k19=6.64904;
k22=-127.9;k23=57.9951;k24=-17.189;k29=-0.15831;
k32=2.57086;k33=-16.9245;k34=39.3485;k39=0.00318211;

A=[0??0??0??0?1??0??0??0;
???0??0??0??0?0??1??0??0;
???0??0??0??0?0??0??1??0;?
???0??0??0??0?0??0??0??1;??
???0??0??0??0?0??0??0??0;
???0?k12?k13?k14?0?0?0?0;
???0?k22?k23?k24?0?0?0?0;
???0?k32?k33?k34?0?0?0?0]

B=[?0??0??0??0??1??k19??k29??k39]‘;

C=[1?0?0??0?0?0?0?0;
???0?1?0??0?0?0?0?0;
???0?0?1??0?0?0?0?0;
???0?0?0??1?0?0?0?0];
D=[0;???0;???0;??0];

?Q11=300;Q22=400;Q33=500;Q44=600;
%Q11=300;Q22=500;Q33=700;Q44=900;
%Q11=1;Q22=1;Q33=1;

Q=[Q11??0???0???0???0???0???0???0;?
????0??Q22??0???0???0???0???0???0;
????0??0???Q33??0???0???0???0???0;
????0???0???0??Q44??0???0???0???0;
????0???0???0???0???0???0???0???0;
????0???0???0???0???0???0???0???0;
????0???0???0???0???0???0???0???0;
????0???0???0???0???0???0???0???0];
R=1;
K=lqr（ABQR）
Ka=[1?K];???%因為輸出總是把第一位省略，所以加上一位
Ac=[（A-B*K）];
Bc=[B];
Cc=[C];
Dc=[D];

T=0:0.005:5;
U=0*ones（size（T））;
Cn=[1?0?0?0?0?0?0?0];

s=size（A1）;
Z=[zeros（[1s]）?1];

N=inv（[AB;Cn0]）*Z‘;
Nx=N（1:s）;
Nu=N（1+s）;
Nbar=Nu+K*Nx;

Bcn=[Nbar*B];
x0=[0?0?0.05?0?0?0?0?0];
[YX]=lsim（AcBcnCcDcUTx0）;
xpos=Y（:1）;
xangle=Y（:2）;
xangle2=Y（:3）;
xangle3=Y（:4）;

%?plot（Txpos‘:‘）
%?hold?on;
%?plot（Txangle‘--‘）
%?hold?on;
%?plot（Txangle2‘*‘）
%?hold?on;
%?plot（Txangle3‘-‘）

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????34278??2013-03-15?14:36??三級倒立擺實驗\L3DIPLQR1.mdl
?????文件????????1623??2013-03-15?14:03??三級倒立擺實驗\test1.asv
?????文件????????1669??2013-03-15?14:29??三級倒立擺實驗\test1.m
?????目錄???????????0??2013-03-15?14:36??三級倒立擺實驗\