In [1]:
include("plotregion.jl")
using Plots
plotlyjs()
ezcontour!(x, y, f) = begin
X = repmat(x', length(y), 1)
Y = repmat(y, 1, length(x))
# Evaluate each f(x, y)
Z = map((x,y) -> (f([x,y])), X, Y)
plot!(x, y, Z, st=:contour)
end
Out[1]:
In [13]:
#
# min 1/2 x'*G*x + c'*x
# s.t. Ax = b
# Bx >= d
type QPProblem
G::Matrix{Float64}
c::Vector{Float64}
A::Matrix{Float64}
b::Vector{Float64}
B::Matrix{Float64}
d::Vector{Float64}
end
type QPActiveState
x::Vector{Float64}
W::Set{Int}
flag::Bool
fval::Float64
P::QPProblem
end
"""A very simple QP KKT solver"""
function qp_kkt_solve(P::QPProblem, W::Set{Int}, x::Vector{Float64})
Aeq = [P.A; P.B[collect(W),:]];
n = size(P.G,1)
m1 = size(P.A,1)
m2 = size(Aeq,1)
g = P.G*x + P.c
G = P.G
sys = [G Aeq'; Aeq zeros(m2,m2)]
rhs = [g; zeros(m2,1)]
sol = sys\rhs
p = -sol[1:n]
lam = sol[n+1:n+m1]
mu = zeros(size(P.B,1))
mu[collect(W)] = sol[n+m1+1:n+m2]
return p, lam, mu
end
function fval(P::QPProblem, x::Vector{Float64})
if norm(P.A*x - P.b) >= 1.0e-8*length(P.b)
return Inf
elseif any(P.B*x - P.d .>= -1.0e-8)
return Inf
else
return 0.5*dot(P.G*x, x) + dot(P.c, x)
end
end
# Setup an active set method for
# min 1/2 x'*G*x + c'*x
# s.t. Ax = b
# Bx >= d
# with W as the initial working set
function qp_init(G,c,A,b,B,d,W, x0)
P = QPProblem(G,c,A,b,B,d)
W = Set{Int}(W) # make a copy
#p, lam, mu = kkt_solve(P, W, x0)
return QPActiveState(x0, W, false, fval(P,x0), P)
end
# Take a step for an active set method for
# min 1/2 x'*G*x + c'*x
# s.t. Ax = b
# Bx >= d
function qp_activeset_step!(S::QPActiveState)
p, lam, mu = qp_kkt_solve(S.P, S.W, S.x)
if norm(p) <= 100*eps(1.0)
# we are done, or update working set
xn = S.x
if all(mu .>= -100*eps(1.0))
S.flag = true # solved it!
else
j = findmin(mu)[2]
delete!(S.W,j)
end
else
# we need to move
# find the first constraint we hit that isn't
# in the working set
alpha = 1.0
ind = 0
for i = 1:size(S.P.B,1)
# for each inequal constraint
if i in S.W
# do nothing
else
bi = S.P.B[i,:]
di = S.P.d[i]
if dot(bi,p) < -100*eps(1.0) # then we might hit a constraint
testalpha = (di - dot(bi,S.x))/(dot(bi,p))
if testalpha <= alpha
alpha = testalpha
ind = i
end
end
end
end
xn = S.x + alpha*p
@show xn, ind
if ind > 0
push!(S.W,ind) # add ind to the working set
end
end
S.x = xn
S.fval = fval(S.P, S.x)
return S
end
Out[13]:
In [3]:
n = 2
G = eye(n)
c = zeros(n)
c[1] = 3; c[2] = -5
#f = @(x) 0.5*(x'*G*x) + c'*x
A = [-2 1; -1 2; 1 0; -0.5 -1]
b = [2; 7; 3; -1.0]
# We used Matlab to determine the vertices of the feasible polytope
plot(Shape([-0.4,3,3,1],[1.2,-0.5,5,4]),fillalpha=0.5, fillcolor="grey", label="")
ezcontour!(-1.0:0.25:3.0,-1.0:0.25:5.0,x -> 0.5*dot(G*x,x) + dot(c,x))
Out[3]:
In [14]:
x0 = [3.0;0.0]
W = [3]
S = qp_init(G,c,zeros(0,n),zeros(0),-A,-b,W,x0)
@show qp_activeset_step!(S)
@show qp_activeset_step!(S)
@show qp_activeset_step!(S)
Out[14]:
In [74]:
Out[74]:
In [15]:
# Animate!
plot(Shape([-0.4,3,3,1],[1.2,-0.5,5,4]),fillalpha=0.5, fillcolor="grey", label="")
ezcontour!(-1.0:0.25:3.0,-1.0:0.25:5.0,x -> 0.5*dot(G*x,x) + dot(c,x))
x0 = [3.0;0.0]
W = [3]
S = qp_init(G,c,zeros(0,n),zeros(0),-A,-b,W,x0)
for i=1:10
xold = S.x
scatter!([S.x[1]], [S.x[2]], label="")
qp_activeset_step!(S)
xnew = S.x
plot!([xold[1],xnew[1]], [xold[2],xnew[2]], marker=:none, label="")
if S.flag == true
break
end
end
if S.flag == true
scatter!([S.x[1]], [S.x[2]], label="", markersize=18)
end
plot!()
Out[15]:
In [23]:
# Show a problem from Figure 16.3 in Nocedal and Wright
n = 2
G = 2*eye(n)
c = [-2.0; -5]
A = [1.0 -2; -1 -2; -1 2; 1 0; 0 1]
b = [-2.0; -6; -2; 0; 0]
# this is Ax >= b
# plotregion uses Ax = b, x >=0
PlotRegion.plotregion([A -eye(size(A,1))],b)
ezcontour!(0:0.25:5.0,0:0.25:5.0,x -> 0.5*dot(G*x,x) + dot(c,x))
Out[23]:
In [28]:
# Animate!
PlotRegion.plotregion([A -eye(size(A,1))],b)
ezcontour!(0:0.25:5.0,0:0.25:5.0,x -> 0.5*dot(G*x,x) + dot(c,x))
x0 = [2.0;0.0]
W = [3,5]
S = qp_init(G,c,zeros(0,n),zeros(0),A,b,W,x0)
for i=1:10
xold = S.x
scatter!([S.x[1]], [S.x[2]], label="")
qp_activeset_step!(S)
xnew = S.x
plot!([xold[1],xnew[1]], [xold[2],xnew[2]], marker=:none, label="")
if S.flag == true
break
end
end
if S.flag == true
scatter!([S.x[1]], [S.x[2]], label="", markersize=18)
end
plot!()
Out[28]:
In [ ]: