84 lines
1.5 KiB
Matlab
84 lines
1.5 KiB
Matlab
% Script to perform SQP on a simple example from the SQP tutorial
|
|
%
|
|
% Problem:
|
|
% min(x) f(x) = (x2-2)^2 - x1^2
|
|
% s.t. c(x) = 4x1^2 + x2^2 - 1 =0
|
|
% state is x = [x1 x2]' , with dim(state) = 2
|
|
% constraint has dim p = 1
|
|
|
|
n = 2;
|
|
p = 1;
|
|
|
|
% initial conditions
|
|
x0 = [2; 4];
|
|
lam0 = -0.5;
|
|
x = x0; lam = lam0;
|
|
|
|
maxIt = 100;
|
|
|
|
X = x0; Lam = lam0;
|
|
Bi = eye(2);
|
|
|
|
for i=1:maxIt
|
|
i
|
|
x1 = x(1); x2 = x(2);
|
|
|
|
% evaluate functions
|
|
fx = (x2-2)^2 + x1^2;
|
|
cx = 4*x1^2 + x2^2 - 1;
|
|
|
|
% evaluate derivatives in x
|
|
dfx = [2*x1; 2*(x2-2)];
|
|
dcx = [8*x1; 2*x2];
|
|
dL = dfx - lam * dcx;
|
|
|
|
% update the hessian (BFGS)
|
|
if (i>1)
|
|
Bis = Bi*s;
|
|
y = dfx - prev_dfx;
|
|
Bi = Bi + (y*y')/(y'*s) - (Bis*Bis')/(s'*Bis);
|
|
end
|
|
prev_dfx = dfx;
|
|
|
|
% evaluate hessians in x
|
|
ddfx = diag([2, 2]);
|
|
ddcx = diag([8, 2]);
|
|
|
|
% construct and solve CQP subproblem
|
|
Bgn0 = dfx * dfx';
|
|
Bgn1 = dfx * dfx' - lam * dcx * dcx'; % GN approx 1
|
|
Bgn2 = dL * dL'; % GN approx 2
|
|
Ba = ddfx - lam * ddcx; % analytic hessians
|
|
|
|
B = Bi;
|
|
g = dfx;
|
|
h = -cx;
|
|
[delta lambda] = solveCQP(B, -dcx, -dcx', g, h);
|
|
|
|
% update
|
|
s = 0.1*delta;
|
|
x = x + s
|
|
lam = lambda
|
|
|
|
|
|
% save
|
|
X = [X x];
|
|
Lam = [Lam lam];
|
|
|
|
end
|
|
|
|
% verify
|
|
xstar = [0; 1];
|
|
lamstar = -1;
|
|
display(fx)
|
|
display(cx)
|
|
final_error = norm(x-xstar) + norm(lam-lamstar)
|
|
|
|
% draw
|
|
figure(1);
|
|
clf;
|
|
ezcontour('(x2-2)^2 + x1^2');
|
|
hold on;
|
|
ezplot('4*x1^2 + x2^2 - 1');
|
|
plot(X(1,:), X(2,:), 'r*-');
|