predict2

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);
 };
 

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) {

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}