#!/usr/bin/python #coding:utf-8 from numpy import * EPS_ZERO = 1.0e-9 # 内点法(Predictor-Collector)による2次計画問題の解法 # object min z = 0.5*x^T*G*x+cT*x # subject Ax >= b def predictor_corrector(x, Gmat, c, Amat, b, tau=0.5, eps=1.0e-3, nroop=30): ndim = Gmat.shape[0] # 次元数 ncnst = Amat.shape[0] # 制約数 alldim = ndim+2*ncnst zeros_ndim_ncnst = zeros((ndim, ncnst)) zeros_ncnst = zeros((ncnst, ncnst)) I_ncnst = identity(ncnst) # yとlmdの初期値設定 f = matrix(vstack([-Gmat*x-c, -Amat*x+b, zeros((ncnst,1))])) ExMat = matrix(vstack([hstack([Gmat, zeros_ndim_ncnst, -Amat.T]), hstack([Amat, -I_ncnst, zeros_ncnst]), hstack([zeros_ndim_ncnst.T, zeros_ncnst, zeros_ncnst])])) delta_xyl0 = linalg.pinv(ExMat)*f y = matrix([[max([1.0, abs(delta_xyl0[ndim+i,0])])] for i in range(ncnst)]) lmd = matrix([[max([1.0, abs(delta_xyl0[ndim+ncnst+i,0])])] for i in range(ncnst)]) rd = Gmat*x - Amat.T*lmd + c rp = Amat*x - y - b for n in range(nroop): err = linalg.norm(rd)**2+linalg.norm(rp)**2+(y.T*lmd)[0,0] if err < eps: break # affine scaling step lmd_y = matrix([[lmd[i,0]*y[i,0]] for i in range(ncnst)]) # [ Gmat, 0, -Amat.T ] # ExMat = [ Amat, -I, 0 ] # [ 0, diag(lmd), diag(y) ] f = matrix(vstack([-rd, -rp, -lmd_y])) ExMat = matrix(vstack([hstack([Gmat, zeros_ndim_ncnst, -Amat.T]), hstack([Amat, -I_ncnst, zeros_ncnst]), hstack([zeros_ndim_ncnst.T, diag([lmd[i,0] for i in range(ncnst)]), diag([y[i,0] for i in range(ncnst)]) ])])) delta_xyl_aff = linalg.pinv(ExMat)*f mu = (y.T*lmd/ncnst)[0,0] # alphaの計算 y_lmd_cnb = matrix(vstack([y, lmd])) yl_dyl = [-y_lmd_cnb[i,0]/delta_xyl_aff[ndim+i,0] for i in range(2*ncnst) if delta_xyl_aff[ndim+i,0] <= -EPS_ZERO] if yl_dyl == [] or min(yl_dyl) > 1.0: alpha_aff = 1.0 else: alpha_aff = min(yl_dyl) mu_aff = ((y + alpha_aff*delta_xyl_aff[ndim:(ndim+ncnst),0]).T*(lmd + alpha_aff*delta_xyl_aff[(ndim+ncnst):alldim,0])/ncnst)[0,0] # 中心パラメータ sig = (mu_aff/mu)**3 # corrector step delta_lmd_y_aff = matrix([[delta_xyl_aff[ndim+i,0]*delta_xyl_aff[ndim+ncnst+i,0]] for i in range(ncnst)]) sig_mu = matrix(sig*mu*ones((ncnst, 1))) f = matrix(vstack([-rd, -rp, -lmd_y-delta_lmd_y_aff+sig_mu])) delta_xyl = linalg.pinv(ExMat)*f # alpha_pri, alpha_dualの計算 y_dy = [-tau*y[i,0]/delta_xyl[ndim+i,0] for i in range(ncnst) if delta_xyl[ndim+i,0] <= -EPS_ZERO] if y_dy == [] or min(y_dy) > 1.0: alpha_pri = 1.0 else: alpha_pri = min(y_dy) lmd_dlmd = [-tau*lmd[i,0]/delta_xyl[ndim+ncnst+i,0] for i in range(ncnst) if delta_xyl[ndim+ncnst+i] <= -EPS_ZERO] if lmd_dlmd == [] or min(lmd_dlmd) > 1.0: alpha_dual = 1.0 else: alpha_dual = min(lmd_dlmd) alpha_hat = min([alpha_pri, alpha_dual]) # x, y, lmdの更新 x += alpha_hat*delta_xyl[0:ndim,0] y += alpha_hat*delta_xyl[ndim:(ndim+ncnst),0] lmd += alpha_hat*delta_xyl[(ndim+ncnst):alldim,0] rp = (1.0 - alpha_pri)*rp rd = (1.0 - alpha_dual)*rd+(alpha_pri - alpha_dual)*Gmat*delta_xyl[0:ndim,0] yield x #return x, y, lmd c = matrix([[-2.0], [-4.0]]) Dmat = matrix([[2.0, 0.0], [0.0, 2.0]]) Amat = matrix([[1.0, 1.0], [1.0, -1.0], [-3.0, 1.0]]) b = matrix([[0.5], [1.0], [-3.0]]) # 描画 from pylab import * ax = subplot(111, aspect='equal') x = arange(-3.0, 3.01, 0.15) y = arange(-3.0, 3.01, 0.15) X,Y = meshgrid(x, y) t = arange(-3.0, 3.01, 0.15) func = lambda x, y : 0.5*(Dmat[0,0]*x**2+Dmat[1,1]*y**2)+c[0,0]*x+c[1,0]*y const = [lambda x : -Amat[i,0]/Amat[i,1]*x + b[i,0]/Amat[i,1] for i in range(Amat.shape[0])] Z = func(X, Y) s = [const[i](t) for i in range(Amat.shape[0])] pcolor(X, Y, Z) for i in range(Amat.shape[0]): ax.plot(t,s[i],'k') for x in predictor_corrector(matrix([[0.0],[0.0]]), Dmat, c, Amat, b, tau=0.5, eps=1.0e-3, nroop=30): print x ax.plot([x[0,0]],[x[1,0]],'ro') axis([-3,3,-3,3]) ax.plot([x[0,0]],[x[1,0]],'yo') axis([-3,3,-3,3]) show()