From ba34a43b1c45b27f3d1de95191de32bb1d97dbc9 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 27 Jan 2012 22:20:43 +0000
Subject: [PATCH] wrapped new KF style and fixed test

---
 gtsam.h                         | 29 +++++++++++++++--------
 tests/matlab/testKalmanFilter.m | 41 ++++++++++++++++++---------------
 2 files changed, 42 insertions(+), 28 deletions(-)

diff --git a/gtsam.h b/gtsam.h
index 8fd178531..92616574d 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -256,6 +256,14 @@ class GaussianConditional {
 	bool equals(const GaussianConditional &cg, double tol) const;
 };
 
+class GaussianDensity {
+	GaussianDensity(int key, Vector d, Matrix R, Vector sigmas);
+	void print(string s) const;
+	Vector mean() const;
+	Matrix information() const;
+	Matrix covariance() const;
+};
+
 class GaussianBayesNet {
 	GaussianBayesNet();
 	void print(string s) const;
@@ -347,16 +355,19 @@ class GaussianSequentialSolver {
 };
 
 class KalmanFilter {
-	KalmanFilter(Vector x0, const SharedDiagonal& P0);
-	KalmanFilter(Vector x0, Matrix P0);
+	KalmanFilter(size_t n);
+	GaussianDensity* init(Vector x0, const SharedDiagonal& P0);
+	GaussianDensity* init(Vector x0, Matrix P0);
 	void print(string s) const;
-	Vector mean() const;
-	Matrix information() const;
-	Matrix covariance() const;
-	KalmanFilter predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model);
-	KalmanFilter predictQ(Matrix F, Matrix B, Vector u, Matrix Q);
-	KalmanFilter predict2(Matrix A0, Matrix A1, Vector b, const SharedDiagonal& model);
-	KalmanFilter update(Matrix H, Vector z, const SharedDiagonal& model);
+	static int step(GaussianDensity* p);
+	GaussianDensity* predict(GaussianDensity* p, Matrix F, Matrix B, Vector u,
+			const SharedDiagonal& modelQ);
+	GaussianDensity* predictQ(GaussianDensity* p, Matrix F, Matrix B, Vector u,
+			Matrix Q);
+	GaussianDensity* predict2(GaussianDensity* p, Matrix A0, Matrix A1, Vector b,
+			const SharedDiagonal& model);
+	GaussianDensity* update(GaussianDensity* p, Matrix H, Vector z,
+			const SharedDiagonal& model);
 };
 
 //*************************************************************************
diff --git a/tests/matlab/testKalmanFilter.m b/tests/matlab/testKalmanFilter.m
index 4a5b5694d..a074c75d3 100644
--- a/tests/matlab/testKalmanFilter.m
+++ b/tests/matlab/testKalmanFilter.m
@@ -47,31 +47,34 @@ expected3 = [3.0, 0.0]';
 P23 = inv(I22) + Q;
 I33 = inv(P23) + inv(R);
 
+%% Create an KalmanFilter object
+KF = KalmanFilter(2);
+
 %% Create the Kalman Filter initialization point
 x_initial = [0.0;0.0];
-P_initial = SharedDiagonal([0.1;0.1]);
+P_initial = 0.01*eye(2);
 
-%% Create an KalmanFilter object
-kalmanFilter = KalmanFilter(x_initial, P_initial)
-EQUALITY('expected0,kalmanFilter.mean', expected0,kalmanFilter.mean);
-EQUALITY('expected0,kalmanFilter.mean', P00,kalmanFilter.covariance);
+%% Create an KF object
+state = KF.init(x_initial, P_initial);
+EQUALITY('expected0,state.mean', expected0,state.mean);
+EQUALITY('expected0,state.mean', P00,state.covariance);
 
 %% Run iteration 1
-kalmanFilter.predict(F, B, u, modelQ);
-EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean);
-EQUALITY('P01,kalmanFilter.covariance', P01,kalmanFilter.covariance);
-kalmanFilter.update(H,z1,modelR);
-EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean);
-EQUALITY('I11,kalmanFilter.information', I11,kalmanFilter.information);
+state = KF.predict(state,F, B, u, modelQ);
+EQUALITY('expected1,state.mean', expected1,state.mean);
+EQUALITY('P01,state.covariance', P01,state.covariance);
+state = KF.update(state,H,z1,modelR);
+EQUALITY('expected1,state.mean', expected1,state.mean);
+EQUALITY('I11,state.information', I11,state.information);
 
 %% Run iteration 2
-kalmanFilter.predict(F, B, u, modelQ);
-EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean);
-kalmanFilter.update(H,z2,modelR);
-EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean);
+state = KF.predict(state,F, B, u, modelQ);
+EQUALITY('expected2,state.mean', expected2,state.mean);
+state = KF.update(state,H,z2,modelR);
+EQUALITY('expected2,state.mean', expected2,state.mean);
 
 %% Run iteration 3
-kalmanFilter.predict(F, B, u, modelQ);
-EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean);
-kalmanFilter.update(H,z3,modelR);
-EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean);
+state = KF.predict(state,F, B, u, modelQ);
+EQUALITY('expected3,state.mean', expected3,state.mean);
+state = KF.update(state,H,z3,modelR);
+EQUALITY('expected3,state.mean', expected3,state.mean);