From 4aa4b8b938ae818d33509a9b74f8037d88937269 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 20 Jan 2012 05:07:06 +0000 Subject: [PATCH] Kalman Filter now functional (i.e., not imperative) --- gtsam/linear/KalmanFilter.cpp | 34 +++++++---------- gtsam/linear/KalmanFilter.h | 17 +++++---- gtsam/linear/tests/testKalmanFilter.cpp | 50 +++++++++++++------------ 3 files changed, 50 insertions(+), 51 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 324b054a9..7ea0726bb 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -32,7 +32,7 @@ namespace gtsam { GaussianConditional* solve(GaussianFactorGraph& factorGraph) { // Solve the factor graph - const bool useQR = false; // make sure we use QR (numerically stable) + const bool useQR = true; // make sure we use QR (numerically stable) GaussianSequentialSolver solver(factorGraph, useQR); GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); @@ -43,6 +43,11 @@ namespace gtsam { return new GaussianConditional(0, r->get_d(), r->get_R(), r->get_sigmas()); } + /* ************************************************************************* */ + KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density) : + n_(n), I_(eye(n_, n_)),density_(density) { + } + /* ************************************************************************* */ KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0) : n_(x0.size()), I_(eye(n_, n_)) { @@ -86,7 +91,7 @@ namespace gtsam { } /* ************************************************************************* */ - void KalmanFilter::predict(const Matrix& F, const Matrix& B, const Vector& u, + KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B, const Vector& u, const SharedDiagonal& model) { // We will create a small factor graph f1-(x0)-f2-(x1) // where factor f1 is just the prior from time t0, P(x0) @@ -101,11 +106,11 @@ namespace gtsam { factorGraph.add(0, -F, 1, I_, B * u, model); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - density_.reset(solve(factorGraph)); + return KalmanFilter(n_,solve(factorGraph)); } /* ************************************************************************* */ - void KalmanFilter::predictQ(const Matrix& F, const Matrix& B, const Vector& u, + KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B, const Vector& u, const Matrix& Q) { #ifndef NDEBUG @@ -132,17 +137,12 @@ namespace gtsam { HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f)); factorGraph.push_back(factor); -#ifdef DEBUG_PREDICTQ - Matrix AbtAb = factorGraph.denseHessian(); - gtsam::print(AbtAb); -#endif - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - density_.reset(solve(factorGraph)); + return KalmanFilter(n_,solve(factorGraph)); } /* ************************************************************************* */ - void KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, const Vector& b, + KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, const Vector& b, const SharedDiagonal& model) { // Same scheme as in predict: @@ -152,17 +152,11 @@ namespace gtsam { // 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 - Matrix AbtAb = factorGraph.denseHessian(); - gtsam::print(AbtAb); -#endif - - density_.reset(solve(factorGraph)); + return KalmanFilter(n_,solve(factorGraph)); } /* ************************************************************************* */ - void KalmanFilter::update(const Matrix& H, const Vector& z, + KalmanFilter KalmanFilter::update(const Matrix& H, const Vector& z, const SharedDiagonal& model) { // We will create a small factor graph f1-(x0)-f2 // where factor f1 is the predictive density @@ -178,7 +172,7 @@ namespace gtsam { factorGraph.add(0, H, z, model); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - density_.reset(solve(factorGraph)); + return KalmanFilter(n_,solve(factorGraph)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 27fbee344..17db2ee6b 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -33,12 +33,15 @@ namespace gtsam { class KalmanFilter { private: - size_t n_; /** dimensionality of state */ - Matrix I_; /** identity matrix of size n*n */ + const size_t n_; /** dimensionality of state */ + const Matrix I_; /** identity matrix of size n*n */ - /** The Kalman filter posterior density is a Gaussian Conditional with no parents */ + /// The Kalman filter posterior density is a Gaussian Conditional with no parents GaussianConditional::shared_ptr density_; + /// private constructor + KalmanFilter(size_t n, GaussianConditional* density); + public: /** @@ -84,7 +87,7 @@ namespace gtsam { * 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. */ - void predict(const Matrix& F, const Matrix& B, const Vector& u, + KalmanFilter predict(const Matrix& F, const Matrix& B, const Vector& u, const SharedDiagonal& modelQ); /* @@ -93,7 +96,7 @@ namespace gtsam { * 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, + KalmanFilter predictQ(const Matrix& F, const Matrix& B, const Vector& u, const Matrix& Q); /** @@ -104,7 +107,7 @@ namespace gtsam { * 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, + KalmanFilter predict2(const Matrix& A0, const Matrix& A1, const Vector& b, const SharedDiagonal& model); /** @@ -115,7 +118,7 @@ namespace gtsam { * Gaussian white noise with covariance R. * Currently, R is restricted to diagonal Gaussians (model parameter) */ - void update(const Matrix& H, const Vector& z, const SharedDiagonal& model); + KalmanFilter update(const Matrix& H, const Vector& z, const SharedDiagonal& model); }; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index bc3c44f27..b4c0ad94d 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -91,30 +91,33 @@ TEST( KalmanFilter, linear1 ) { State x_initial(0.0,0.0); SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,0.1); - // Create an KalmanFilter object - KalmanFilter kalmanFilter(x_initial, P_initial); - EXPECT(assert_equal(expected0,kalmanFilter.mean())); - EXPECT(assert_equal(P00,kalmanFilter.covariance())); + // Create initial KalmanFilter object + KalmanFilter KF0(x_initial, P_initial); + EXPECT(assert_equal(expected0,KF0.mean())); + EXPECT(assert_equal(P00,KF0.covariance())); // Run iteration 1 - kalmanFilter.predict(F, B, u, modelQ); - EXPECT(assert_equal(expected1,kalmanFilter.mean())); - EXPECT(assert_equal(P01,kalmanFilter.covariance())); - kalmanFilter.update(H,z1,modelR); - EXPECT(assert_equal(expected1,kalmanFilter.mean())); - EXPECT(assert_equal(I11,kalmanFilter.information())); + KalmanFilter KF1p = KF0.predict(F, B, u, modelQ); + EXPECT(assert_equal(expected1,KF1p.mean())); + EXPECT(assert_equal(P01,KF1p.covariance())); + + KalmanFilter KF1 = KF1p.update(H,z1,modelR); + EXPECT(assert_equal(expected1,KF1.mean())); + EXPECT(assert_equal(I11,KF1.information())); // Run iteration 2 (with full covariance) - kalmanFilter.predictQ(F, B, u, Q); - EXPECT(assert_equal(expected2,kalmanFilter.mean())); - kalmanFilter.update(H,z2,modelR); - EXPECT(assert_equal(expected2,kalmanFilter.mean())); + KalmanFilter KF2p = KF1.predictQ(F, B, u, Q); + EXPECT(assert_equal(expected2,KF2p.mean())); + + KalmanFilter KF2 = KF2p.update(H,z2,modelR); + EXPECT(assert_equal(expected2,KF2.mean())); // Run iteration 3 - kalmanFilter.predict(F, B, u, modelQ); - EXPECT(assert_equal(expected3,kalmanFilter.mean())); - kalmanFilter.update(H,z3,modelR); - EXPECT(assert_equal(expected3,kalmanFilter.mean())); + KalmanFilter KF3p = KF2.predict(F, B, u, modelQ); + EXPECT(assert_equal(expected3,KF3p.mean())); + + KalmanFilter KF3 = KF3p.update(H,z3,modelR); + EXPECT(assert_equal(expected3,KF3.mean())); } /* ************************************************************************* */ @@ -133,18 +136,17 @@ TEST( KalmanFilter, predict ) { SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,1); // Create two KalmanFilter objects - KalmanFilter kalmanFilter1(x_initial, P_initial); - KalmanFilter kalmanFilter2(x_initial, P_initial); + KalmanFilter KF0(x_initial, P_initial); // Ensure predictQ and predict2 give same answer for non-trivial inputs - kalmanFilter1.predictQ(F, B, u, Q); + KalmanFilter KFa = KF0.predictQ(F, B, u, Q); // We have A1 = -F, A2 = I_, b = B*u, pre-multipled with R to match Q noise model Matrix A1 = -R*F, A2 = R; Vector b = R*B*u; SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0); - kalmanFilter2.predict2(A1,A2,b,nop); - EXPECT(assert_equal(kalmanFilter1.mean(),kalmanFilter2.mean())); - EXPECT(assert_equal(kalmanFilter1.covariance(),kalmanFilter2.covariance())); + KalmanFilter KFb = KF0.predict2(A1,A2,b,nop); + EXPECT(assert_equal(KFa.mean(),KFb.mean())); + EXPECT(assert_equal(KFa.covariance(),KFb.covariance())); } /* ************************************************************************* */