diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index fdbfe1d0e..da201de2d 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -25,11 +25,15 @@ #include #include #include +#include namespace gtsam { + using namespace std; + /// Auxiliary function to solve factor graph and return pointer to root conditional - GaussianConditional* solve(GaussianFactorGraph& factorGraph, bool useQR) { + GaussianConditional::shared_ptr solve(GaussianFactorGraph& factorGraph, + bool useQR) { // Solve the factor graph GaussianSequentialSolver solver(factorGraph, useQR); @@ -37,14 +41,12 @@ namespace gtsam { // As this is a filter, all we need is the posterior P(x_t), // so we just keep the root of the Bayes net - // We need to create a new density, because we always keep the index at 0 - const GaussianConditional::shared_ptr& r = bayesNet->back(); - return new GaussianConditional(0, r->get_d(), r->get_R(), r->get_sigmas()); + return bayesNet->back(); } /* ************************************************************************* */ - KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density, - Factorization method) : + KalmanFilter::KalmanFilter(size_t n, + const GaussianConditional::shared_ptr& density, Factorization method) : n_(n), I_(eye(n_, n_)), method_(method), density_(density) { } @@ -70,7 +72,7 @@ namespace gtsam { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma - density_.reset(solve(factorGraph, method_ == QR)); + density_ = solve(factorGraph, method_ == QR); } /* ************************************************************************* */ @@ -83,17 +85,29 @@ namespace gtsam { // 0.5*(x-x0)'*inv(Sigma)*(x-x0) HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); factorGraph.push_back(factor); - density_.reset(solve(factorGraph, method_ == QR)); + density_ = solve(factorGraph, method_ == QR); + } + + /* ************************************************************************* */ + void KalmanFilter::print(const string& s) const { + cout << s << "\n"; + density_->print("density: "); + Vector m = mean(); + Matrix P = covariance(); + gtsam::print(m, "mean: "); + gtsam::print(P, "covariance: "); } /* ************************************************************************* */ Vector KalmanFilter::mean() const { // Solve for mean - Index nVars = 1; - VectorValues x(nVars, n_); + VectorValues x; + Index k = step(); + // a VectorValues that only has a value for k: cannot be printed + x.insert(k, Vector(n_)); density_->rhs(x); density_->solveInPlace(x); - return x[0]; + return x[k]; } /* ************************************************************************* */ @@ -112,7 +126,8 @@ namespace gtsam { // 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 - return add(new JacobianFactor(0, -F, 1, I_, B * u, model)); + Index k = step(); + return add(new JacobianFactor(k, -F, k+1, I_, B * u, model)); } /* ************************************************************************* */ @@ -136,7 +151,8 @@ namespace gtsam { Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; Vector b = B * u, g2 = M * b, g1 = -Ft * g2; double f = dot(b, g2); - return add(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f)); + Index k = step(); + return add(new HessianFactor(k, k+1, G11, G12, g1, G22, g2, f)); } /* ************************************************************************* */ @@ -144,7 +160,8 @@ namespace gtsam { const Vector& b, const SharedDiagonal& model) { // Nhe factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 - return add(new JacobianFactor(0, A0, 1, A1, b, model)); + Index k = step(); + return add(new JacobianFactor(k, A0, k+1, A1, b, model)); } /* ************************************************************************* */ @@ -153,7 +170,8 @@ namespace gtsam { // The factor related to the measurements would be defined as // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T - return add(new JacobianFactor(0, H, z, model)); + Index k = step(); + return add(new JacobianFactor(k, H, z, model)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index acec1b97a..51e2da3ae 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -57,8 +57,8 @@ namespace gtsam { GaussianConditional::shared_ptr density_; /// private constructor - KalmanFilter(size_t n, GaussianConditional* density, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION); + KalmanFilter(size_t n, const GaussianConditional::shared_ptr& density, + Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION); /// add a new factor and marginalize to new Kalman filter KalmanFilter add(GaussianFactor* newFactor); @@ -84,13 +84,10 @@ namespace gtsam { KALMANFILTER_DEFAULT_FACTORIZATION); /// print - void print(const std::string& s = "") const { - std::cout << s << "\n"; - Vector m = mean(); - Matrix P = covariance(); - gtsam::print(m, "mean: "); - gtsam::print(P, "covariance: "); - } + void print(const std::string& s = "") const; + + /** Return step index k, starts at 0, incremented at each predict. */ + Index step() const { return density_->firstFrontalKey();} /** Return mean of posterior P(x|Z) at given all measurements Z */ Vector mean() const; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index da846fbcd..98964e377 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -18,10 +18,9 @@ */ #include -#include #include #include -#include +#include #include using namespace std; @@ -115,9 +114,11 @@ TEST( KalmanFilter, linear1 ) { // Run iteration 3 KalmanFilter KF3p = KF2.predict(F, B, u, modelQ); EXPECT(assert_equal(expected3,KF3p.mean())); + LONGS_EQUAL(3,KF3p.step()); KalmanFilter KF3 = KF3p.update(H,z3,modelR); EXPECT(assert_equal(expected3,KF3.mean())); + LONGS_EQUAL(3,KF3.step()); } /* ************************************************************************* */ @@ -192,9 +193,11 @@ TEST( KalmanFilter, QRvsCholesky ) { // Create two KalmanFilter using different factorization method and compare KalmanFilter KFa = KalmanFilter(mean, covariance,KalmanFilter::QR).predictQ(Psi_k,B,u,dt_Q_k); KalmanFilter KFb = KalmanFilter(mean, covariance,KalmanFilter::LDL).predictQ(Psi_k,B,u,dt_Q_k); + + // Check that they yield the same result EXPECT(assert_equal(KFa.mean(),KFb.mean())); -// EXPECT(assert_equal(KFa.information(),KFb.information())); -// EXPECT(assert_equal(KFa.covariance(),KFb.covariance())); + EXPECT(assert_equal(KFa.information(),KFb.information(),1e-7)); + EXPECT(assert_equal(KFa.covariance(),KFb.covariance(),1e-7)); } /* ************************************************************************* */