%应用改进Boltzmann-DHNN解决LQ问题
%得到A,B矩阵
m1=2;
m2=10;
m3=1;
M=5;
k=20;
l1=0.05;
l2=0.3;
l3=0.05;
g=9.8;
K12=(3*(g*(6*l2*l3*(M+m1+2*m2+m3)+3*l3^2*(M+m1+2*m2+m3)+l2^2*(7*M+7*m1+10*m2+7*m3))*(l3*(m2+m3)+l1*(m1+2*(m2+m3)))-...
k*(3*l3^2*(2*M+2*m1+4*m2+3*m3)+3*l2*l3*(3*M+3*m1+6*m2+4*m3)+l2^2*(7*M+7*m1+10*m2+7*m3)+3*l1*(l2+l3)*(2*M+3*m1+4*m2+4*m3))))/...
(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3))));
K13=(3*(-3*(l2+l3)^2*m2*g*(l3*(M+m1+2*(m2+m3))+l1*(2*M+3*m1+4*(m2+m3)))+...
k*(3*l3^2*(2*M+2*m1+4*m2+3*m3)+3*l2*l3*(3*M+3*m1+6*m2+4*m3)+l2^2*(7*M+7*m1+10*m2+7*m3)+3*l1*(l2+l3)*(2*M+3*m1+4*(m2+m3)))))/...
(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3))));
K17=(3*(l3*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+l1*(6*l2*l3*(m1+2*m3)+3*l3^2*(m1+2*m3)+l2^2*(7*m1+8*m2+14*m3))))/...
(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3))));
K22=-(3*(3*g*(l2+l3)*(l3*(M+m1)+l1*(2*M+m1))*m2*(l3*(m2+m3)+l1*(m1+2*(m2+m3)))-...
k*(3*l1*(2*M+m1)*(l2*m2+3*l3*m2+2*l3*m3)+3*l3*(M+m1)*(l2*m2+l3*(2*m2+m3))+l1^2*(m1*(4*m1+7*(m2+m3))+M*(7*m1+12*(m2+m3))))))/...
(m2*(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3)))));
K23=(3*((l2+l3)*m2*g*(3*l3^2*(M+m1)*(m2+m3)+6*l1*l3*(2*M+m1)*(m2+m3)+l1^2*(m1*(4*m1+7*(m2+m3))+M*(7*m1+12*(m2+m3))))-...
k*(3*l1*(2*M+m1)*(l2*m2+3*l3*m2+2*l3*m3)+3*l3*(M+m1)*(l2*m2+l3*(2*m2+m3))+l1^2*(m1*(4*m1+7*(m2+m3))+M*(7*m1+12*(m2+m3))))))/...
(m2*(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3)))));
K27=(3*l1*(l1-3*l3)*(l2+l3)*m1)/...
(3*l3^2*(M+m1)*(6*l2*l3*m3+3*l3^2*m3+l2^2*(4*m2+7*m3))+...
6*l1*l3*(6*l2*l3*(-m1*m2+2*M*m3+m1*m3)+3*l3^2*(-m1*m2+2*M*m3+m1*m3)+l2^2*(8*M*m2+m1*m2+14*M*m3+7*m1*m3))+...
l1^2*(6*l2*l3*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+3*l3^2*(m1*(4*m1+2*m2+7*m3)+M*(7*m1+12*m3))+...
l2^2*(m1*(28*m1+34*m2+49*m3)+M*(49*m1+48*m2+84*m3))));
K32=(3*(-k*(l3*(m2+m3)+l1*(m1+2*(m2+m3)))*((2*l1+l3)^2*(l2+l3)^2*m2^2-(1/3)*(7*l2^2+6*l2*l3+3*l3^2)*m2*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))+...
(-(2*l1+l3)*(l2+l3)^2*m2^2-(1/3)*(7*l2^2+6*l2*l3+3*l3^2)*m2*(l3*(m2+m3)+l1*(m1+2*(m2+m3))))*...
(k*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3))+(2*l1+l3)*(l2+l3)*m2*(k-g*(l3*(m2+m3)+l1*(m1+2*(m2+m3)))))))/...
((l2+l3)*m2*((l3*(M+m1)+l1*(2*M+m1))*m2*(-3*(2*l1+l3)^2*(l2+l3)^2*m2+(7*l2^2+6*l2*l3+3*l3^2)*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))+...
(1/3)*l1*(l1-3*l3)*m1*m2*(l3*(6*l2*l3*(2*m2+m3)+3*l3^2*(2*m2+m3)+l2^2*(10*m2+7*m3))+...
l1*(6*l2*l3*(m1+4*m2+2*m3)+3*l3^2*(m1+4*m2+2*m3)+l2^2*(7*m1+20*m2+14*m3)))));
K33=(3*((k-(l2+l3)*m2*g)*(l3*(m2+m3)+l1*(m1+2*(m2+m3)))*...
((2*l1+l3)^2*(l2+l3)^2*m2^2-(1/3)*(7*l2^2+6*l2*l3+3*l3^2)*m2*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))+...
(-k*(2*l1+l3)*(l2+l3)*m2-(k-(l2+l3)*m2*g)*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))*...
(-(2*l1+l3)*(l2+l3)^2*m2^2-(1/3)*(7*l2^2+6*l2*l3+3*l3^2)*m2*(l3*(m2+m3)+l1*(m1+2*(m2+m3))))))/...
((l2+l3)*m2*((l3*(M+m1)+l1*(2*M+m1))*m2*(-3*(2*l1+l3)^2*(l2+l3)^2*m2+(7*l2^2+6*l2*l3+3*l3^2)*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))+...
(1/3)*l1*(l1-3*l3)*m1*m2*(l3*(6*l2*l3*(2*m2+m3)+3*l3^2*(2*m2+m3)+l2^2*(10*m2+7*m3))+...
l1*(6*l2*l3*(m1+4*m2+2*m3)+3*l3^2*(m1+4*m2+2*m3)+l2^2*(7*m1+20*m2+14*m3)))));
K37=-(3*(2*l1+l3)*((2*l1+l3)^2*(l2+l3)^2*m2^2-(1/3)*(7*l2^2+6*l2*l3+3*l3^2)*m2*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3))))/...
((l3*(M+m1)+l1*(2*M+m1))*m2*(-3*(2*l1+l3)^2*(l2+l3)^2*m2+(7*l2^2+6*l2*l3+3*l3^2)*((7/3)*l1^2*m1+(2*l1+l3)^2*(m2+m3)))+...
(1/3)*l1*(l1-3*l3)*m1*m2*(l3*(6*l2*l3*(2*m2+m3)+3*l3^2*(2*m2+m3)+l2^2*(10*m2+7*m3))+...
l1*(6*l2*l3*(m1+4*m2+2*m3)+3*l3^2*(m1+4*m2+2*m3)+l2^2*(7*m1+20*m2+14*m3))));
A=[0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1;
0 K32 K33 0 0 0;
0 K12 K13 0 0 0;
0 K22 K23 0 0 0];
B=[0;0;0;K37;K17;K27];
%......................................
H=[1];%[1 0 0 0 0 0;0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
Q=[1];%[1 0 0 0 0 0;0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1];
n=1;
r=1;
N=6;
M=64;
m=6;
L=r*N*M;
%.........构造得到D矩阵
D11=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D22=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D33=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D44=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D55=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D66=[2^-1,2^-2,2^-3,2^-4,2^-5,2^-6,2^-7,2^-8,2^-9,2^-10,2^-11,2^-12,2^-13,2^-14,2^-15,2^-16,...
2^-17,2^-18,2^-19,2^-20,2^-21,2^-22,2^-23,2^-24,2^-25,2^-26,2^-27,2^-28,2^-29,2^-30,2^-31,2^-32,...
2^-33,2^-34,2^-35,2^-36,2^-37,2^-38,2^-39,2^-40,2^-41,2^-42,2^-43,2^-44,2^-45,2^-46,2^-47,2^-48,...
2^-49,2^-50,2^-51,2^-52,2^-53,2^-54,2^-55,2^-56,2^-57,2^-58,2^-59,2^-60,2^-61,2^-62,2^-63,2^-64];
D12=zeros(1,64);
D13=zeros(1,64);
D14=zeros(1,64);