SQP.zip_SQP_SQP优化_matlab SQP

function [d,mu,lam,val, k]=qpsubp(dfk,Bk,Ae,hk,Ai,gk) % 功能: 求解二次规划子问题: min qk(d)=0.5*d'*Bk*d+dfk'*d, % s.t. hk+Ae*d=0, gk+Ai*d>=0. %输入: dfk是xk处的梯度, Bk是第k次近似Hesse阵, Ae,hk线性等式约束 % 的有关参数, Ai,gk是线性不等式约束的有关参数 %输出: d,val分别是是最优解和最优值, mu, lam是乘子向量, k是迭代次数. n=length(dfk); l=length(hk); m=length(gk); gamma=0.05; epsilon=1.0e-6; rho=0.5; sigma=0.2; ep0=0.05; mu0=0.05*zeros(l,1); lam0=0.05*zeros(m,1); d0=ones(n,1); u0=[ep0;zeros(n+l+m,1)]; z0=[ep0; d0; mu0;lam0,]; k=0; %k为迭代次数 z=z0; ep=ep0; d=d0; mu=mu0; lam=lam0; while (k<=150) dh=dah(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk); if(norm(dh)<epsilon) break; end A=JacobiH(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk); b=beta(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk,gamma)*u0-dh; dz=A\b; if(l>0&m>0) de=dz(1); dd=dz(2:n+1); du=dz(n+2:n+l+1); dl=dz(n+l+2:n+l+m+1); end if(l==0) de=dz(1); dd=dz(2:n+1); dl=dz(n+2:n+m+1); end if(m==0) de=dz(1); dd=dz(2:n+1); du=dz(n+2:n+l+1); end i=0; %mk=0; while (i<=20) if(l>0&m>0) dh1=dah(ep+rho^i*de,d+rho^i*dd,mu+rho^i*du,lam+rho^i*dl,dfk,Bk,Ae,hk,Ai,gk); end if(l==0) dh1=dah(ep+rho^i*de,d+rho^i*dd,mu,lam+rho^i*dl,dfk,Bk,Ae,hk,Ai,gk); end if(m==0) dh1=dah(ep+rho^i*de,d+rho^i*dd,mu+rho^i*du,lam,dfk,Bk,Ae,hk,Ai,gk); end if(norm(dh1)<=(1-sigma*(1-gamma*ep0)*rho^i)*norm(dh)) mk=i; break; end i=i+1; if(i==20), mk=10; end end alpha=rho^mk; if(l>0&m>0) ep=ep+alpha*de; d=d+alpha*dd; mu=mu+alpha*du; lam=lam+alpha*dl; end if(l==0) ep=ep+alpha*de; d=d+alpha*dd; lam=lam+alpha*dl; end if(m==0) ep=ep+alpha*de; d=d+alpha*dd; mu=mu+alpha*du; end k=k+1; end val=0.5*d'*Bk*d+dfk'*d; %%%%%%%%%%%%%%%%%%%%%%%%%% function p=phi(ep,a,b) p=a+b-sqrt(a^2+b^2+2*ep^2); %%%%%%%%%%%%%%%%%%%%%%%%%% function dh=dah(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk) n=length(dfk); l=length(hk); m=length(gk); dh=zeros(n+l+m+1,1); dh(1)=ep; if(l>0&m>0) dh(2:n+1)=Bk*d-Ae'*mu-Ai'*lam+dfk; dh(n+2:n+l+1)=hk+Ae*d; for(i=1:m) dh(n+l+1+i)=phi(ep,lam(i),gk(i)+Ai(i,:)*d); end end if(l==0) dh(2:n+1)=Bk*d-Ai'*lam+dfk; for(i=1:m) dh(n+1+i)=phi(ep,lam(i),gk(i)+Ai(i,:)*d); end end if(m==0) dh(2:n+1)=Bk*d-Ae'*mu+dfk; dh(n+2:n+l+1)=hk+Ae*d; end dh=dh(:); %%%%%%%%%%%%%%%%%%%%%%%%%% function bet=beta(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk,gamma) dh=dah(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk); bet=gamma*norm(dh)*min(1,norm(dh)); %%%%%%%%%%%%%%%%%%%%%%%%%% function [dd1,dd2,v1]=ddv(ep,d,lam,Ai,gk) m=length(gk); dd1=zeros(m,m); dd2=zeros(m,m); v1=zeros(m,1); for(i=1:m) fm=sqrt(lam(i)^2+(gk(i)+Ai(i,:)*d)^2+2*ep^2); dd1(i,i)=1-lam(i)/fm; dd2(i,i)=1-(gk(i)+Ai(i,:)*d)/fm; v1(i)=-2*ep/fm; end %%%%%%%%%%%%%%%%%%%%%%%%%% function A=JacobiH(ep,d,mu,lam,dfk,Bk,Ae,hk,Ai,gk) n=length(dfk); l=length(hk); m=length(gk); A=zeros(n+l+m+1,n+l+m+1); [dd1,dd2,v1]=ddv(ep,d,lam,Ai,gk); if(l>0&m>0) A=[1, zeros(1,n), zeros(1,l), zeros(1,m); zeros(n,1), Bk, -Ae', - Ai'; zeros(l,1), Ae, zeros(l,l), zeros(l,m) ; v1, dd2*Ai, zeros(m,l), dd1]; end if(l==0) A=[1, zeros(1,n), zeros(1,m); zeros(n,1), Bk, -Ai'; v1, dd2*Ai, dd1]; end if(m==0) A=[1, zeros(1,n), zeros(1,l); zeros(n,1), Bk, -Ae'; zeros(l,1), Ae, zeros(l,l), ]; end