diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index 5fee1f3e6..39bc4801d 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -28,7 +28,7 @@ namespace gtsam {
 		bool Gaussian::equals(const Base& m, double tol) const {
 			const Gaussian* p = dynamic_cast<const Gaussian*> (&m);
 			if (p == NULL) return false;
-			return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, tol);
+			return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol));
 		}
 
 		Vector Gaussian::whiten(const Vector& v) const {
@@ -45,27 +45,11 @@ namespace gtsam {
 			return inner_prod(w, w);
 		}
 
-		// functional
 		Matrix Gaussian::Whiten(const Matrix& H) const {
-//			size_t m = H.size1(), n = H.size2();
-//			Matrix W(m, n);
-//			for (int j = 0; j < n; j++) {
-//				Vector wj = whiten(column(H, j));
-//				for (int i = 0; i < m; i++)
-//					W(i, j) = wj(i);
-//			}
-//			return W;
 		  return sqrt_information_ * H;
 		}
 
-		// in place
 		void Gaussian::WhitenInPlace(Matrix& H) const {
-//			size_t m = H.size1(), n = H.size2();
-//			for (int j = 0; j < n; j++) {
-//				Vector wj = whiten(column(H, j));
-//				for (int i = 0; i < m; i++)
-//					H(i, j) = wj(i);
-//			}
 		  H = sqrt_information_ * H;
 		}
 
@@ -88,6 +72,14 @@ namespace gtsam {
 			return emul(v, sigmas_);
 		}
 
+		Matrix Diagonal::Whiten(const Matrix& H) const {
+		  return vector_scale(invsigmas_, H);
+		}
+
+		void Diagonal::WhitenInPlace(Matrix& H) const {
+		  H = vector_scale(invsigmas_, H);
+		}
+
 		/* ************************************************************************* */
 
 		void Constrained::print(const std::string& name) const {
@@ -126,6 +118,14 @@ namespace gtsam {
 			return v * sigma_;
 		}
 
+		Matrix Isotropic::Whiten(const Matrix& H) const {
+		  return invsigma_ * H;
+		}
+
+		void Isotropic::WhitenInPlace(Matrix& H) const {
+		  H *= invsigma_;
+		}
+
 		/* ************************************************************************* */
 		void Unit::print(const std::string& name) const {
 			cout << "Unit " << name << endl;
diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h
index c8fd02edf..a1e86fee9 100644
--- a/cpp/NoiseModel.h
+++ b/cpp/NoiseModel.h
@@ -51,8 +51,7 @@ namespace gtsam {	namespace noiseModel {
     virtual void unwhitenInPlace(Vector& v) const {
     	v = unwhiten(v);
     }
-
-  };
+    };
 
   /**
 	 * Gaussian implements the mathematical model
@@ -123,6 +122,14 @@ namespace gtsam {	namespace noiseModel {
 		 */
 		virtual void WhitenInPlace(Matrix& H) const;
 
+    /**
+     * Whiten a system, in place as well
+     */
+    inline void WhitenSystem(Matrix& A, Vector& b) const {
+  		WhitenInPlace(A);
+  		whitenInPlace(b);
+    }
+
 		/**
 		 * Return R itself, but note that Whiten(H) is cheaper than R*H
 		 */
@@ -180,7 +187,16 @@ namespace gtsam {	namespace noiseModel {
 		virtual void print(const std::string& name) const;
     virtual Vector whiten(const Vector& v) const;
     virtual Vector unwhiten(const Vector& v) const;
-  };
+		virtual Matrix Whiten(const Matrix& H) const;
+		virtual void WhitenInPlace(Matrix& H) const;
+
+		/**
+     * Return standard deviations (sqrt of diagonal)
+     */
+    inline const Vector& sigmas() const { return sigmas_; }
+    inline double sigma(size_t i) const { return sigmas_(i); }
+
+  }; // Diagonal
 
   /**
    * A Constrained constrained model is a specialization of Diagonal which allows
@@ -223,7 +239,8 @@ namespace gtsam {	namespace noiseModel {
 
 		virtual Matrix Whiten(const Matrix& H) const;
 		virtual void WhitenInPlace(Matrix& H) const;
-  };
+
+  }; // Constrained
 
   /**
    * An isotropic noise model corresponds to a scaled diagonal covariance
@@ -266,6 +283,13 @@ namespace gtsam {	namespace noiseModel {
 		virtual double Mahalanobis(const Vector& v) const;
     virtual Vector whiten(const Vector& v) const;
     virtual Vector unwhiten(const Vector& v) const;
+		virtual Matrix Whiten(const Matrix& H) const;
+		virtual void WhitenInPlace(Matrix& H) const;
+
+    /**
+     * Return standard deviation
+     */
+    inline double sigma() const { return sigma_; }
   };
 
   /**
@@ -316,5 +340,7 @@ namespace gtsam {	namespace noiseModel {
 #endif
 		};
 
+	typedef Diagonal::shared_ptr sharedDiagonal;
+
 
 } // namespace gtsam