g = 9.8;  %重力加速度 單位：m/s2
pi = 3.14; %圓周率

mh = 1.8; %直升機本體質量 單位：kg
la = 0.88; %直升機本體到旋轉軸的距離 單位：m
mw = 3.433; %平衡塊的質量 單位：kg
lb = 0.35; %平衡快到支點的距離 單位：m

mf = 0.552; %直升機單個推進器的質量 單位：kg
mb = 0.552; %直升機單個推進器的質量 單位：kg
lh = 0.17; %直升機推進器到翻轉軸的距離 單位：m

Jp = mf*lh^2 + mb*lh^2 %直升機系統翻轉軸的轉動慣量 單位：m*kg2

Tg = mh*g*la - mw*g*lb %系統有效的重力矩 單位：N*m
Je = mh*la^2 + mw*lb^2 %直升機系統旋轉軸的轉動慣量 單位：m*kg2

Kp = Tg/la %直升機的等效重力 單位：N
Jt = mh*la^2 + mw*lb^2 %直升機系統旋轉軸的轉動慣量 單位：m*kg2

%Kf = 1.0; %直升機電機的力常數
Kf = 12; %直升機電機的力常數

A = [ 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 0;
      0    0     0 0 0 0 0 0;
      0 Kp*la/Jt 0 0 0 0 0 0;
      1    0     0 0 0 0 0 0;
      0    0     0 0 0 1 0 0];
B = [    0         0; 
         0         0;
         0         0;
      Kf*la/Je  Kf*la/Je;
      Kf*lh/Jp  -Kf*lh/Jp;
         0         0;
         0         0;
         0         0];
R = diag([1 1]);
%Q = diag([.1 1 .1 1 1 1 .5]);
Q = diag([2 .2 .2 .02 .02 2 .02 .01]);
K = lqr(A,B,Q,R)