From 035923449be1caec5861547eee01f5ee2aa51d35 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Wed, 18 Jan 2012 04:44:16 +0000
Subject: [PATCH] new predict variant predictQ

---
 gtsam.h                       |  3 +-
 gtsam/linear/KalmanFilter.cpp | 61 ++++++++++++++++++++++++++++++++++-
 gtsam/linear/KalmanFilter.h   | 15 ++++++---
 3 files changed, 72 insertions(+), 7 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index a4110ebd5..f5d6eabe8 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -311,12 +311,13 @@ class GaussianSequentialSolver {
 
 class KalmanFilter {
 	KalmanFilter(Vector x0, const SharedDiagonal& P0);
-	KalmanFilter(Vector x0, const Matrix& P0);
+	KalmanFilter(Vector x0, Matrix P0);
 	void print(string s) const;
 	Vector mean() const;
 	Matrix information() const;
 	Matrix covariance() const;
 	void predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model);
+	void predictQ(Matrix F, Matrix B, Vector u, Matrix Q);
 	void predict2(Matrix A0, Matrix A1, Vector b, const SharedDiagonal& model);
 	void update(Matrix H, Vector z, const SharedDiagonal& model);
 };
diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp
index 76ea27ccc..de4a2959f 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -104,17 +104,76 @@ namespace gtsam {
 		density_.reset(solve(factorGraph));
 	}
 
+	/* ************************************************************************* */
+	void KalmanFilter::predictQ(const Matrix& F, const Matrix& B, const Vector& u,
+			const Matrix& Q) {
+
+#ifndef NDEBUG
+		int n = F.cols();
+		assert(F.rows() == n);
+		assert(B.rows() == n);
+		assert(B.cols() == u.size());
+		assert(Q.rows() == n);
+		assert(Q.cols() == n);
+#endif
+
+		// Same scheme as in predict:
+		GaussianFactorGraph factorGraph;
+		factorGraph.push_back(density_->toFactor());
+
+		// The factor related to the motion model is defined as
+		// f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
+		// See documentation in HessianFactor, we have A1 = -F,  A2 = I_, b = B*u:
+		Vector b = B*u;
+		Matrix M = inverse(Q);
+		Matrix Ft = trans(F);
+		Matrix G12 = -Ft*M;
+		Matrix G11 = -G12*F;
+		Matrix G22 = M;
+		Vector g2 = M*b;
+		Vector g1 = -Ft*g2;
+		double f = dot(b,g2);
+		HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f));
+
+//#define DEBUG_PREDICTQ
+#ifdef DEBUG_PREDICTQ
+		gtsam::print(b);
+		gtsam::print(M);
+		gtsam::print(G11);
+		gtsam::print(G12);
+		gtsam::print(G22);
+		gtsam::print(g1);
+		gtsam::print(g2);
+		cout << f << endl;
+		factor->print("factor");
+#endif
+
+		factorGraph.push_back(factor);
+
+		// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
+		density_.reset(solve(factorGraph));
+	}
+
 	/* ************************************************************************* */
 	void KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, const Vector& b,
 			const SharedDiagonal& model) {
 
-		// Exactly the same schem as in predict:
+		// Same scheme as in predict:
 		GaussianFactorGraph factorGraph;
 		factorGraph.push_back(density_->toFactor());
 
 		// However, now the factor related to the motion model is defined as
 		// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
 		factorGraph.add(0, A0, 1, A1, b, model);
+#ifdef DEBUG_PREDICTQ
+		gtsam::print(Matrix(trans(A0)*A0));
+		gtsam::print(Matrix(trans(A0)*A1));
+		gtsam::print(Matrix(trans(A1)*A1));
+		gtsam::print(Matrix(trans(A0)*b));
+		gtsam::print(Matrix(trans(A1)*b));
+		cout << dot(b,b) << endl;
+#endif
+
 		density_.reset(solve(factorGraph));
 	}
 
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 61a49c621..27fbee344 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -83,13 +83,18 @@ namespace gtsam {
 		 *   In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
 		 *   where F is the state transition model/matrix, B is the control input model,
 		 *   and w is zero-mean, Gaussian white noise with covariance Q.
-		 *   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.
-		 *   In the current implementation, the noise model for w is restricted to a diagonal.
-		 *   TODO: allow for a G
 		 */
 		void predict(const Matrix& F, const Matrix& B, const Vector& u,
-				const SharedDiagonal& model);
+				const SharedDiagonal& modelQ);
+
+		/*
+		 *  Version of predict 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.
+		 */
+		void predictQ(const Matrix& F, const Matrix& B, const Vector& u,
+				const Matrix& Q);
 
 		/**
 		 * Predict the state P(x_{t+1}|Z^t)