diff --git a/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp b/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp
new file mode 100644
index 000000000..8b1077ebf
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/testLinearInequalityManifolds.cpp
@@ -0,0 +1,65 @@
+///* ----------------------------------------------------------------------------
+//
+// * 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    testLinearInequalityManifolds.cpp
+// * @brief   Unit tests for testLinearInequalityManifolds
+// * @author  Krunal Chande
+// * @author  Duy-Nguyen Ta
+// * @author  Luca Carlone
+// * @date    Dec 15, 2014
+// */
+//
+//#include <gtsam/inference/Symbol.h>
+//#include <CppUnitLite/TestHarness.h>
+//#include <iostream>
+//
+//using namespace std;
+//using namespace gtsam::symbol_shorthand;
+//using namespace gtsam;
+//const double tol = 1e-10;
+//
+//namespace gtsam {
+//class PositionConstraint : public LinearInequalityManifold1 {
+//public:
+//  PositionConstraint()
+//};
+//}
+////******************************************************************************
+//TEST(testLinearEqualityManifolds, equals ) {
+//  // Instantiate a class LinearEqualityManifolds
+//  LinearEqualityManifolds le1();
+//
+//  // Instantiate another class LinearEqualityManifolds
+//  LinearEqualityManifolds le2();
+//
+//  // check equals
+//  CHECK(assert_equal(le1,le1));
+//  CHECK(le2.equals(le2));
+//  CHECK(!le2.equals(le1));
+//  CHECK(!le1.equals(le2));
+//}
+//
+//////******************************************************************************
+////TEST(testLinearEqualityManifolds, linearize ) {
+////}
+////
+//////******************************************************************************
+////TEST(testLinearEqualityManifolds, size ) {
+////}
+//
+////******************************************************************************
+//int main() {
+//  TestResult tr;
+//  return TestRegistry::runAllTests(tr);
+//}
+////******************************************************************************
+//
diff --git a/gtsam_unstable/slam/tests/testPositionConstraints.cpp b/gtsam_unstable/slam/tests/testPositionConstraints.cpp
new file mode 100644
index 000000000..04eb4a260
--- /dev/null
+++ b/gtsam_unstable/slam/tests/testPositionConstraints.cpp
@@ -0,0 +1,176 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testPositionUpperBoundX.cpp
+ * @brief   Unit tests for testPositionUpperBoundX
+ * @author  Krunal Chande
+ * @author  Duy-Nguyen Ta
+ * @author  Luca Carlone
+ * @date    Dec 16, 2014
+ */
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <CppUnitLite/TestHarness.h>
+#include <iostream>
+
+namespace gtsam {
+
+template<class T>
+class LinearInequalityManifold1: public NoiseModelFactor1<T> {
+  typedef NoiseModelFactor1<T> Base;
+  typedef LinearInequalityManifold1<T> This;
+protected:
+  bool active_;
+public:
+
+  /// Constructor
+  LinearInequalityManifold1(Key key) :
+      Base(noiseModel::Constrained::All(1), key), active_(true) {
+  }
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const {
+    std::cout << s << " key = { " << keyFormatter(this->key()) << "}"
+        << std::endl;
+    if (active_)
+      std::cout << " Active" << std::endl;
+    else
+      std::cout << " Inactive" << std::endl;
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected,
+      double tol = 1e-9) const {
+    const This* e = dynamic_cast<const This*>(&expected);
+    return e != NULL && Base::equals(*e, tol) && (active_ == e->active_);
+  }
+
+  /** Evaluate error MUST return a *one dimensional* vector,
+   * because we don't support multi-valued inequality factors
+   */
+  virtual Vector evaluateError(const T& x, boost::optional<Matrix&> H =
+      boost::none) const = 0;
+
+};
+
+/**
+ * This class defines an inequality upper bound on x for a Pose3
+ *      x <= upperBound
+ */
+class PositionUpperBoundX: public LinearInequalityManifold1<Pose3> {
+  double upperBound_;
+  typedef LinearInequalityManifold1<Pose3> Base;
+  typedef PositionUpperBoundX This;
+public:
+  PositionUpperBoundX(Key key, const double upperBound) :
+      Base(key), upperBound_(upperBound) {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** implement functions needed for Testable */
+
+  /** print */
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const {
+    Base::print(s);
+    std::cout << "PositionUpperBoundX" << std::endl;
+    std::cout << "x <= " << upperBound_ << std::endl;
+  }
+
+  /** equals */
+  virtual bool equals(const NonlinearFactor& expected,
+      double tol = 1e-9) const {
+    const This* e = dynamic_cast<const This*>(&expected);
+    return e != NULL && Base::equals(*e, tol) && (upperBound_ == e->upperBound_);
+  }
+
+  /**
+   * error = x - upperBound_
+   */
+  double computeError(const Pose3& pose) const {
+    return pose.x() - upperBound_;
+  }
+
+  /**
+   * error = x - upperBound_
+   */
+  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
+      boost::none) const {
+    if (H)
+      *H = (Matrix(1, 6) << 0, 0, 0, 1, 0, 0).finished();
+    return (Vector(1) << computeError(pose)).finished();
+  }
+};
+}
+
+//******************************************************************************
+
+using namespace std;
+using namespace gtsam::symbol_shorthand;
+using namespace gtsam;
+
+//******************************************************************************
+TEST(PositionUpperBoundX, equals ) {
+  // Instantiate a class PositionUpperBoundX
+  PositionUpperBoundX ineq1(X(1), 1);
+
+  // Instantiate another class PositionUpperBoundX
+  PositionUpperBoundX ineq2(X(1), 1);
+
+  // check equals
+  CHECK(ineq1.equals(ineq2, 1e-5));
+}
+
+//******************************************************************************
+TEST(PositionUpperBoundX, evaluateError ) {
+  // Instantiate a class PositionUpperBoundX
+  PositionUpperBoundX ineq(X(1), 45.6);
+
+  Pose3 pose(Rot3::ypr(0.1, 0.3, 0.2), Point3(43.0, 27.8, 91.1));
+  Matrix H;
+  Vector error = ineq.evaluateError(pose, H);
+
+  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
+          boost::bind(&PositionUpperBoundX::evaluateError, ineq, _1, boost::none),
+      pose, 1e-5);
+
+  cout << "expectedH: " << expectedH << endl;
+  cout << "H: " << H << endl;
+
+  Vector expectedError = (Vector(1) << 0.0).finished();
+//  CHECK(assert_equal(expectedError, error, 1e-100));
+//  CHECK(assert_equal(expectedH, H, 1e-100));
+
+  Matrix hessian = numericalHessian<Pose3>(
+          boost::bind(&PositionUpperBoundX::computeError, ineq, _1),
+      pose, 1e-5);
+  cout << "Hessian: \n" << hessian << endl;
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+