From 1dce90bd76af88cc11ecf2a6acbf268fb777e038 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 21 Apr 2010 20:13:11 +0000
Subject: [PATCH] Added BFGS to the sqp example for working sqp

---
 matlab/example/alex01_simple1.m | 82 +++++++++++++++++++++++++++++++++
 matlab/example/sqp_simple.m     | 54 ----------------------
 2 files changed, 82 insertions(+), 54 deletions(-)
 create mode 100644 matlab/example/alex01_simple1.m
 delete mode 100644 matlab/example/sqp_simple.m

diff --git a/matlab/example/alex01_simple1.m b/matlab/example/alex01_simple1.m
new file mode 100644
index 000000000..fa36f1e88
--- /dev/null
+++ b/matlab/example/alex01_simple1.m
@@ -0,0 +1,82 @@
+% 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]; 
+
+    % 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
+    dL = dfx - lam * dcx;
+    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.2*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*-');
diff --git a/matlab/example/sqp_simple.m b/matlab/example/sqp_simple.m
deleted file mode 100644
index 0f0063acb..000000000
--- a/matlab/example/sqp_simple.m
+++ /dev/null
@@ -1,54 +0,0 @@
-% 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)
\ No newline at end of file