diff --git a/.cproject b/.cproject
index c71ec5b6e..440f9e1a6 100644
--- a/.cproject
+++ b/.cproject
@@ -300,6 +300,7 @@
 <buildTargets>
 <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -307,6 +308,7 @@
 </target>
 <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -322,6 +324,7 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -337,7 +340,6 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -345,6 +347,7 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -352,7 +355,6 @@
 </target>
 <target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testGaussianConditional.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -360,6 +362,7 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -367,6 +370,7 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -374,7 +378,6 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -382,6 +385,7 @@
 </target>
 <target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -389,6 +393,7 @@
 </target>
 <target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -396,7 +401,6 @@
 </target>
 <target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -404,6 +408,7 @@
 </target>
 <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testPose3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -411,7 +416,6 @@
 </target>
 <target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVectorConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -419,7 +423,6 @@
 </target>
 <target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testPoint2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -427,6 +430,7 @@
 </target>
 <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testNonlinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -434,6 +438,7 @@
 </target>
 <target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>timeGaussianFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -441,6 +446,7 @@
 </target>
 <target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>timeGaussianFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -448,6 +454,7 @@
 </target>
 <target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGaussianBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -455,7 +462,6 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -463,6 +469,7 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -470,7 +477,6 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -478,6 +484,7 @@
 </target>
 <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testVector.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -485,6 +492,7 @@
 </target>
 <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testMatrix.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -492,7 +500,6 @@
 </target>
 <target name="testInference.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testInference.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -500,7 +507,6 @@
 </target>
 <target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testVSLAMGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -508,15 +514,21 @@
 </target>
 <target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testNonlinearEquality.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>testSQP.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -524,7 +536,6 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -532,7 +543,6 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 85a8a8aff..63d3e291b 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -113,7 +113,7 @@ headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
 headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 sources += NonlinearFactor.cpp 
 sources += NonlinearEquality.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP
 testNonlinearFactor_SOURCES      = $(example) testNonlinearFactor.cpp
 testNonlinearFactor_LDADD        = libgtsam.la
 testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
@@ -122,6 +122,8 @@ testNonlinearOptimizer_SOURCES   = $(example) testNonlinearOptimizer.cpp
 testNonlinearOptimizer_LDADD     = libgtsam.la
 testNonlinearEquality_SOURCES    = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD      = libgtsam.la
+testSQP_SOURCES                  = testSQP.cpp
+testSQP_LDADD                    = libgtsam.la
 
 # geometry
 sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp
new file mode 100644
index 000000000..41e0788db
--- /dev/null
+++ b/cpp/testSQP.cpp
@@ -0,0 +1,125 @@
+/*
+ * @file testSQP.cpp
+ * @brief demos of SQP using existing gtsam components
+ * @author Alex Cunningham
+ */
+
+#include <iostream>
+#include <cmath>
+#include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/map.hpp> // for insert
+#include <boost/foreach.hpp>
+#include <CppUnitLite/TestHarness.h>
+#include <GaussianFactorGraph.h>
+#include <Ordering.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::assign;
+
+// trick from some reading group
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+/**
+ * This example uses a nonlinear objective function and
+ * nonlinear equality constraint.  The formulation is actually
+ * the Choleski form that creates the full Hessian explicitly,
+ * which should really be avoided with our QR-based machinery
+ */
+TEST (SQP, problem1 ) {
+	bool verbose = false;
+	// use a nonlinear function of f(x) = x^2+y^2
+	// nonlinear equality constraint: g(x) = x^2-5-y=0
+	// Lagrangian: f(x) + lam*g(x)
+
+	// state structure: [x y lam]
+	VectorConfig init, state;
+	init.insert("x", Vector_(1, 1.0));
+	init.insert("y", Vector_(1, 1.0));
+	init.insert("lam", Vector_(1, 1.0));
+	state = init;
+
+	if (verbose) init.print("Initial State");
+
+	// loop until convergence
+	int maxIt = 50;
+	for (int i = 0; i<maxIt; ++i) {
+		if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
+
+		// extract the states
+		double x, y, lam;
+		x = state["x"](0);
+		y = state["y"](0);
+		lam = state["lam"](0);
+
+		// calculate the components
+		Matrix H1, H2, gradG;
+		Vector gradL, gx;
+
+		// hessian of lagrangian function, in two columns:
+		H1 = Matrix_(2,1,
+				2.0+2.0*lam,
+				0.0);
+		H2 = Matrix_(2,1,
+				0.0,
+				2.0);
+
+		// deriviative of lagrangian function
+		gradL = Vector_(2,
+				2.0*x*(1+lam),
+				2.0*y-lam);
+
+		// constraint derivatives
+		gradG = Matrix_(2,1,
+				2.0*x,
+				0.0);
+
+		// constraint value
+		gx = Vector_(1,
+				x*x-5-y);
+
+		// create a factor for the states
+		GaussianFactor::shared_ptr f1(new
+				GaussianFactor("x", H1, "y", H2, "lam", gradG, gradL, 1.0));
+
+		// create a factor for the lagrange multiplier
+		GaussianFactor::shared_ptr f2(new
+				GaussianFactor("x", -sub(gradG, 0, 1, 0, 1),
+							   "y", -sub(gradG, 1, 2, 0, 1), -gx, 0.0));
+
+		// construct graph
+		GaussianFactorGraph fg;
+		fg.push_back(f1);
+		fg.push_back(f2);
+		if (verbose) fg.print("Graph");
+
+		// solve
+		Ordering ord;
+		ord += "x", "y", "lam";
+		VectorConfig delta = fg.optimize(ord);
+		if (verbose) delta.print("Delta");
+
+		// update initial estimate
+		double gain = 0.3;
+		VectorConfig newState;
+		newState.insert("x", state["x"]-gain*delta["x"]);
+		newState.insert("y", state["y"]-gain*delta["y"]);
+		newState.insert("lam", state["lam"]-gain*delta["lam"]);
+		state = newState;
+
+		if (verbose) state.print("Updated State");
+	}
+
+	// verify that it converges to the nearest optimal point
+	VectorConfig expected;
+	expected.insert("x", Vector_(1, 2.12));
+	expected.insert("y", Vector_(1, -0.5));
+	CHECK(assert_equal(state["x"], expected["x"], 1e-2));
+	CHECK(assert_equal(state["y"], expected["y"], 1e-2));
+
+}
+	
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */