lqr控制算法程序

fid=fopen('3.txt','r');%读取地震波dzhbo=fscanf(fid,'%f',1501);xg=dzhbo/max(abs(dzhbo))*2;dt=0.02;t=(0:length(dzhbo)-1)*dt;symsw;K=1.0e8*[4-20;-24-2;0-22];%刚度矩阵M=1.0e5*[400;040;004];%质量矩阵a=K-w^2*M;%频率方程b=solve(det(a));%求解频率方程行列式等于0z=real(double(b));w1=z(3);w2=z(5);zeta1=0.05;zeta2=0.05;%解得频率w1，w2arphc=(2*w1*w2*(zeta1*w2-zeta2*w1))/(w2^2-w1^2);betac=(2*(zeta2*w2-zeta1*w1))/(w2^2-w1^2);C=arphc*M+betac*K;%阻尼矩阵Bs=[1-10;01-1;001];%地震作用位置矩阵Ds=-M*ones(3,1);%环境位置干扰矩阵A=[zeros(3)eye(3);inv(-M)*Kinv(-M)*C];B=[zeros(3);inv(M)*Bs];D=[zeros(3,1);inv(M)*Ds];I=eye(3);arph=100;beta=[0.010.10.513581215204060]*1.0e-6;fori=1:12;betai=beta(i);Q=arph*[Kzeros(3);zeros(3)M];R=betai*I;G=lqr(A,B,Q,R);%连续状态方程控制力状态反馈增益矩阵[y,V]=lsim(A,D,eye(6),zeros(6,1),xg,t);%无控时输入响应[Y,Z]=lsim((A-B*G),D,eye(6),zeros(6,1),xg,t);%有控时输入响应V1=A*V'+D*xg';%无控结构状态方程Z1=(A-B*G)*Z'+D*xg';%有空结构状态方程U=-G*Z'/1000;%最优控制力f01=max(abs(V(:,1)));%各层无控位移：mmax(abs(V(:,2)));max(abs(V(:,3)));f02=max(abs(V(:,2)))-max(abs(V(:,1)));%第二层无控层间位移：mf03=max(abs(V(:,3)))-max(abs(V(:,2)));%第三层无控层间位移：mmax(abs(V(:,4)));%各层无控速度：mf03=max(abs(V(:,3)))-max(abs(V(:,2)));%第三层无控层间位移：mmax(abs(V(:,4)));%各层无控速度：mmax(abs(V(:,5)));max(abs(V(:,6)));f11=max(abs(Z(:,1)));%各层有控位移：mmax(abs(Z(:,2)));max(abs(Z(:,3)));f12=max(abs(Z(:,2)))-max(abs(Z(:,1)));%第二层有控层间位移：mf13=max(abs(Z(:,3)))-max(abs(Z(:,2)));%第三层有控层间位移：mf=[f01f02f03f11f12f13];max(abs(Z(:,4)));%各层有控速度：mmax(abs(Z(:,5)));max(abs(Z(:,6)));max(abs(V1(1,:)));%各层无控速度：mmax(abs(V1(2,:)));max(abs(V1(3,:)));s01=max(abs(V1(4,:)));%各层无控加速度：ms02=max(abs(V1(5,:)));s03=max(abs(V1(6,:)));max(abs(Z1(1,:)));%各层有控速度：mmax(abs(Z1(2,:)));max(abs(Z1(3,:)));s11=max(abs(Z1(4,:)));%各层有控加速度：ms12=max(abs(Z1(5,:)));s13=max(abs(Z1(6,:)));s=[s01s02s03s11s12s13];n(i)=max(abs(U(1,:)));%各层最大控制力：KNmax(abs(U(2,:)));max(abs(U(3,:)));a1=figure;plot(t,V(:,1),'--',t,Z(:,1));xlabel('时间/s');ylabel('第一层位移/m');a2=figure;plot(t,U(1,:));xlabel('时间/s');ylabel('第一层控制力/m');weiyi(i)=f11;f03=max(abs(V(:,3)))-max(abs(V(:,2)));%第三层无控层间位移：mmax(abs(V(:,4)));%各层无控速度：mmax(abs(V(:,5)));max(abs(V(:,6)));f11=max(abs(Z(:,1)));%各层有控位移：mmax(abs(Z(:,2)));max(abs(Z(:,3)));f12=max(abs(Z(:,2)))-max(abs(Z(:,1)));%第二层有控层间位移：mf13=max(abs(Z(:,3)))-max(abs(Z(:,2)));%第三层有控层间位移：mf=[f01f02f03f11f12f13];max(abs(Z(:,4)));%各层有控速度：mmax(abs(Z(:,5)));max(abs(Z(:,6)));max(abs(V1(1,:)));%各层无控速度：mmax(abs(V1(2,:)));max(abs(V1(3,:)));s01=max(abs(V1(4,:)));%各层无控加速度：ms02=max(abs(V1(5,:)));s03=max(abs(V1(6,:)));max(abs(Z1(1,:)));%各层有控速度：mmax(abs(Z1(2,:)));max(abs(Z1(3,:)));s11=max(abs(Z1(4,:)));%各层有控加速度：ms12=max(abs(Z1(5,:)));s13=max(abs(Z1(6,:)));s=[s01s02s03s11s12s13];n(i)=max(abs(U(1,:)));%各层最大控制力：KNmax(abs(U(2,:)));max(abs(U(3,:)));a1=figure;plot(t,V(:,1),'--',t,Z(:,1));xlabel('时间/s');ylabel('第一层位移/m');a2=figure;plot(t,U(1,:));xlabel('时间/s');ylabel('第一层控制力/m');weiyi(i)=f11;endc=figure;plot(beta,weiyi*100,'-s',beta,n/500);xlabel('β');ylabel('第一层位移/cm和第一层控制力/500KN');