diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h
index 18dc07aec..34d8e2b5d 100644
--- a/gtsam_unstable/linear/ActiveSetSolver-inl.h
+++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h
@@ -189,6 +189,13 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
   return workingGraph;
 }
 
+//******************************************************************************
+Template VectorValues
+This::evaluateCostFunction(const VectorValues &xk) const {
+  GaussianFactorGraph costGraph;
+  costGraph.push_back(POLICY::buildCostFunction(problem_, xk));
+  return costGraph.optimize();
+}
 //******************************************************************************
 Template typename This::State This::iterate(
     const typename This::State& state) const {
diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h
index 67386cb78..1780fdfd9 100644
--- a/gtsam_unstable/linear/ActiveSetSolver.h
+++ b/gtsam_unstable/linear/ActiveSetSolver.h
@@ -167,7 +167,9 @@ public: /// Just for testing...
   GaussianFactorGraph buildWorkingGraph(
       const InequalityFactorGraph& workingSet,
       const VectorValues& xk = VectorValues()) const;
-
+  
+  VectorValues evaluateCostFunction(const VectorValues& xk) const;
+  
   /// Iterate 1 step, return a new state with a new workingSet and values
   State iterate(const State& state) const;
 
diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp
index 737b28f15..40fc3b52b 100644
--- a/gtsam_unstable/linear/tests/testQPSolver.cpp
+++ b/gtsam_unstable/linear/tests/testQPSolver.cpp
@@ -257,6 +257,20 @@ TEST(QPSolver, ParserSemanticTest) {
   CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
 }
 
+TEST(QPSolver, QPExampleTest){
+  QP problem = QPSParser("QPExample.QPS").Parse();
+  VectorValues actualSolution;
+  auto solver = QPSolver(problem);
+  boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize();
+  VectorValues expectedSolution;
+  expectedSolution.insert(Symbol('X',1),0.7625*I_1x1);
+  expectedSolution.insert(Symbol('X',2),0.4750*I_1x1);
+  VectorValues actualCost = solver.evaluateCostFunction(actualSolution);
+  VectorValues expectedCost = solver.evaluateCostFunction(expectedCost);
+  GTSAM_PRINT(actualCost);
+  CHECK(assert_equal(expectedCost, actualCost))
+  CHECK(assert_equal(expectedSolution, actualSolution, 1e-7))
+}
 /* ************************************************************************* */
 // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
 QP createTestMatlabQPEx() {