diff --git a/.cproject b/.cproject
index 32ddccfa1..21cfaf8a6 100644
--- a/.cproject
+++ b/.cproject
@@ -743,6 +743,14 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testNoiseModel.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments>-j2</buildArguments>
+<buildTarget>testNoiseModel.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>-j2</buildArguments>
diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index af50ab360..a6e28efe0 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -2,7 +2,62 @@
  * NoiseModel.cpp
  *
  *  Created on: Jan 13, 2010
- *      Author: richard
+ *      Author: Richard Roberts
+ *      Author: Frank Dellaert
  */
 
+//#include <boost/numeric/ublas/traits.hpp>
+
 #include "NoiseModel.h"
+
+namespace gtsam {
+
+	Vector Isotropic::whiten(const Vector& v) const {
+		return v * invsigma_;
+	}
+
+	Vector Isotropic::unwhiten(const Vector& v) const {
+		return v * sigma_;
+	}
+
+	Diagonal::Diagonal(const Vector& sigmas) :
+		sigmas_(sigmas), invsigmas_(1.0 / sigmas) {
+	}
+
+	Diagonal::Diagonal(const Diagonal& d) :
+		sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) {
+	}
+
+	Vector Diagonal::whiten(const Vector& v) const {
+		return emul(v, invsigmas_);
+	}
+
+	Vector Diagonal::unwhiten(const Vector& v) const {
+		return emul(v, sigmas_);
+	}
+
+	Variances::Variances(const Vector& variances) {
+		sigmas_.resize(variances.size());
+		std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt);
+		invsigmas_ = reciprocal(sigmas_);
+	}
+
+	FullCovariance::FullCovariance(const Matrix& cov) :
+		sqrt_covariance_(square_root_positive(cov)), sqrt_inv_covariance_(
+				inverse_square_root(cov)) {
+	}
+
+	FullCovariance::FullCovariance(const FullCovariance& cov) :
+		sqrt_covariance_(cov.sqrt_covariance_), sqrt_inv_covariance_(
+				cov.sqrt_inv_covariance_) {
+	}
+
+	Vector FullCovariance::whiten(const Vector& v) const {
+		return sqrt_inv_covariance_ * v;
+	}
+
+	Vector FullCovariance::unwhiten(const Vector& v) const {
+		return sqrt_covariance_ * v;
+	}
+
+} // gtsam
diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h
index 5e37c86ab..8b84c0464 100644
--- a/cpp/NoiseModel.h
+++ b/cpp/NoiseModel.h
@@ -2,31 +2,24 @@
  * NoiseModel.h
  *
  *  Created on: Jan 13, 2010
- *      Author: richard
+ *      Author: Richard Roberts
+ *      Author: Frank Dellaert
  */
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
-#include <boost/numeric/ublas/traits.hpp>
-//#include <iostream>
-//using namespace std;
-
 #include "Vector.h"
 #include "Matrix.h"
 
 namespace gtsam {
 
-  // Forward declaration
-  class NoiseModel;
-
-  /*****************************************************************************
-   * NoiseModelBase is the abstract base class for all noise models.  NoiseModels
+  /**
+   * NoiseModel is the abstract base class for all noise models.  NoiseModels
    * must implement a 'whiten' function to normalize an error vector, and an
    * 'unwhiten' function to unnormalize an error vector.
    */
-  class NoiseModelBase {
-  public:
+  struct NoiseModel {
+
     /**
      * Whiten an error vector.
      */
@@ -36,82 +29,14 @@ namespace gtsam {
      * Unwhiten an error vector.
      */
     virtual Vector unwhiten(const Vector& v) const = 0;
-
-    friend class NoiseModel;
-
-  private:
-    /**
-     * Used internally to duplicate the object while retaining the type.
-     */
-    virtual boost::shared_ptr<NoiseModelBase> clone() const = 0;
   };
 
-
-
-  /*****************************************************************************
-   * NoiseModel is a container for NoiseModelBase, which internally stores
-   * a shared_ptr to a NoiseModelBase as to support fast and compact storage and
-   * copies.  Copying this class simply assigns the internal shared_ptr.
-   */
-  class NoiseModel {
-  private:
-    const boost::shared_ptr<const NoiseModelBase> base_;
-
-    /**
-     * Fast constructor, simply assigns shared_ptr.
-     */
-    NoiseModel(boost::shared_ptr<NoiseModelBase> noiseModel): base_(noiseModel) {
-      /*std::cout << "Assigning pointer" << std::endl;*/
-    }
-
-  public:
-    /**
-     * Fast copy constructor, simply assigns shared_ptr.
-     */
-    NoiseModel(const NoiseModel& noiseModel): base_(noiseModel.base_) { /*std::cout << "Assigning pointer" << std::endl;*/ }
-
-    /**
-     * Constructor that creates a fast-copyable NoiseModel class by cloning
-     * a non-pointer NoiseModelBase.  The type is retained and can be retrieved
-     * using a dynamic_cast.
-     */
-    template<class T>
-    NoiseModel(const T& noiseModel): base_(noiseModel.clone()) {}
-
-    /**
-     * Cast to boost::shared_ptr<NoiseModelBase> to retrieve a pointer to the
-     * NoiseModelBase type.  Can be used with dynamic_pointer_cast to retrieve
-     * the type at runtime.
-     * E.g.:  dynamic_pointer_cast<const Isotropic>(noiseModel).
-     */
-    operator const boost::shared_ptr<const NoiseModelBase> () const {
-      return base_; }
-
-    /**
-     * Call the NoiseModelBase virtual whiten function
-     */
-    Vector whiten(const Vector& v) const { return base_->whiten(v); }
-
-    /**
-     * Call the NoiseModelBase virtual unwhiten function
-     */
-    Vector unwhiten(const Vector& v) const { return base_->unwhiten(v); }
-
-    template<class T> friend boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p);
-  };
-
-  template<class T>
-  boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p) {
-    return boost::dynamic_pointer_cast<const T>(p.base_); }
-
-
-
-  /*****************************************************************************
-   * An isotropic noise model assigns the same sigma to each vector element.
+  /**
+   * An isotropic noise model corresponds to a scaled diagonal covariance
    * This class has no public constructors.  Instead, use either either the
    * Sigma or Variance class.
    */
-  class Isotropic : public NoiseModelBase {
+  class Isotropic : public NoiseModel {
   protected:
     double sigma_;
     double invsigma_;
@@ -121,125 +46,48 @@ namespace gtsam {
       sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {}
 
   public:
-    /**
-     * Whiten error vector by dividing by sigma
-     */
-    virtual Vector whiten(const Vector& v) const { return v * invsigma_; }
-
-    /**
-     * Unwhiten error vector by multiplying by sigma
-     */
-    virtual Vector unwhiten(const Vector& v) const { return v * sigma_; }
-
-    /**
-     * Clone is used to duplicate object while retaining type
-     */
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      /*cout << "Cloning Isotropic" << endl;*/
-      return boost::shared_ptr<NoiseModelBase>(new Isotropic(*this)); }
+    Vector whiten(const Vector& v) const;
+    Vector unwhiten(const Vector& v) const;
   };
 
-
-  /*****************************************************************************
-   * A diagonal noise model implements a diagonal covariance matrix, with the
-   * elements of the diagonal specified in a Vector.  This class has no public
-   * constructors, instead, use either the Sigmas or Variances class.
-   */
-  class Diagonal : public NoiseModelBase {
-  protected:
-    Vector sigmas_;
-    Vector invsigmas_;
-
-    Diagonal() {}
-    Diagonal(const Vector& sigmas): sigmas_(sigmas), invsigmas_(1.0 / sigmas) {}
-    Diagonal(const Diagonal& d): sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) {}
-
-  public:
-    /**
-     * Whiten error vector by dividing by sigmas
-     */
-    virtual Vector whiten(const Vector& v) const { return emul(v, invsigmas_); }
-
-    /**
-     * Unwhiten error vector by multiplying by sigmas
-     */
-    virtual Vector unwhiten(const Vector& v) const { return emul(v, sigmas_); }
-
-    /**
-     * Clone is used to duplicate object while retaining type
-     */
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      /*cout << "Cloning Isotropic" << endl;*/
-      return boost::shared_ptr<NoiseModelBase>(new Diagonal(*this)); }
-  };
-
-
-  /*****************************************************************************
-   * A full covariance noise model.
-   */
-  class FullCovariance : public NoiseModelBase {
-  protected:
-    Matrix sqrt_covariance_;
-    Matrix sqrt_inv_covariance_;
-
-  public:
-    FullCovariance(const Matrix& covariance):
-        sqrt_covariance_(square_root_positive(covariance)),
-        sqrt_inv_covariance_(inverse_square_root(covariance)) {}
-
-    FullCovariance(const FullCovariance& c):
-        sqrt_covariance_(c.sqrt_covariance_), sqrt_inv_covariance_(c.sqrt_inv_covariance_) {}
-
-    /**
-     * Whiten error vector by dividing by sigmas
-     */
-    virtual Vector whiten(const Vector& v) const { return sqrt_inv_covariance_ * v; }
-
-    /**
-     * Unwhiten error vector by multiplying by sigmas
-     */
-    virtual Vector unwhiten(const Vector& v) const { return sqrt_covariance_ * v; }
-
-    /**
-     * Clone is used to duplicate object while retaining type
-     */
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      /*cout << "Cloning Isotropic" << endl;*/
-      return boost::shared_ptr<NoiseModelBase>(new FullCovariance(*this)); }
-  };
-
-
-
-  /*****************************************************************************
-   * An isotropic noise model using sigma, the noise standard
-   * deviation.
+  /**
+   * An isotropic noise model using sigma, the standard deviation.
    */
   class Sigma : public Isotropic {
   public:
-    Sigma(const Sigma& isotropic): Isotropic(isotropic) { /*cout << "Constructing Sigma from Sigma" << endl;*/ }
-
-    Sigma(double sigma): Isotropic(sigma) { /*cout << "Constructing Sigma from double" << endl;*/ }
-
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      return boost::shared_ptr<NoiseModelBase>(new Sigma(*this)); }
+    Sigma(const Sigma& isotropic): Isotropic(isotropic) {}
+    Sigma(double sigma): Isotropic(sigma) {}
   };
 
-
-  /*****************************************************************************
+  /**
    * An isotropic noise model using the noise variance = sigma^2.
    */
   class Variance : public Isotropic {
   public:
     Variance(const Variance& v): Isotropic(v) {}
-
     Variance(double variance): Isotropic(sqrt(variance)) {}
-
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      return boost::shared_ptr<NoiseModelBase>(new Variance(*this)); }
   };
 
+  /**
+   * A diagonal noise model implements a diagonal covariance matrix, with the
+   * elements of the diagonal specified in a Vector.  This class has no public
+   * constructors, instead, use either the Sigmas or Variances class.
+   */
+  class Diagonal : public NoiseModel {
+  protected:
+    Vector sigmas_;
+    Vector invsigmas_;
 
-  /*****************************************************************************
+    Diagonal() {}
+    Diagonal(const Vector& sigmas);
+    Diagonal(const Diagonal& d);
+
+  public:
+    Vector whiten(const Vector& v) const;
+    Vector unwhiten(const Vector& v) const;
+  };
+
+  /**
    * A diagonal noise model created by specifying a Vector of sigmas, i.e.
    * standard devations, i.e. the diagonal of the square root covariance
    * matrix.
@@ -247,29 +95,34 @@ namespace gtsam {
   class Sigmas : public Diagonal {
   public:
     Sigmas(const Sigmas& s): Diagonal(s) {}
-
     Sigmas(const Vector& sigmas): Diagonal(sigmas) {}
-
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      return boost::shared_ptr<NoiseModelBase>(new Sigmas(*this)); }
   };
 
-
-  /*****************************************************************************
+  /**
    * A diagonal noise model created by specifying a Vector of variances, i.e.
    * i.e. the diagonal of the covariance matrix.
    */
   class Variances : public Diagonal {
   public:
     Variances(const Variances& s): Diagonal(s) {}
+    Variances(const Vector& variances);
+  };
 
-    Variances(const Vector& variances) {
-      std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt);
-      invsigmas_ = 1.0 / sigmas_;
-    }
+  /**
+   * A full covariance noise model.
+   */
+  class FullCovariance : public NoiseModel {
+  protected:
+    Matrix sqrt_covariance_;
+    Matrix sqrt_inv_covariance_;
 
-    boost::shared_ptr<NoiseModelBase> clone() const {
-      return boost::shared_ptr<NoiseModelBase>(new Variances(*this)); }
+  public:
+
+    FullCovariance(const Matrix& covariance);
+    FullCovariance(const FullCovariance& c);
+
+    Vector whiten(const Vector& v) const;
+    Vector unwhiten(const Vector& v) const;
   };
 
 }
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 281c77576..9357c2e7c 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -606,24 +606,24 @@ TEST( matrix, inverse_square_root )
 }
 
 /* ************************************************************************* */
-//TEST( matrix, square_root_positive )
-//{
-//  Matrix cov = Matrix_(3,3,
-//      4.25, 1.5, 0.0,
-//      1.5,  2.0, 0.0,
-//      0.0,  0.0, 1.0);
-//
-//  Matrix expected = Matrix_(3,3,
-//      -2.0,-1.0, 0.0,
-//      -0.5, 1.0, 0.0,
-//       0.0, 0.0, 1.0);
-//
-//  Matrix actual = square_root_positive(cov);
-//
-//  CHECK(assert_equal(expected, actual));
-//  CHECK(assert_equal(cov, prod(trans(actual),actual)));
-//}
+TEST( matrix, square_root_positive )
+{
+  Matrix cov = Matrix_(3,3,
+			4.0, 0.0, 0.0,
+			0.0, 4.0, 0.0,
+			0.0, 0.0, 100.0
+      );
 
+  Matrix expected = Matrix_(3,3,
+			2.0, 0.0, 0.0,
+			0.0, 2.0, 0.0,
+			0.0, 0.0, 10.0
+			);
+
+  Matrix actual = square_root_positive(cov);
+  CHECK(assert_equal(expected, actual));
+  CHECK(assert_equal(cov, prod(trans(actual),actual)));
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp
index 8a80c6001..9f40dca99 100644
--- a/cpp/testNoiseModel.cpp
+++ b/cpp/testNoiseModel.cpp
@@ -2,7 +2,8 @@
  * testNoiseModel.cpp
  *
  *  Created on: Jan 13, 2010
- *      Author: richard
+ *      Author: Richard Roberts
+ *      Author: Frank Dellaert
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -12,46 +13,43 @@
 #include "NoiseModel.h"
 
 using namespace std;
-using namespace boost;
 using namespace gtsam;
 
-class TakesNoiseModel {
-public:
-  NoiseModel noiseModel_;
-public:
-  //template<class N>
-  TakesNoiseModel(const NoiseModel& noiseModel): noiseModel_(noiseModel) {}
-};
-
-
-TEST(NoiseModel, sharedptr)
+/* ************************************************************************* */
+TEST(NoiseModel, constructors)
 {
-//  TakesNoiseModel tnm1(Sigma(1.0));
-//  cout << endl;
-//  TakesNoiseModel tnm2(tnm1.noiseModel_);
-//
-//  if(dynamic_pointer_cast<Sigma>(tnm1.noiseModel_))
-//    cout << "tnm1 has a Sigma!" << endl;
-//  if(dynamic_pointer_cast<Variance>(tnm1.noiseModel_))
-//    cout << "tnm1 has a Variance!" << endl;
-//  if(dynamic_pointer_cast<Isotropic>(tnm1.noiseModel_))
-//    cout << "tnm1 has an Isotropic!" << endl;
-//  if(dynamic_pointer_cast<NoiseModelBase>(tnm1.noiseModel_))
-//    cout << "tnm1 has a NoiseModelBase!" << endl;
-//
-//  if(dynamic_pointer_cast<Sigma>(tnm2.noiseModel_))
-//    cout << "tnm2 has a Sigma!" << endl;
-//  if(dynamic_pointer_cast<Variance>(tnm2.noiseModel_))
-//    cout << "tnm2 has a Variance!" << endl;
-//  if(dynamic_pointer_cast<Isotropic>(tnm2.noiseModel_))
-//    cout << "tnm2 has an Isotropic!" << endl;
-//  if(dynamic_pointer_cast<NoiseModelBase>(tnm2.noiseModel_))
-//    cout << "tnm2 has a NoiseModelBase!" << endl;
+	double sigma = 2, var = sigma*sigma;
+	Vector whitened = Vector_(3,5.0,10.0,15.0);
+	Vector unwhitened = Vector_(3,10.0,20.0,30.0);
+
+	// Construct noise models
+	Sigma m1(sigma);
+	Variance m2(var);
+	Sigmas m3(Vector_(3, sigma, sigma, sigma));
+	Variances m4(Vector_(3, var, var, var));
+	FullCovariance m5(Matrix_(3, 3,
+			var, 0.0, 0.0,
+			0.0, var, 0.0,
+			0.0, 0.0, var));
+
+	// test whiten
+	CHECK(assert_equal(whitened,m1.whiten(unwhitened)));
+	CHECK(assert_equal(whitened,m2.whiten(unwhitened)));
+	CHECK(assert_equal(whitened,m3.whiten(unwhitened)));
+	CHECK(assert_equal(whitened,m4.whiten(unwhitened)));
+	CHECK(assert_equal(whitened,m5.whiten(unwhitened)));
+
+	// test unwhiten
+	CHECK(assert_equal(unwhitened,m1.unwhiten(whitened)));
+	CHECK(assert_equal(unwhitened,m2.unwhiten(whitened)));
+	CHECK(assert_equal(unwhitened,m3.unwhiten(whitened)));
+	CHECK(assert_equal(unwhitened,m4.unwhiten(whitened)));
+	CHECK(assert_equal(unwhitened,m5.unwhiten(whitened)));
 }
 
 /* ************************************************************************* */
 int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
 }
 /* ************************************************************************* */