KF update method that takes full covariance instead of diagonal

gtsam/linear/KalmanFilter.cpp

@@ -144,6 +144,17 @@ namespace gtsam {
 		return fuse(p, new JacobianFactor(k, H, z, model), useQR());
 	}
 
+  /* ************************************************************************* */
+  KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, const Vector& z,
+      const Matrix& Q) {
+    Index k = step(p);
+    Matrix M = inverse(Q), Ht = trans(H);
+    Matrix G = Ht * M * H;
+    Vector g = Ht * M * z;
+    double f = dot(z, M * z);
+    return fuse(p, new HessianFactor(k, G, g, f), useQR());
+  }
+
 /* ************************************************************************* */
 
 } // \namespace gtsam

gtsam/linear/KalmanFilter.h

@@ -136,6 +136,15 @@ namespace gtsam {
 		 */
 		State update(const State& p, const Matrix& H, const Vector& z,
 				const SharedDiagonal& model);
+
+    /*
+     *  Version of update with full covariance
+     *  Q is normally derived as G*w*G^T where w models uncertainty of some
+     *  physical property, such as velocity or acceleration, and G is derived from physics.
+     *  This version allows more realistic models than a diagonal covariance matrix.
+     */
+    State update(const State& p, const Matrix& H, const Vector& z,
+        const Matrix& Q);
 	};
 
 } // \namespace gtsam

gtsam/linear/tests/testKalmanFilter.cpp

@@ -269,6 +269,22 @@ TEST( KalmanFilter, QRvsCholesky ) {
 			-0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8);
 	EXPECT(assert_equal(expected2, pa2->covariance(), 1e-7));
 	EXPECT(assert_equal(expected2, pb2->covariance(), 1e-7));
+
+	// do the above update again, this time with a full Matrix Q
+	Matrix modelQ = diag(emul(sigmas,sigmas));
+  KalmanFilter::State pa3 = kfa.update(pa, H, z, modelQ);
+  KalmanFilter::State pb3 = kfb.update(pb, H, z, modelQ);
+
+  // Check that they yield the same mean and information matrix
+  EXPECT(assert_equal(pa3->mean(), pb3->mean()));
+  EXPECT(assert_equal(pa3->information(), pb3->information(), 1e-7));
+
+  // and in addition attain the correct mean and covariance
+  EXPECT(assert_equal(expectedMean2, pa3->mean(), 1e-4));
+  EXPECT(assert_equal(expectedMean2, pb3->mean(), 1e-4));
+
+  EXPECT(assert_equal(expected2, pa3->covariance(), 1e-7));
+  EXPECT(assert_equal(expected2, pb3->covariance(), 1e-7));
 }
 
 /* ************************************************************************* */