Whole_Skyhok_LQR.m

m = 1200;
m1 = 50;
m2 = 50;
m3 = 50;
m4 = 50;
jx = 500;
jy = 2000;
k1 = 20000;
k2 = 20000;
k3 = 25000;
k4 = 25000;
c1 = 1000;
c2 = 1000;
c3 = 2000;
c4 = 2000;
a = 1.2;
b = 1.5;
s = 0.7;
l = 1.4;
kt1 = 200000;
kt2 = 200000;
kt3 = 200000;
kt4 = 200000;

A = [0 0 0 0 0 0 0 1 0 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 0 1 0 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 0 1 0 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 0 1;
     -[k1+k2+k3+k4 （k1-k2+k3-k4）*s -（k1+k2）*a+（k3+k4）*b -k1 -k2 -k3 -k4 c1+c2+c3+c4 （c1-c2+c3-c4）*s -（c1+c2）*a+（c3+c4）*b -c1 -c2 -c3 -c4]/m;
     -[（k1-k2+k3-k4）*s （k1+k2+k3+k4）*s*s （-k1*a+k2*a+k3*b-k4*b）*s -k1*s k2*s -k3*s k4*s （c1-c2+c3-c4）*s （c1+c2+c3+c4）*s*s （-c1*a+c2*a+c3*b-c4*b）*s -c1*s c2*s -c3*s c4*s]/jx;
     -[-（k1+k2）*a+（k3+k4）*b （-k1*a+k2*a+k3*b-k4*b）*l （k1+k2）*a*a+（k3+k4）*b*b k1*a k2*a -k3*b -k4*b -（c1+c2）*a+（c3+c4）*b （-c1*a+c2*a+c3*b-c4*b）*l （c1+c2）*a*a+（c3+c4）*b*b c1*a c2*a -c3*b -c4*b]/jy;
     -[-k1 -k1*s k1*a k1+kt1 0 0 0 -c1 -c1*s c1*a c1 0 0 0]/m1;
     -[-k2 k2*s k2*a 0 k2+kt2 0 0 -c2 c2*s c2*a 0 c2 0 0]/m2;
     -[-k3 -k3*s -k3*b 0 0 k3+kt3 0 -c3 -c3*s -c3*b 0 0 c3 0]/m3;
     -[-k4 k4*s -k4*b 0 0 0 k4+kt4 -c4 c4*s -c4*b 0 0 0 c4]/m4
    ];
B = [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;
     [1 1 1 1]/m;
     [s -s s -s]/jx;
     [-a -a b b]/jy;
     [-1 0 0 0]/m1;
     [0 -1 0 0]/m2;
     [0 0 -1 0]/m3;
     [0 0 0 -1]/m4
    ];