diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp
index a8b177b43..06c5fe7fb 100644
--- a/gtsam/linear/NoiseModel.cpp
+++ b/gtsam/linear/NoiseModel.cpp
@@ -376,8 +376,11 @@ double Constrained::distance(const Vector& v) const {
 
 /* ************************************************************************* */
 Matrix Constrained::Whiten(const Matrix& H) const {
-  // selective scaling
-  return vector_scale(invsigmas(), H, true);
+  Matrix A = H;
+  for (DenseIndex i=0; i<(DenseIndex)dim_; ++i)
+    if (!constrained(i)) // if constrained, leave row of A as is
+      A.row(i) *= invsigmas_(i);
+  return A;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h
index 05ed21977..a6c63da50 100644
--- a/gtsam/linear/NoiseModel.h
+++ b/gtsam/linear/NoiseModel.h
@@ -81,6 +81,9 @@ namespace gtsam {
       /// Whiten an error vector.
       virtual Vector whiten(const Vector& v) const = 0;
 
+      /// Whiten a matrix.
+      virtual Matrix Whiten(const Matrix& H) const = 0;
+
       /// Unwhiten an error vector.
       virtual Vector unwhiten(const Vector& v) const = 0;
 
@@ -856,6 +859,8 @@ namespace gtsam {
       // TODO: functions below are dummy but necessary for the noiseModel::Base
       inline virtual Vector whiten(const Vector& v) const
       { Vector r = v; this->WhitenSystem(r); return r; }
+      inline virtual Matrix Whiten(const Matrix& A) const
+      { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
       inline virtual Vector unwhiten(const Vector& /*v*/) const
       { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
       inline virtual double distance(const Vector& v) const