Added SQP matlab examples

matlab/example/solveCQP.m

@@ -0,0 +1,16 @@
+function [delta lambda] = solveCQP(B, A, At, g, h)
+
+n = size(B,1);
+p = size(A,2);
+
+% form the KKT matrix system
+G = [B A; At zeros(p,p)];
+rhs = -[g; h];
+
+% solve with LDL
+[L D] = ldl(G);
+approx_error = norm(G - L*D*L'); %% verify error
+sol = L'\(D\(L\rhs));
+
+delta = sol(1:n);
+lambda = sol(n+1:size(sol));

matlab/example/sqp_simple.m

@@ -0,0 +1,54 @@
+% 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 = 1;
+
+for i=1:maxIt
+    
+    x1 = x(1); x2 = x(2);
+
+    % evaluate normal 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]; 
+    
+    % evaluate hessians in x
+    ddfx = diag([-2, 2]);
+    ddcx = diag([8, 2]);
+
+    % construct and solve CQP subproblem
+    Bgn = dfx * dfx' - lam * dcx * dcx' % GN approx
+    Ba = ddfx - lam * ddcx % analytic hessians
+    B = Ba;
+    g = dfx;
+    h = -cx;
+    [delta lambda] = solveCQP(B, -dcx, -dcx', g, h);
+
+    % update 
+    x = x + delta
+    lam = lambda
+
+end
+
+% verify
+xstar = [0; 1];
+lamstar = -1;
+display(fx)
+display(cx)
+final_error = norm(x-xstar) + norm(lam-lamstar)

matlab/example/testCQP.m

@@ -0,0 +1,16 @@
+% Test example for simple Constrained Quadratic Programming problem
+
+A = [-1    -1;
+     -2     1;
+      1    -1];
+At = A';
+B = 2*eye(3,3);
+
+b = [4; -2];
+g = zeros(3,1);
+
+[delta lambda] = solveCQP(B, A, At, g, b);
+
+expX = [2/7 10/7 -6/7]';
+
+state_error = norm(expX-delta)