From 02d75c0db8cb8672bef80b0c4fc03acad5df6e31 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 4 Nov 2011 14:10:32 +0000
Subject: [PATCH] predict2

---
 gtsam.h                       |  1 +
 gtsam/linear/KalmanFilter.cpp | 14 ++++++++++++++
 gtsam/linear/KalmanFilter.h   | 11 +++++++++++
 3 files changed, 26 insertions(+)

diff --git a/gtsam.h b/gtsam.h
index 7eb0a27b3..45c13ec72 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -122,6 +122,7 @@ class KalmanFilter {
 	Matrix information() const;
 	Matrix covariance() const;
 	void predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model);
+	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 bc1658fb0..8b37f8cb3 100644
--- a/gtsam/linear/KalmanFilter.cpp
+++ b/gtsam/linear/KalmanFilter.cpp
@@ -90,6 +90,20 @@ namespace gtsam {
 		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:
+		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);
+		density_.reset(solve(factorGraph));
+	}
+
 	/* ************************************************************************* */
 	void KalmanFilter::update(const Matrix& H, const Vector& z,
 			const SharedDiagonal& model) {
diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h
index 5aa697bec..eeee0dccc 100644
--- a/gtsam/linear/KalmanFilter.h
+++ b/gtsam/linear/KalmanFilter.h
@@ -77,6 +77,17 @@ namespace gtsam {
 		void predict(const Matrix& F, const Matrix& B, const Vector& u,
 				const SharedDiagonal& model);
 
+		/**
+		 * Predict the state P(x_{t+1}|Z^t)
+		 *   In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
+		 *   After the call, that is the density that can be queried.
+		 * Details and parameters:
+		 *   This version of predict takes GaussianFactor motion model [A0 A1 b]
+		 *   with an optional noise model.
+		 */
+		void predict2(const Matrix& A0, const Matrix& A1, const Vector& b,
+				const SharedDiagonal& model);
+
 		/**
 		 * Update Kalman filter with a measurement
 		 * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t}