forked from Hannah-Zhou/Optimization_Algorithm
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathqpsubp.m
123 lines (122 loc) · 3.66 KB
/
qpsubp.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
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 (mm<=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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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