diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h
new file mode 100644
index 000000000..18dc07aec
--- /dev/null
+++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h
@@ -0,0 +1,290 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file     ActiveSetSolver-inl.h
+ * @brief    Implmentation of ActiveSetSolver.
+ * @author   Ivan Dario Jimenez
+ * @author   Duy Nguyen Ta
+ * @date     2/11/16
+ */
+
+#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
+
+/******************************************************************************/
+// Convenient macros to reduce syntactic noise. undef later.
+#define Template template <class PROBLEM, class POLICY, class INITSOLVER>
+#define This ActiveSetSolver<PROBLEM, POLICY, INITSOLVER>
+
+/******************************************************************************/
+
+namespace gtsam {
+
+/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
+ * If some inactive inequality constraints complain about the full step (alpha = 1),
+ * we have to adjust alpha to stay within the inequality constraints' feasible regions.
+ *
+ * For each inactive inequality j:
+ *  - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
+ *  - We want: aj'*(xk + alpha*p) - bj <= 0
+ *  - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
+ *  it's good!
+ *  - We only care when aj'*p > 0. In this case, we need to choose alpha so that
+ *  aj'*xk + alpha*aj'*p - bj <= 0  --> alpha <= (bj - aj'*xk) / (aj'*p)
+ *  We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
+ *
+ * We want the minimum of all those alphas among all inactive inequality.
+ */
+Template boost::tuple<double, int> This::computeStepSize(
+    const InequalityFactorGraph& workingSet, const VectorValues& xk,
+    const VectorValues& p, const double& maxAlpha) const {
+  double minAlpha = maxAlpha;
+  int closestFactorIx = -1;
+  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
+    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
+    double b = factor->getb()[0];
+    // only check inactive factors
+    if (!factor->active()) {
+      // Compute a'*p
+      double aTp = factor->dotProductRow(p);
+
+      // Check if  a'*p >0. Don't care if it's not.
+      if (aTp <= 0)
+        continue;
+
+      // Compute a'*xk
+      double aTx = factor->dotProductRow(xk);
+
+      // alpha = (b - a'*xk) / (a'*p)
+      double alpha = (b - aTx) / aTp;
+      // We want the minimum of all those max alphas
+      if (alpha < minAlpha) {
+        closestFactorIx = factorIx;
+        minAlpha = alpha;
+      }
+    }
+  }
+  return boost::make_tuple(minAlpha, closestFactorIx);
+}
+
+/******************************************************************************/
+/*
+ * The goal of this function is to find currently active inequality constraints
+ * that violate the condition to be active. The one that violates the condition
+ * the most will be removed from the active set. See Nocedal06book, pg 469-471
+ *
+ * Find the BAD active inequality that pulls x strongest to the wrong direction
+ * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
+ *
+ * For active inequality constraints (those that are enforced as equality constraints
+ * in the current working set), we want lambda < 0.
+ * This is because:
+ *   - From the Lagrangian L = f - lambda*c, we know that the constraint force
+ *     is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
+ *     on the constraint surface, the constraint force has to balance out with
+ *     other unconstrained forces that are pulling x towards the unconstrained
+ *     minimum point. The other unconstrained forces are pulling x toward (-\grad f),
+ *     hence the constraint force has to be exactly \grad f, so that the total
+ *     force is 0.
+ *   - We also know that  at the constraint surface c(x)=0, \grad c points towards + (>= 0),
+ *     while we are solving for - (<=0) constraint.
+ *   - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
+ *     i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
+ *     That means we want lambda < 0.
+ *   - This is because when the constrained force pulls x towards the infeasible region (+),
+ *     the unconstrained force is pulling x towards the opposite direction into
+ *     the feasible region (again because the total force has to be 0 to make x stay still)
+ *     So we can drop this constraint to have a lower error but feasible solution.
+ *
+ * In short, active inequality constraints with lambda > 0 are BAD, because they
+ * violate the condition to be active.
+ *
+ * And we want to remove the worst one with the largest lambda from the active set.
+ *
+ */
+Template int This::identifyLeavingConstraint(
+    const InequalityFactorGraph& workingSet,
+    const VectorValues& lambdas) const {
+  int worstFactorIx = -1;
+  // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
+  // inactive or a good inequality constraint, so we don't care!
+  double maxLambda = 0.0;
+  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
+    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
+    if (factor->active()) {
+      double lambda = lambdas.at(factor->dualKey())[0];
+      if (lambda > maxLambda) {
+        worstFactorIx = factorIx;
+        maxLambda = lambda;
+      }
+    }
+  }
+  return worstFactorIx;
+}
+
+//******************************************************************************
+Template JacobianFactor::shared_ptr This::createDualFactor(
+    Key key, const InequalityFactorGraph& workingSet,
+    const VectorValues& delta) const {
+  // Transpose the A matrix of constrained factors to have the jacobian of the
+  // dual key
+  TermsContainer Aterms = collectDualJacobians<LinearEquality>(
+      key, problem_.equalities, equalityVariableIndex_);
+  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
+      key, workingSet, inequalityVariableIndex_);
+  Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
+                AtermsInequalities.end());
+
+  // Collect the gradients of unconstrained cost factors to the b vector
+  if (Aterms.size() > 0) {
+    Vector b = problem_.costGradient(key, delta);
+    // to compute the least-square approximation of dual variables
+    return boost::make_shared<JacobianFactor>(Aterms, b);
+  } else {
+    return boost::make_shared<JacobianFactor>();
+  }
+}
+
+/******************************************************************************/
+/*  This function will create a dual graph that solves for the
+ *  lagrange multipliers for the current working set.
+ *  You can use lagrange multipliers as a necessary condition for optimality.
+ *  The factor graph that is being solved is f' = -lambda * g'
+ *  where f is the optimized function and g is the function resulting from
+ *  aggregating the working set.
+ *  The lambdas give you information about the feasibility of a constraint.
+ *  if lambda < 0  the constraint is Ok
+ *  if lambda = 0  you are on the constraint
+ *  if lambda > 0  you are violating the constraint.
+ */
+Template GaussianFactorGraph::shared_ptr This::buildDualGraph(
+    const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
+  GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
+  for (Key key : constrainedKeys_) {
+    // Each constrained key becomes a factor in the dual graph
+    JacobianFactor::shared_ptr dualFactor =
+        createDualFactor(key, workingSet, delta);
+    if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
+  }
+  return dualGraph;
+}
+
+//******************************************************************************
+Template GaussianFactorGraph
+This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
+                        const VectorValues& xk) const {
+  GaussianFactorGraph workingGraph;
+  workingGraph.push_back(POLICY::buildCostFunction(problem_, xk));
+  workingGraph.push_back(problem_.equalities);
+  for (const LinearInequality::shared_ptr& factor : workingSet)
+    if (factor->active()) workingGraph.push_back(factor);
+  return workingGraph;
+}
+
+//******************************************************************************
+Template typename This::State This::iterate(
+    const typename This::State& state) const {
+  // Algorithm 16.3 from Nocedal06book.
+  // Solve with the current working set eqn 16.39, but instead of solving for p
+  // solve for x
+  GaussianFactorGraph workingGraph =
+      buildWorkingGraph(state.workingSet, state.values);
+  VectorValues newValues = workingGraph.optimize();
+  // If we CAN'T move further
+  // if p_k = 0 is the original condition, modified by Duy to say that the state
+  // update is zero.
+  if (newValues.equals(state.values, 1e-7)) {
+    // Compute lambda from the dual graph
+    GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
+        newValues);
+    VectorValues duals = dualGraph->optimize();
+    int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
+    // If all inequality constraints are satisfied: We have the solution!!
+    if (leavingFactor < 0) {
+      return State(newValues, duals, state.workingSet, true,
+          state.iterations + 1);
+    } else {
+      // Inactivate the leaving constraint
+      InequalityFactorGraph newWorkingSet = state.workingSet;
+      newWorkingSet.at(leavingFactor)->inactivate();
+      return State(newValues, duals, newWorkingSet, false,
+          state.iterations + 1);
+    }
+  } else {
+    // If we CAN make some progress, i.e. p_k != 0
+    // Adapt stepsize if some inactive constraints complain about this move
+    double alpha;
+    int factorIx;
+    VectorValues p = newValues - state.values;
+    boost::tie(alpha, factorIx) = // using 16.41
+        computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha);
+    // also add to the working set the one that complains the most
+    InequalityFactorGraph newWorkingSet = state.workingSet;
+    if (factorIx >= 0)
+      newWorkingSet.at(factorIx)->activate();
+    // step!
+    newValues = state.values + alpha * p;
+    return State(newValues, state.duals, newWorkingSet, false,
+        state.iterations + 1);
+  }
+}
+
+//******************************************************************************
+Template InequalityFactorGraph This::identifyActiveConstraints(
+    const InequalityFactorGraph& inequalities,
+    const VectorValues& initialValues, const VectorValues& duals,
+    bool useWarmStart) const {
+  InequalityFactorGraph workingSet;
+  for (const LinearInequality::shared_ptr& factor : inequalities) {
+    LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
+    if (useWarmStart && duals.size() > 0) {
+      if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
+      else workingFactor->inactivate();
+    } else {
+      double error = workingFactor->error(initialValues);
+      // Safety guard. This should not happen unless users provide a bad init
+      if (error > 0) throw InfeasibleInitialValues();
+      if (fabs(error) < 1e-7)
+        workingFactor->activate();
+      else
+        workingFactor->inactivate();
+    }
+    workingSet.push_back(workingFactor);
+  }
+  return workingSet;
+}
+
+//******************************************************************************
+Template std::pair<VectorValues, VectorValues> This::optimize(
+    const VectorValues& initialValues, const VectorValues& duals,
+    bool useWarmStart) const {
+  // Initialize workingSet from the feasible initialValues
+  InequalityFactorGraph workingSet = identifyActiveConstraints(
+      problem_.inequalities, initialValues, duals, useWarmStart);
+  State state(initialValues, duals, workingSet, false, 0);
+
+  /// main loop of the solver
+  while (!state.converged) state = iterate(state);
+
+  return std::make_pair(state.values, state.duals);
+}
+
+//******************************************************************************
+Template std::pair<VectorValues, VectorValues> This::optimize() const {
+  INITSOLVER initSolver(problem_);
+  VectorValues initValues = initSolver.solve();
+  return optimize(initValues);
+}
+
+}
+
+#undef Template
+#undef This
\ No newline at end of file
diff --git a/gtsam_unstable/linear/ActiveSetSolver.cpp b/gtsam_unstable/linear/ActiveSetSolver.cpp
deleted file mode 100644
index 6736de31a..000000000
--- a/gtsam_unstable/linear/ActiveSetSolver.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/**
- * @file     ActiveSetSolver.cpp
- * @brief    Implmentation of ActiveSetSolver.
- * @author   Ivan Dario Jimenez
- * @author   Duy Nguyen Ta
- * @date     2/11/16
- */
-
-#include <gtsam_unstable/linear/ActiveSetSolver.h>
-
-namespace gtsam {
-
-/*
- * The goal of this function is to find currently active inequality constraints
- * that violate the condition to be active. The one that violates the condition
- * the most will be removed from the active set. See Nocedal06book, pg 469-471
- *
- * Find the BAD active inequality that pulls x strongest to the wrong direction
- * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
- *
- * For active inequality constraints (those that are enforced as equality constraints
- * in the current working set), we want lambda < 0.
- * This is because:
- *   - From the Lagrangian L = f - lambda*c, we know that the constraint force
- *     is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
- *     on the constraint surface, the constraint force has to balance out with
- *     other unconstrained forces that are pulling x towards the unconstrained
- *     minimum point. The other unconstrained forces are pulling x toward (-\grad f),
- *     hence the constraint force has to be exactly \grad f, so that the total
- *     force is 0.
- *   - We also know that  at the constraint surface c(x)=0, \grad c points towards + (>= 0),
- *     while we are solving for - (<=0) constraint.
- *   - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
- *     i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
- *     That means we want lambda < 0.
- *   - This is because when the constrained force pulls x towards the infeasible region (+),
- *     the unconstrained force is pulling x towards the opposite direction into
- *     the feasible region (again because the total force has to be 0 to make x stay still)
- *     So we can drop this constraint to have a lower error but feasible solution.
- *
- * In short, active inequality constraints with lambda > 0 are BAD, because they
- * violate the condition to be active.
- *
- * And we want to remove the worst one with the largest lambda from the active set.
- *
- */
-int ActiveSetSolver::identifyLeavingConstraint(
-    const InequalityFactorGraph& workingSet,
-    const VectorValues& lambdas) const {
-  int worstFactorIx = -1;
-// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is
-// either
-// inactive or a good inequality constraint, so we don't care!
-  double maxLambda = 0.0;
-  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
-    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
-    if (factor->active()) {
-      double lambda = lambdas.at(factor->dualKey())[0];
-      if (lambda > maxLambda) {
-        worstFactorIx = factorIx;
-        maxLambda = lambda;
-      }
-    }
-  }
-  return worstFactorIx;
-}
-
-/*  This function will create a dual graph that solves for the
- *  lagrange multipliers for the current working set.
- *  You can use lagrange multipliers as a necessary condition for optimality.
- *  The factor graph that is being solved is f' = -lambda * g'
- *  where f is the optimized function and g is the function resulting from
- *  aggregating the working set.
- *  The lambdas give you information about the feasibility of a constraint.
- *  if lambda < 0  the constraint is Ok
- *  if lambda = 0  you are on the constraint
- *  if lambda > 0  you are violating the constraint.
- */
-GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
-    const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
-  GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
-  for (Key key : constrainedKeys_) {
-    // Each constrained key becomes a factor in the dual graph
-    JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet,
-        delta);
-    if (!dualFactor->empty())
-      dualGraph->push_back(dualFactor);
-  }
-  return dualGraph;
-}
-
-/*
- * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
- *
- *    @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
- *            is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
- *            This constraint will be added to the working set and become active
- *            in the next iteration.
- */
-boost::tuple<double, int> ActiveSetSolver::computeStepSize(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk,
-    const VectorValues& p, const double& startAlpha) const {
-  double minAlpha = startAlpha;
-  int closestFactorIx = -1;
-  for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
-    const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
-    double b = factor->getb()[0];
-    // only check inactive factors
-    if (!factor->active()) {
-      // Compute a'*p
-      double aTp = factor->dotProductRow(p);
-
-      // Check if  a'*p >0. Don't care if it's not.
-      if (aTp <= 0)
-        continue;
-
-      // Compute a'*xk
-      double aTx = factor->dotProductRow(xk);
-
-      // alpha = (b - a'*xk) / (a'*p)
-      double alpha = (b - aTx) / aTp;
-      // We want the minimum of all those max alphas
-      if (alpha < minAlpha) {
-        closestFactorIx = factorIx;
-        minAlpha = alpha;
-      }
-    }
-  }
-  return boost::make_tuple(minAlpha, closestFactorIx);
-}
-
-}
diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h
index 4519ad5a8..3477cf84f 100644
--- a/gtsam_unstable/linear/ActiveSetSolver.h
+++ b/gtsam_unstable/linear/ActiveSetSolver.h
@@ -1,6 +1,17 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     ActiveSetSolver.h
- * @brief    Abstract class above for solving problems with the abstract set method.
+ * @brief    Active set method for solving LP, QP problems
  * @author   Ivan Dario Jimenez
  * @author   Duy Nguyen Ta
  * @date     1/25/16
@@ -14,26 +25,101 @@
 namespace gtsam {
 
 /**
- * This is a base class for all implementations of the active set algorithm for solving 
- * Programming problems. It provides services and variables all active set implementations
- * share.
+ * This class implementations the active set algorithm for solving convex
+ * Programming problems.
+ *
+ * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or 
+ *                 QP (quadratic program).
+ * @tparam POLICY specific detail policy tailored for the particular program
+ * @tparam INITSOLVER Solver for an initial feasible solution of this problem.
  */
+template <class PROBLEM, class POLICY, class INITSOLVER>
 class ActiveSetSolver {
+public:
+  /**
+   * This struct contains the state information for a single iteration of an
+   * active set method iteration.
+   */
+  struct State {
+    VectorValues values;  //!< current best values at each step
+    VectorValues duals;   //!< current values of dual variables at each step
+    InequalityFactorGraph workingSet; /*!< keep track of current active/inactive
+                                           inequality constraints */
+    bool converged;     //!< True if the algorithm has converged to a solution
+    size_t iterations;  //!< number of iterations. Incremented at the end of
+                        //each iter.
+
+    /// Default constructor
+    State()
+        : values(), duals(), workingSet(), converged(false), iterations(0) {}
+
+    /// Constructor with initial values
+    State(const VectorValues& initialValues, const VectorValues& initialDuals,
+          const InequalityFactorGraph& initialWorkingSet, bool _converged,
+          size_t _iterations)
+        : values(initialValues),
+          duals(initialDuals),
+          workingSet(initialWorkingSet),
+          converged(_converged),
+          iterations(_iterations) {}
+  };
+
 protected:
-  KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
+  const PROBLEM& problem_;  //!< the particular [convex] problem to solve
   VariableIndex equalityVariableIndex_,
-      inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
+      inequalityVariableIndex_;  /*!< index to corresponding factors to build
+                                 dual graphs */
+  KeySet constrainedKeys_;  /*!< all constrained keys, will become factors in
+                                 dual graphs */
+
+  /// Vector of key matrix pairs. Matrices are usually the A term for a factor.
+  typedef std::vector<std::pair<Key, Matrix> > TermsContainer; 
 
 public:
-  typedef std::vector<std::pair<Key, Matrix> > TermsContainer; //!< vector of key matrix pairs
-  //Matrices are usually the A term for a factor.
+  /// Constructor
+  ActiveSetSolver(const PROBLEM& problem) :  problem_(problem) {
+    equalityVariableIndex_ = VariableIndex(problem_.equalities);
+    inequalityVariableIndex_ = VariableIndex(problem_.inequalities);
+    constrainedKeys_ = problem_.equalities.keys();
+    constrainedKeys_.merge(problem_.inequalities.keys());
+  }
+
   /**
-   * Creates a dual factor from the current workingSet and the key of the
-   * the variable used to created the dual factor.
+   * Optimize with provided initial values
+   * For this version, it is the responsibility of the caller to provide
+   * a feasible initial value, otherwise, an exception will be thrown.
+   * @return a pair of <primal, dual> solutions
    */
-  virtual JacobianFactor::shared_ptr createDualFactor(Key key,
-      const InequalityFactorGraph& workingSet,
-      const VectorValues& delta) const = 0;
+  std::pair<VectorValues, VectorValues> optimize(
+      const VectorValues& initialValues,
+      const VectorValues& duals = VectorValues(),
+      bool useWarmStart = false) const;
+
+  /**
+   * For this version the caller will not have to provide an initial value
+   * @return a pair of <primal, dual> solutions
+   */
+  std::pair<VectorValues, VectorValues> optimize() const;
+
+protected:
+  /**
+   * Compute minimum step size alpha to move from the current point @p xk to the 
+   * next feasible point along a direction @p p:  x' = xk + alpha*p, 
+   * where alpha \in [0,maxAlpha]. 
+   * 
+   * For QP, maxAlpha = 1. For LP: maxAlpha = Inf.
+   *
+   * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex
+   * is the closest inactive inequality constraint that blocks xk to move 
+   * further and that has the minimum alpha, or (-1, maxAlpha) if there is no 
+   * such inactive blocking constraint.
+   * 
+   * If there is a blocking constraint, the closest one will be added to the 
+   * working set and become active in the next iteration.
+   */
+  boost::tuple<double, int> computeStepSize(
+      const InequalityFactorGraph& workingSet, const VectorValues& xk,
+      const VectorValues& p, const double& maxAlpha) const;
 
   /**
    * Finds the active constraints in the given factor graph and returns the 
@@ -59,35 +145,46 @@ public:
     }
     return Aterms;
   }
-  /**
-   * Identifies active constraints that shouldn't be active anymore.
-   */
-  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
-      const VectorValues& lambdas) const;
 
   /**
-   * Builds a dual graph from the current working set.
+   * Creates a dual factor from the current workingSet and the key of the
+   * the variable used to created the dual factor.
    */
+  JacobianFactor::shared_ptr createDualFactor(
+    Key key, const InequalityFactorGraph& workingSet,
+    const VectorValues& delta) const;
+
+public: /// Just for testing...
+
+  /// Builds a dual graph from the current working set.
   GaussianFactorGraph::shared_ptr buildDualGraph(
       const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
 
-protected:
   /**
-   * Protected constructor because this class doesn't have any meaning without
-   * a concrete Programming problem to solve.
+   * Build a working graph of cost, equality and active inequality constraints
+   * to solve at each iteration.
+   * @param  workingSet the collection of all cost and constrained factors
+   * @param  xk   current solution, used to build a special quadratic cost in LP
+   * @return      a new better solution
    */
-  ActiveSetSolver() :
-      constrainedKeys_() {
-  }
+  GaussianFactorGraph buildWorkingGraph(
+      const InequalityFactorGraph& workingSet,
+      const VectorValues& xk = VectorValues()) const;
+
+  /// Iterate 1 step, return a new state with a new workingSet and values
+  State iterate(const State& state) const;
+
+  /// Identify active constraints based on initial values.
+  InequalityFactorGraph identifyActiveConstraints(
+      const InequalityFactorGraph& inequalities,
+      const VectorValues& initialValues,
+      const VectorValues& duals = VectorValues(),
+      bool useWarmStart = false) const;
+
+  /// Identifies active constraints that shouldn't be active anymore.
+  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
+      const VectorValues& lambdas) const;
 
-  /**
-   * Computes the distance to move from the current point being examined to the next 
-   * location to be examined by the graph. This should only be used where there are less 
-   * than two constraints active.
-   */
-  boost::tuple<double, int> computeStepSize(
-      const InequalityFactorGraph& workingSet, const VectorValues& xk,
-      const VectorValues& p, const double& startAlpha) const;
 };
 
 /**
@@ -106,3 +203,5 @@ Key maxKey(const PROBLEM& problem) {
 }
 
 } // namespace gtsam
+
+#include <gtsam_unstable/linear/ActiveSetSolver-inl.h>
\ No newline at end of file
diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h
index 7c08584c2..60adb872e 100644
--- a/gtsam_unstable/linear/InfeasibleInitialValues.h
+++ b/gtsam_unstable/linear/InfeasibleInitialValues.h
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file InfeasibleInitialValues.h
  * @brief Exception thrown when given Infeasible Initial Values.
diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
index d3a5a14f1..f7c5a5c79 100644
--- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
+++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     InfeasibleOrUnboundedProblem.h
  * @brief    Throw when the problem is either infeasible or unbounded
@@ -5,6 +16,8 @@
  * @date     1/24/16
  */
 
+#pragma once 
+
 namespace gtsam {
 
 class InfeasibleOrUnboundedProblem: public ThreadsafeException<
diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp
index 890803082..8c3df3132 100644
--- a/gtsam_unstable/linear/LPInitSolver.cpp
+++ b/gtsam_unstable/linear/LPInitSolver.cpp
@@ -19,6 +19,7 @@
 
 #include <gtsam_unstable/linear/LPInitSolver.h>
 #include <gtsam_unstable/linear/LPSolver.h>
+#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
 
 namespace gtsam {
 
diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h
index a14e8e9f8..5297f3f95 100644
--- a/gtsam_unstable/linear/LPInitSolver.h
+++ b/gtsam_unstable/linear/LPInitSolver.h
@@ -21,7 +21,7 @@
 
 #include <gtsam_unstable/linear/LP.h>
 #include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
-#include <gtsam_unstable/linear/QPSolver.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <CppUnitLite/Test.h>
 
 namespace gtsam {
diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp
index 1cc409e22..c1319a5ec 100644
--- a/gtsam_unstable/linear/LPSolver.cpp
+++ b/gtsam_unstable/linear/LPSolver.cpp
@@ -18,199 +18,8 @@
  */
 
 #include <gtsam_unstable/linear/LPSolver.h>
-#include <gtsam_unstable/linear/LPInitSolver.h>
-#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 
 namespace gtsam {
-//******************************************************************************
-LPSolver::LPSolver(const LP &lp) :
-    lp_(lp) {
-  equalityVariableIndex_ = VariableIndex(lp_.equalities);
-  inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
-  constrainedKeys_ = lp_.equalities.keys();
-  constrainedKeys_.merge(lp_.inequalities.keys());
-}
-
-//******************************************************************************
-GaussianFactorGraph LPSolver::buildCostFunction(const VectorValues &xk) const {
-  GaussianFactorGraph graph;
-  for (LinearCost::const_iterator it = lp_.cost.begin(); it != lp_.cost.end();
-       ++it) {
-    size_t dim = lp_.cost.getDim(it);
-    Vector b = xk.at(*it) - lp_.cost.getA(it).transpose();  // b = xk-g
-    graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
-  }
-
-  KeySet allKeys = lp_.inequalities.keys();
-  allKeys.merge(lp_.equalities.keys());
-  allKeys.merge(KeySet(lp_.cost.keys()));
-  // Add corresponding factors for all variables that are not explicitly in the
-  // cost function. Gradients of the cost function wrt to these variables are 
-  // zero (g=0), so b=xk
-  if (lp_.cost.keys().size() != allKeys.size()) {
-    KeySet difference;
-    std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
-        lp_.cost.end(), std::inserter(difference, difference.end()));
-    for (Key k : difference) {
-      size_t dim = lp_.constrainedKeyDimMap().at(k);
-      graph.push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
-    }
-  }
-  return graph;
-}
-
-//******************************************************************************
-GaussianFactorGraph LPSolver::buildWorkingGraph(
-    const InequalityFactorGraph &workingSet, const VectorValues &xk) const {
-  GaussianFactorGraph workingGraph;
-  // || X - Xk + g ||^2
-  workingGraph.push_back(buildCostFunction(xk));
-  workingGraph.push_back(lp_.equalities);
-  for (const LinearInequality::shared_ptr &factor : workingSet) {
-    if (factor->active()) workingGraph.push_back(factor);
-  }
-  return workingGraph;
-}
-
-//******************************************************************************
-LPState LPSolver::iterate(const LPState &state) const {
-  // Solve with the current working set
-  // LP: project the objective neg. gradient to the constraint's null space
-  // to find the direction to move
-  GaussianFactorGraph workingGraph =
-      buildWorkingGraph(state.workingSet, state.values);
-  VectorValues newValues = workingGraph.optimize();
-
-  // If we CAN'T move further
-  // LP: projection on the constraints' nullspace is zero: we are at a vertex
-  if (newValues.equals(state.values, 1e-7)) {
-    // Find and remove the bad inequality constraint by computing its lambda
-    // Compute lambda from the dual graph
-    // LP: project the objective's gradient onto each constraint gradient to
-    // obtain the dual scaling factors
-    //  is it true??
-    GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
-        newValues);
-    VectorValues duals = dualGraph->optimize();
-    // LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
-    int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
-    // If all inequality constraints are satisfied: We have the solution!!
-    if (leavingFactor < 0) {
-      // TODO If we still have infeasible equality constraints: the problem is
-      // over-constrained. No solution!
-      // ...
-      return LPState(newValues, duals, state.workingSet, true,
-          state.iterations + 1);
-    } else {
-      // Inactivate the leaving constraint
-      // LP: remove the bad ineq constraint out of the working set
-      InequalityFactorGraph newWorkingSet = state.workingSet;
-      newWorkingSet.at(leavingFactor)->inactivate();
-      return LPState(newValues, duals, newWorkingSet, false,
-          state.iterations + 1);
-    }
-  } else {
-    // If we CAN make some progress, i.e. p_k != 0
-    // Adapt stepsize if some inactive constraints complain about this move
-    // LP: projection on nullspace is NOT zero:
-    //    find and put a blocking inactive constraint to the working set,
-    //    otherwise the problem is unbounded!!!
-    double alpha;
-    int factorIx;
-    VectorValues p = newValues - state.values;
-//    GTSAM_PRINT(p);
-    boost::tie(alpha, factorIx) = // using 16.41
-        computeStepSize(state.workingSet, state.values, p);
-    // also add to the working set the one that complains the most
-    InequalityFactorGraph newWorkingSet = state.workingSet;
-    if (factorIx >= 0)
-      newWorkingSet.at(factorIx)->activate();
-    // step!
-    newValues = state.values + alpha * p;
-    return LPState(newValues, state.duals, newWorkingSet, false,
-        state.iterations + 1);
-  }
-}
-
-//******************************************************************************
-boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
-    Key key, const InequalityFactorGraph &workingSet,
-    const VectorValues &delta) const {
-  // Transpose the A matrix of constrained factors to have the jacobian of the
-  // dual key
-  TermsContainer Aterms = collectDualJacobians<LinearEquality>(
-      key, lp_.equalities, equalityVariableIndex_);
-  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
-      key, workingSet, inequalityVariableIndex_);
-  Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
-                AtermsInequalities.end());
-
-  // Collect the gradients of unconstrained cost factors to the b vector
-  if (Aterms.size() > 0) {
-    Vector b = lp_.costGradient(key, delta);
-    // to compute the least-square approximation of dual variables
-    return boost::make_shared<JacobianFactor>(Aterms, b);
-  } else {
-    return boost::make_shared<JacobianFactor>();
-  }
-}
-
-//******************************************************************************
-InequalityFactorGraph LPSolver::identifyActiveConstraints(
-    const InequalityFactorGraph &inequalities,
-    const VectorValues &initialValues, const VectorValues &duals,
-    bool useWarmStart) const {
-  InequalityFactorGraph workingSet;
-  for (const LinearInequality::shared_ptr &factor : inequalities) {
-    LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    if (useWarmStart && duals.size() > 0) {
-      if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
-      else workingFactor->inactivate();
-    } else {
-      double error = workingFactor->error(initialValues);
-      // Safety guard. This should not happen unless users provide a bad init
-      if (error > 0) throw InfeasibleInitialValues();
-      if (fabs(error) < 1e-7)
-        workingFactor->activate();
-      else
-        workingFactor->inactivate();
-    }
-    workingSet.push_back(workingFactor);
-  }
-  return workingSet;
-}
-
-//******************************************************************************
-std::pair<VectorValues, VectorValues> LPSolver::optimize(
-    const VectorValues &initialValues, const VectorValues &duals,
-    bool useWarmStart) const {
-  {
-    // Initialize workingSet from the feasible initialValues
-    InequalityFactorGraph workingSet = identifyActiveConstraints(
-        lp_.inequalities, initialValues, duals, useWarmStart);
-    LPState state(initialValues, duals, workingSet, false, 0);
-
-    /// main loop of the solver
-    while (!state.converged) {
-      state = iterate(state);
-    }
-    return make_pair(state.values, state.duals);
-  }
-}
-
-//******************************************************************************
-boost::tuples::tuple<double, int> LPSolver::computeStepSize(
-    const InequalityFactorGraph &workingSet, const VectorValues &xk,
-    const VectorValues &p) const {
-  return ActiveSetSolver::computeStepSize(workingSet, xk, p,
-      std::numeric_limits<double>::infinity());
-}
-
-//******************************************************************************
-pair<VectorValues, VectorValues> LPSolver::optimize() const {
-  LPInitSolver initSolver(lp_);
-  VectorValues initValues = initSolver.solve();
-  return optimize(initValues);
-}
+constexpr double LPPolicy::maxAlpha;
 }
 
diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h
index 7a0e6781b..f878d28da 100644
--- a/gtsam_unstable/linear/LPSolver.h
+++ b/gtsam_unstable/linear/LPSolver.h
@@ -11,39 +11,25 @@
 
 /**
  * @file     LPSolver.h
- * @brief    Class used to solve Linear Programming Problems as defined in LP.h
+ * @brief    Policy of ActiveSetSolver to solve Linear Programming Problems
  * @author   Duy Nguyen Ta
  * @author   Ivan Dario Jimenez
- * @date     1/24/16
+ * @date     6/16/16
  */
 
-#pragma once
-
-#include <gtsam_unstable/linear/LPState.h>
 #include <gtsam_unstable/linear/LP.h>
 #include <gtsam_unstable/linear/ActiveSetSolver.h>
-#include <gtsam_unstable/linear/LinearCost.h>
-#include <gtsam/linear/VectorValues.h>
+#include <gtsam_unstable/linear/LPInitSolver.h>
+
+#include <limits>
+#include <algorithm>
 
 namespace gtsam {
 
-class LPSolver: public ActiveSetSolver {
-  const LP &lp_; //!< the linear programming problem
-public:
-  /// Constructor
-  LPSolver(const LP &lp);
-
-  const LP &lp() const {
-    return lp_;
-  }
-
-  /*
-   * This function performs an iteration of the Active Set Method for solving
-   * LP problems. At the end of this iteration the problem should either be found
-   * to be unfeasible, solved or the current state changed to reflect a new
-   * working set.
-   */
-  LPState iterate(const LPState &state) const;
+/// Policy for ActivetSetSolver to solve Linear Programming \sa LP problems
+struct LPPolicy {
+  /// Maximum alpha for line search. For LP, it's infinity
+  static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
 
   /**
    * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution
@@ -57,45 +43,37 @@ public:
    * The least-square solution of this quadratic subject to a set of linear constraints
    * is the projection of the gradient onto the constraints' subspace
    */
-  GaussianFactorGraph buildCostFunction(const VectorValues &xk) const;
+  static GaussianFactorGraph buildCostFunction(const LP& lp,
+                                               const VectorValues& xk) {
+    GaussianFactorGraph graph;
+    for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
+         ++it) {
+      size_t dim = lp.cost.getDim(it);
+      Vector b = xk.at(*it) - lp.cost.getA(it).transpose();  // b = xk-g
+      graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
+    }
 
-  GaussianFactorGraph buildWorkingGraph(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk) const;
-
-  /*
-   * A dual factor takes the objective function and a set of constraints.
-   * It then creates a least-square approximation of the lagrangian multipliers
-   * for the following problem: f' = - lambda * g' where f is the objection
-   * function g are dual factors and lambda is the lagrangian multiplier.
-   */
-  JacobianFactor::shared_ptr createDualFactor(Key key,
-      const InequalityFactorGraph &workingSet, const VectorValues &delta) const;
-
-  /// TODO(comment)
-  boost::tuple<double, int> computeStepSize(
-      const InequalityFactorGraph &workingSet, const VectorValues &xk,
-      const VectorValues &p) const;
-
-  /*
-   * Given an initial value this function determine which constraints are active
-   * which can be used to initialize the working set.
-   * A constraint Ax <= b  is active if we have an x' s.t. Ax' = b
-   */
-  InequalityFactorGraph identifyActiveConstraints(
-      const InequalityFactorGraph &inequalities,
-      const VectorValues &initialValues, const VectorValues &duals,
-      bool useWarmStart = false) const;
-
-  /** Optimize with the provided feasible initial values
-   * TODO: throw exception if the initial values is not feasible wrt inequality constraints
-   * TODO: comment duals
-   */
-  pair<VectorValues, VectorValues> optimize(const VectorValues &initialValues,
-      const VectorValues &duals = VectorValues(), bool useWarmStart = false) const;
-
-  /**
-   * Optimize without initial values.
-   */
-  pair<VectorValues, VectorValues> optimize() const;
+    KeySet allKeys = lp.inequalities.keys();
+    allKeys.merge(lp.equalities.keys());
+    allKeys.merge(KeySet(lp.cost.keys()));
+    // Add corresponding factors for all variables that are not explicitly in
+    // the cost function. Gradients of the cost function wrt to these variables 
+    // are zero (g=0), so b=xk
+    if (lp.cost.keys().size() != allKeys.size()) {
+      KeySet difference;
+      std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
+                          lp.cost.end(),
+                          std::inserter(difference, difference.end()));
+      for (Key k : difference) {
+        size_t dim = lp.constrainedKeyDimMap().at(k);
+        graph.push_back(
+            JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
+      }
+    }
+    return graph;
+  }
 };
-} // namespace gtsam
+
+using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
+
+}
\ No newline at end of file
diff --git a/gtsam_unstable/linear/LPState.h b/gtsam_unstable/linear/LPState.h
deleted file mode 100644
index 187ea2b51..000000000
--- a/gtsam_unstable/linear/LPState.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/**
- * @file     LPState.h
- * @brief    This struct holds the state of QPSolver at each iteration    
- * @author   Ivan Dario Jimenez
- * @date     1/24/16
- */
-
-#include <gtsam/linear/VectorValues.h>
-#include "InequalityFactorGraph.h"
-
-namespace gtsam {
-
-/*
- * This struct contains the state information for a single iteration of an
- * active set method iteration.
- */
-struct LPState {
-  // A itermediate value for the value of the final solution.
-  VectorValues values;
-  // Constains the set of duals computed during the iteration that retuned this
-  // state.
-  VectorValues duals;
-  // An inequality Factor Graph that contains only the active constriants.
-  InequalityFactorGraph workingSet;
-  // True if the algorithm has converged to a solution
-  bool converged;
-  // counter for the number of iteration. Incremented at the end of each iter.
-  size_t iterations;
-
-  /// default constructor
-  LPState() :
-      values(), duals(), workingSet(), converged(false), iterations(0) {
-  }
-
-  /// constructor with initial values
-  LPState(const VectorValues& initialValues, const VectorValues& initialDuals,
-      const InequalityFactorGraph& initialWorkingSet, bool _converged,
-      size_t _iterations) :
-      values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
-          _converged), iterations(_iterations) {
-  }
-};
-
-}
diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp
index 75a5e5c1a..5a4ac79ca 100644
--- a/gtsam_unstable/linear/QPSolver.cpp
+++ b/gtsam_unstable/linear/QPSolver.cpp
@@ -17,157 +17,7 @@
  */
 
 #include <gtsam_unstable/linear/QPSolver.h>
-#include <gtsam_unstable/linear/QPInitSolver.h>
-#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
-
-using namespace std;
 
 namespace gtsam {
-
-//******************************************************************************
-QPSolver::QPSolver(const QP& qp) :
-    qp_(qp) {
-  equalityVariableIndex_ = VariableIndex(qp_.equalities);
-  inequalityVariableIndex_ = VariableIndex(qp_.inequalities);
-  constrainedKeys_ = qp_.equalities.keys();
-  constrainedKeys_.merge(qp_.inequalities.keys());
-}
-
-//******************************************************************************
-GaussianFactorGraph QPSolver::buildWorkingGraph(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk) const {
-  GaussianFactorGraph workingGraph;
-  workingGraph.push_back(buildCostFunction(xk));
-  workingGraph.push_back(qp_.equalities);
-  for (const LinearInequality::shared_ptr& factor : workingSet) {
-    if (factor->active()) workingGraph.push_back(factor);
-  }
-  return workingGraph;
-}
-
-//******************************************************************************
-JacobianFactor::shared_ptr QPSolver::createDualFactor(
-    Key key, const InequalityFactorGraph& workingSet,
-    const VectorValues& delta) const {
-  // Transpose the A matrix of constrained factors to have the jacobian of the
-  // dual key
-  TermsContainer Aterms = collectDualJacobians<LinearEquality>(
-      key, qp_.equalities, equalityVariableIndex_);
-  TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
-      key, workingSet, inequalityVariableIndex_);
-  Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
-                AtermsInequalities.end());
-
-  // Collect the gradients of unconstrained cost factors to the b vector
-  if (Aterms.size() > 0) {
-    Vector b = qp_.costGradient(key, delta);
-    // to compute the least-square approximation of dual variables
-    return boost::make_shared<JacobianFactor>(Aterms, b);
-  } else {
-    return boost::make_shared<JacobianFactor>();
-  }
-}
-
-//******************************************************************************
-boost::tuple<double, int> QPSolver::computeStepSize(
-    const InequalityFactorGraph& workingSet, const VectorValues& xk,
-    const VectorValues& p) const {
-  return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1);
-}
-
-//******************************************************************************
-QPState QPSolver::iterate(const QPState& state) const {
-  // Algorithm 16.3 from Nocedal06book.
-  // Solve with the current working set eqn 16.39, but instead of solving for p
-  // solve for x
-  GaussianFactorGraph workingGraph =
-      buildWorkingGraph(state.workingSet, state.values);
-  VectorValues newValues = workingGraph.optimize();
-  // If we CAN'T move further
-  // if p_k = 0 is the original condition, modified by Duy to say that the state
-  // update is zero.
-  if (newValues.equals(state.values, 1e-7)) {
-    // Compute lambda from the dual graph
-    GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
-        newValues);
-    VectorValues duals = dualGraph->optimize();
-    int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
-    // If all inequality constraints are satisfied: We have the solution!!
-    if (leavingFactor < 0) {
-      return QPState(newValues, duals, state.workingSet, true,
-          state.iterations + 1);
-    } else {
-      // Inactivate the leaving constraint
-      InequalityFactorGraph newWorkingSet = state.workingSet;
-      newWorkingSet.at(leavingFactor)->inactivate();
-      return QPState(newValues, duals, newWorkingSet, false,
-          state.iterations + 1);
-    }
-  } else {
-    // If we CAN make some progress, i.e. p_k != 0
-    // Adapt stepsize if some inactive constraints complain about this move
-    double alpha;
-    int factorIx;
-    VectorValues p = newValues - state.values;
-    boost::tie(alpha, factorIx) = // using 16.41
-        computeStepSize(state.workingSet, state.values, p);
-    // also add to the working set the one that complains the most
-    InequalityFactorGraph newWorkingSet = state.workingSet;
-    if (factorIx >= 0)
-      newWorkingSet.at(factorIx)->activate();
-    // step!
-    newValues = state.values + alpha * p;
-    return QPState(newValues, state.duals, newWorkingSet, false,
-        state.iterations + 1);
-  }
-}
-
-//******************************************************************************
-InequalityFactorGraph QPSolver::identifyActiveConstraints(
-    const InequalityFactorGraph& inequalities,
-    const VectorValues& initialValues, const VectorValues& duals,
-    bool useWarmStart) const {
-  InequalityFactorGraph workingSet;
-  for (const LinearInequality::shared_ptr& factor : inequalities) {
-    LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
-    if (useWarmStart && duals.size() > 0) {
-      if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
-      else workingFactor->inactivate();
-    } else {
-      double error = workingFactor->error(initialValues);
-      // Safety guard. This should not happen unless users provide a bad init
-      if (error > 0) throw InfeasibleInitialValues();
-      if (fabs(error) < 1e-7)
-        workingFactor->activate();
-      else
-        workingFactor->inactivate();
-    }
-    workingSet.push_back(workingFactor);
-  }
-  return workingSet;
-}
-
-//******************************************************************************
-pair<VectorValues, VectorValues> QPSolver::optimize(
-    const VectorValues& initialValues, const VectorValues& duals,
-    bool useWarmStart) const {
-  // Initialize workingSet from the feasible initialValues
-  InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities,
-      initialValues, duals, useWarmStart);
-  QPState state(initialValues, duals, workingSet, false, 0);
-
-  /// main loop of the solver
-  while (!state.converged)
-    state = iterate(state);
-
-  return make_pair(state.values, state.duals);
-}
-
-//******************************************************************************
-pair<VectorValues, VectorValues> QPSolver::optimize() const {
-  QPInitSolver initSolver(qp_);
-  VectorValues initValues = initSolver.solve();
-  return optimize(initValues);
-}
-
-} /* namespace gtsam */
+constexpr double QPPolicy::maxAlpha;
+}
\ No newline at end of file
diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h
index 6fa60b048..2d7f4d801 100644
--- a/gtsam_unstable/linear/QPSolver.h
+++ b/gtsam_unstable/linear/QPSolver.h
@@ -10,96 +10,33 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    QPSolver.h
- * @brief   A quadratic programming solver implements the active set method
- * @date    Apr 15, 2014
- * @author  Ivan Dario Jimenez
- * @author  Duy-Nguyen Ta
+ * @file     QPSolver.h
+ * @brief    Policy of ActiveSetSolver to solve Quadratic Programming Problems
+ * @author   Duy Nguyen Ta
+ * @author   Ivan Dario Jimenez
+ * @date     6/16/16
  */
 
-#pragma once
-
 #include <gtsam_unstable/linear/QP.h>
 #include <gtsam_unstable/linear/ActiveSetSolver.h>
-#include <gtsam_unstable/linear/QPState.h>
-#include <gtsam/linear/VectorValues.h>
-
-#include <vector>
-#include <set>
+#include <gtsam_unstable/linear/QPInitSolver.h>
+#include <limits>
+#include <algorithm>
 
 namespace gtsam {
 
-/**
- * This QPSolver uses the active set method to solve a quadratic programming problem
- * defined in the QP struct.
- * Note: This version of QPSolver only works with a feasible initial value.
- */
-//TODO: Remove Vector Values
-class QPSolver: public ActiveSetSolver {
+/// Policy for ActivetSetSolver to solve Linear Programming \sa QP problems
+struct QPPolicy {
+  /// Maximum alpha for line search. For QP, it's 1
+  static constexpr double maxAlpha = 1.0;
 
-  const QP& qp_; //!< factor graphs of the QP problem, can't be modified!
-
-public:
-  /// Constructor
-  QPSolver(const QP& qp);
-
-  const GaussianFactorGraph& buildCostFunction(
-      const VectorValues& xk = VectorValues()) const {
-    return qp_.cost;
+  /// Simply the cost of the QP problem
+  static const GaussianFactorGraph& buildCostFunction(
+      const QP& qp, const VectorValues& xk = VectorValues()) {
+    return qp.cost;
   }
-
-  GaussianFactorGraph buildWorkingGraph(
-      const InequalityFactorGraph& workingSet,
-      const VectorValues& xk = VectorValues()) const;
-
-  /// Create a dual factor
-  JacobianFactor::shared_ptr createDualFactor(Key key,
-      const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
-
-  /* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
-   * If some inactive inequality constraints complain about the full step (alpha = 1),
-   * we have to adjust alpha to stay within the inequality constraints' feasible regions.
-   *
-   * For each inactive inequality j:
-   *  - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
-   *  - We want: aj'*(xk + alpha*p) - bj <= 0
-   *  - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
-   *  it's good!
-   *  - We only care when aj'*p > 0. In this case, we need to choose alpha so that
-   *  aj'*xk + alpha*aj'*p - bj <= 0  --> alpha <= (bj - aj'*xk) / (aj'*p)
-   *  We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
-   *
-   * We want the minimum of all those alphas among all inactive inequality.
-   */
-  boost::tuple<double, int> computeStepSize(
-      const InequalityFactorGraph& workingSet, const VectorValues& xk,
-      const VectorValues& p) const;
-
-  /// Iterate 1 step, return a new state with a new workingSet and values
-  QPState iterate(const QPState& state) const;
-
-  /// Identify active constraints based on initial values.
-  InequalityFactorGraph identifyActiveConstraints(
-      const InequalityFactorGraph& inequalities,
-      const VectorValues& initialValues, const VectorValues& duals =
-          VectorValues(), bool useWarmStart = true) const;
-
-  /**
-   * Optimize with provided initial values
-   * For this version, it is the responsibility of the caller to provide
-   * a feasible initial value, otherwise, an exception will be thrown.
-   * @return a pair of <primal, dual> solutions
-   */
-  std::pair<VectorValues, VectorValues> optimize(
-      const VectorValues& initialValues, const VectorValues& duals =
-          VectorValues(), bool useWarmStart = true) const;
-
-  /**
-   * For this version the caller will not have to provide an initial value
-   * @return a pair of <primal, dual> solutions
-   */
-  std::pair<VectorValues, VectorValues> optimize() const;
-
 };
 
-} // namespace gtsam
+using QPSolver = ActiveSetSolver<QP, QPPolicy, QPInitSolver>;
+
+}
\ No newline at end of file
diff --git a/gtsam_unstable/linear/QPState.h b/gtsam_unstable/linear/QPState.h
deleted file mode 100644
index d88a88c5f..000000000
--- a/gtsam_unstable/linear/QPState.h
+++ /dev/null
@@ -1,29 +0,0 @@
-//
-// Created by ivan on 1/25/16.
-//
-
-#pragma once
-
-namespace gtsam {
-/// This struct holds the state of QPSolver at each iteration
-struct QPState {
-  VectorValues values;
-  VectorValues duals;
-  InequalityFactorGraph workingSet;
-  bool converged;
-  size_t iterations;
-
-  /// default constructor
-  QPState() :
-      values(), duals(), workingSet(), converged(false), iterations(0) {
-  }
-
-  /// constructor with initial values
-  QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
-      const InequalityFactorGraph& initialWorkingSet, bool _converged,
-      size_t _iterations) :
-      values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
-          _converged), iterations(_iterations) {
-  }
-};
-}
diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp
index e707ac911..737b28f15 100644
--- a/gtsam_unstable/linear/tests/testQPSolver.cpp
+++ b/gtsam_unstable/linear/tests/testQPSolver.cpp
@@ -21,8 +21,6 @@
 #include <gtsam_unstable/linear/QPSolver.h>
 #include <gtsam_unstable/linear/QPSParser.h>
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
-#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
 
 using namespace std;
 using namespace gtsam;
@@ -179,7 +177,8 @@ TEST(QPSolver, iterate) {
   InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
       qp.inequalities, currentSolution);
 
-  QPState state(currentSolution, VectorValues(), workingSet, false, 100);
+  QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
+                        100);
 
   int it = 0;
   while (!state.converged) {