From 6df52db2c9edd578906b8d3d8054e81db02e2af7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 22 Jan 2012 06:27:54 +0000 Subject: [PATCH] Kalman filter now functional, manipulates a functional state. --- gtsam/linear/KalmanFilter.cpp | 93 ++++-------- gtsam/linear/KalmanFilter.h | 73 +++++----- gtsam/linear/tests/testKalmanFilter.cpp | 180 ++++++++++++++---------- 3 files changed, 165 insertions(+), 181 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index da201de2d..e0977d632 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -32,7 +32,7 @@ namespace gtsam { using namespace std; /// Auxiliary function to solve factor graph and return pointer to root conditional - GaussianConditional::shared_ptr solve(GaussianFactorGraph& factorGraph, + KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, bool useQR) { // Solve the factor graph @@ -41,98 +41,64 @@ 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 - return bayesNet->back(); + GaussianConditional::shared_ptr conditional = bayesNet->back(); + return *conditional; } /* ************************************************************************* */ - KalmanFilter::KalmanFilter(size_t n, - const GaussianConditional::shared_ptr& density, Factorization method) : - n_(n), I_(eye(n_, n_)), method_(method), density_(density) { - } - - /* ************************************************************************* */ - KalmanFilter KalmanFilter::add(GaussianFactor* newFactor) { + KalmanFilter::State fuse(const KalmanFilter::State& p, + GaussianFactor* newFactor, bool useQR) { // Create a factor graph GaussianFactorGraph factorGraph; // push back previous solution and new factor - factorGraph.push_back(density_->toFactor()); + factorGraph.push_back(p.toFactor()); factorGraph.push_back(GaussianFactor::shared_ptr(newFactor)); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return KalmanFilter(n_, solve(factorGraph, method_ == QR), method_); + return solve(factorGraph, useQR); } /* ************************************************************************* */ - KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0, - Factorization method) : - n_(x0.size()), I_(eye(n_, n_)), method_(method) { + KalmanFilter::State KalmanFilter::init(const Vector& x0, + const SharedDiagonal& P0) { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma - density_ = solve(factorGraph, method_ == QR); + return solve(factorGraph, useQR()); } /* ************************************************************************* */ - KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0, - Factorization method) : - n_(x.size()), I_(eye(n_, n_)), method_(method) { + KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; // 0.5*(x-x0)'*inv(Sigma)*(x-x0) HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); factorGraph.push_back(factor); - density_ = solve(factorGraph, method_ == QR); + return solve(factorGraph, useQR()); } /* ************************************************************************* */ 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: "); + cout << s << ", " << n_ << "-dimensional Kalman filter\n"; } /* ************************************************************************* */ - Vector KalmanFilter::mean() const { - // Solve for mean - 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[k]; - } - - /* ************************************************************************* */ - Matrix KalmanFilter::information() const { - return density_->computeInformation(); - } - - /* ************************************************************************* */ - Matrix KalmanFilter::covariance() const { - return inverse(information()); - } - - /* ************************************************************************* */ - KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& model) { + KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const SharedDiagonal& model) { // 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 - Index k = step(); - return add(new JacobianFactor(k, -F, k+1, I_, B * u, model)); + Index k = step(p); + return fuse(p, new JacobianFactor(k, -F, k + 1, I_, B * u, model), useQR()); } /* ************************************************************************* */ - KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q) { + KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const Matrix& Q) { #ifndef NDEBUG int n = F.cols(); @@ -151,27 +117,28 @@ 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); - Index k = step(); - return add(new HessianFactor(k, k+1, G11, G12, g1, G22, g2, f)); + Index k = step(p); + return fuse(p, new HessianFactor(k, k + 1, G11, G12, g1, G22, g2, f), + useQR()); } /* ************************************************************************* */ - KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) { + KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, + const Matrix& A1, 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 - Index k = step(); - return add(new JacobianFactor(k, A0, k+1, A1, b, model)); + Index k = step(p); + return fuse(p, new JacobianFactor(k, A0, k + 1, A1, b, model), useQR()); } /* ************************************************************************* */ - KalmanFilter KalmanFilter::update(const Matrix& H, const Vector& z, - const SharedDiagonal& model) { + KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, + const Vector& z, const SharedDiagonal& model) { // 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 - Index k = step(); - return add(new JacobianFactor(k, H, z, model)); + Index k = step(p); + return fuse(p, new JacobianFactor(k, H, z, model), useQR()); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 51e2da3ae..b9515e4c1 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -20,8 +20,7 @@ * @author Frank Dellaert */ -#include -#include +#include #ifndef KALMANFILTER_DEFAULT_FACTORIZATION #define KALMANFILTER_DEFAULT_FACTORIZATION QR @@ -33,7 +32,13 @@ namespace gtsam { class SharedGaussian; /** - * Linear Kalman Filter + * Kalman Filter class + * + * Knows how to maintain a Gaussian density under linear-Gaussian motion and + * measurement models. It uses the square-root information form, as usual in GTSAM. + * + * The filter is functional, in that it does not have state: you call init() to create + * an initial state, then predict() and update() that create new states out of old. */ class KalmanFilter { @@ -47,56 +52,42 @@ namespace gtsam { QR, LDL }; + typedef GaussianDensity State; + private: const size_t n_; /** dimensionality of state */ const Matrix I_; /** identity matrix of size n*n */ const Factorization method_; /** algorithm */ - /// The Kalman filter posterior density is a Gaussian Conditional with no parents - GaussianConditional::shared_ptr density_; - - /// private constructor - 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); + bool useQR() const { return method_==QR; } public: + // private constructor + KalmanFilter(size_t n, Factorization method = + KALMANFILTER_DEFAULT_FACTORIZATION) : + n_(n), I_(eye(n_, n_)), method_(method) { + } + /** - * Constructor from prior density at time k=0 - * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} + * Create initial state, i.e., prior density at time k=0 + * In Kalman Filter notation, this are is x_{0|0} and P_{0|0} * @param x0 estimate at time 0 * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' */ - KalmanFilter(const Vector& x0, const SharedDiagonal& P0, - Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION); + State init(const Vector& x0, const SharedDiagonal& P0); - /** - * Constructor from prior density at time k=0 - * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, full Gaussian - */ - KalmanFilter(const Vector& x0, const Matrix& P0, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION); + /// version of init with a full covariance matrix + State init(const Vector& x0, const Matrix& P0); /// print 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; - - /** Return information matrix of posterior P(x|Z) at given all measurements Z */ - Matrix information() const; - - /** Return covariance of posterior P(x|Z) at given all measurements Z */ - Matrix covariance() const; + static Index step(const State& p) { + return p.firstFrontalKey(); + } /** * Predict the state P(x_{t+1}|Z^t) @@ -107,8 +98,8 @@ 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. */ - KalmanFilter predict(const Matrix& F, const Matrix& B, const Vector& u, - const SharedDiagonal& modelQ); + KalmanFilter::State predict(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const SharedDiagonal& modelQ); /* * Version of predict with full covariance @@ -116,8 +107,8 @@ 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. */ - KalmanFilter predictQ(const Matrix& F, const Matrix& B, const Vector& u, - const Matrix& Q); + KalmanFilter::State predictQ(const State& p, const Matrix& F, + const Matrix& B, const Vector& u, const Matrix& Q); /** * Predict the state P(x_{t+1}|Z^t) @@ -127,8 +118,8 @@ namespace gtsam { * This version of predict takes GaussianFactor motion model [A0 A1 b] * with an optional noise model. */ - KalmanFilter predict2(const Matrix& A0, const Matrix& A1, const Vector& b, - const SharedDiagonal& model); + KalmanFilter::State predict2(const State& p, const Matrix& A0, + const Matrix& A1, const Vector& b, const SharedDiagonal& model); /** * Update Kalman filter with a measurement @@ -138,7 +129,7 @@ namespace gtsam { * Gaussian white noise with covariance R. * Currently, R is restricted to diagonal Gaussians (model parameter) */ - KalmanFilter update(const Matrix& H, const Vector& z, + KalmanFilter::State update(const State& p, const Matrix& H, const Vector& z, const SharedDiagonal& model); }; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index d906b4372..65fee2e98 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -37,42 +37,50 @@ struct State: Vector { /* ************************************************************************* */ TEST( KalmanFilter, constructor ) { - // Create the Kalman Filter initialization point - State x_initial(0.0,0.0); - SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2,0.1); - // Create an KalmanFilter object - KalmanFilter kf1(x_initial, P1); - Matrix Sigma = Matrix_(2,2,0.01,0.0,0.0,0.01); - EXPECT(assert_equal(Sigma,kf1.covariance())); + // Create a Kalman filter of dimension 2 + KalmanFilter kf1(2); + + // Create inital mean/covariance + State x_initial(0.0, 0.0); + SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2, 0.1); + + // Get initial state by passing initial mean/covariance to the p + KalmanFilter::State p1 = kf1.init(x_initial, P1); + + // Assert it has the correct mean, covariance and information + EXPECT(assert_equal(x_initial, p1.mean())); + Matrix Sigma = Matrix_(2, 2, 0.01, 0.0, 0.0, 0.01); + EXPECT(assert_equal(Sigma, p1.covariance())); + EXPECT(assert_equal(inverse(Sigma), p1.information())); // Create one with a sharedGaussian - KalmanFilter kf2(x_initial, Sigma); - EXPECT(assert_equal(Sigma,kf2.covariance())); + KalmanFilter::State p2 = kf1.init(x_initial, Sigma); + EXPECT(assert_equal(Sigma, p2.covariance())); // Now make sure both agree - EXPECT(assert_equal(kf1.covariance(),kf2.covariance())); + EXPECT(assert_equal(p1.covariance(), p2.covariance())); } /* ************************************************************************* */ TEST( KalmanFilter, linear1 ) { // Create the controls and measurement properties for our example - Matrix F = eye(2,2); - Matrix B = eye(2,2); + Matrix F = eye(2, 2); + Matrix B = eye(2, 2); Vector u = Vector_(2, 1.0, 0.0); SharedDiagonal modelQ = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix Q = 0.01*eye(2,2); - Matrix H = eye(2,2); + Matrix Q = 0.01*eye(2, 2); + Matrix H = eye(2, 2); State z1(1.0, 0.0); State z2(2.0, 0.0); State z3(3.0, 0.0); SharedDiagonal modelR = noiseModel::Isotropic::Sigma(2, 0.1); - Matrix R = 0.01*eye(2,2); + Matrix R = 0.01*eye(2, 2); // Create the set of expected output TestValues State expected0(0.0, 0.0); - Matrix P00 = 0.01*eye(2,2); + Matrix P00 = 0.01*eye(2, 2); State expected1(1.0, 0.0); Matrix P01 = P00 + Q; @@ -86,68 +94,74 @@ TEST( KalmanFilter, linear1 ) { Matrix P23 = inverse(I22) + Q; Matrix I33 = inverse(P23) + inverse(R); + // Create a Kalman filter of dimension 2 + KalmanFilter kf(2); + // Create the Kalman Filter initialization point - State x_initial(0.0,0.0); - SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,0.1); + State x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 0.1); // Create initial KalmanFilter object - KalmanFilter KF0(x_initial, P_initial); - EXPECT(assert_equal(expected0,KF0.mean())); - EXPECT(assert_equal(P00,KF0.covariance())); + KalmanFilter::State p0 = kf.init(x_initial, P_initial); + EXPECT(assert_equal(expected0, p0.mean())); + EXPECT(assert_equal(P00, p0.covariance())); // Run iteration 1 - KalmanFilter KF1p = KF0.predict(F, B, u, modelQ); - EXPECT(assert_equal(expected1,KF1p.mean())); - EXPECT(assert_equal(P01,KF1p.covariance())); + KalmanFilter::State p1p = kf.predict(p0, F, B, u, modelQ); + EXPECT(assert_equal(expected1, p1p.mean())); + EXPECT(assert_equal(P01, p1p.covariance())); - KalmanFilter KF1 = KF1p.update(H,z1,modelR); - EXPECT(assert_equal(expected1,KF1.mean())); - EXPECT(assert_equal(I11,KF1.information())); + KalmanFilter::State p1 = kf.update(p1p, H, z1, modelR); + EXPECT(assert_equal(expected1, p1.mean())); + EXPECT(assert_equal(I11, p1.information())); // Run iteration 2 (with full covariance) - KalmanFilter KF2p = KF1.predictQ(F, B, u, Q); - EXPECT(assert_equal(expected2,KF2p.mean())); + KalmanFilter::State p2p = kf.predictQ(p1, F, B, u, Q); + EXPECT(assert_equal(expected2, p2p.mean())); - KalmanFilter KF2 = KF2p.update(H,z2,modelR); - EXPECT(assert_equal(expected2,KF2.mean())); + KalmanFilter::State p2 = kf.update(p2p, H, z2, modelR); + EXPECT(assert_equal(expected2, p2.mean())); // Run iteration 3 - KalmanFilter KF3p = KF2.predict(F, B, u, modelQ); - EXPECT(assert_equal(expected3,KF3p.mean())); - LONGS_EQUAL(3,KF3p.step()); + KalmanFilter::State p3p = kf.predict(p2, F, B, u, modelQ); + EXPECT(assert_equal(expected3, p3p.mean())); + LONGS_EQUAL(3, KalmanFilter::step(p3p)); - KalmanFilter KF3 = KF3p.update(H,z3,modelR); - EXPECT(assert_equal(expected3,KF3.mean())); - LONGS_EQUAL(3,KF3.step()); + KalmanFilter::State p3 = kf.update(p3p, H, z3, modelR); + EXPECT(assert_equal(expected3, p3.mean())); + LONGS_EQUAL(3, KalmanFilter::step(p3)); } /* ************************************************************************* */ TEST( KalmanFilter, predict ) { // Create dynamics model - Matrix F = Matrix_(2,2, 1.0,0.1, 0.2,1.1); - Matrix B = Matrix_(2,3, 1.0,0.1,0.2, 1.1,1.2,0.8); + Matrix F = Matrix_(2, 2, 1.0, 0.1, 0.2, 1.1); + Matrix B = Matrix_(2, 3, 1.0, 0.1, 0.2, 1.1, 1.2, 0.8); Vector u = Vector_(3, 1.0, 0.0, 2.0); - Matrix R = Matrix_(2,2, 1.0,0.5, 0.0,3.0); + Matrix R = Matrix_(2, 2, 1.0, 0.5, 0.0, 3.0); Matrix M = trans(R)*R; Matrix Q = inverse(M); - // Create the Kalman Filter initialization point - State x_initial(0.0,0.0); - SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,1); + // Create a Kalman filter of dimension 2 + KalmanFilter kf(2); - // Create two KalmanFilter objects - KalmanFilter KF0(x_initial, P_initial); + // Create the Kalman Filter initialization point + State x_initial(0.0, 0.0); + SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2, 1); + + // Create initial KalmanFilter state + KalmanFilter::State p0 = kf.init(x_initial, P_initial); // Ensure predictQ and predict2 give same answer for non-trivial inputs - 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 + KalmanFilter::State pa = kf.predictQ(p0, 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); - KalmanFilter KFb = KF0.predict2(A1,A2,b,nop); - EXPECT(assert_equal(KFa.mean(),KFb.mean())); - EXPECT(assert_equal(KFa.covariance(),KFb.covariance())); + KalmanFilter::State pb = kf.predict2(p0, A1, A2, b, nop); + EXPECT(assert_equal(pa.mean(), pb.mean())); + EXPECT(assert_equal(pa.covariance(), pb.covariance())); } /* ************************************************************************* */ @@ -155,7 +169,7 @@ TEST( KalmanFilter, predict ) { TEST( KalmanFilter, QRvsCholesky ) { Vector mean = ones(9); - Matrix covariance = 1e-6*Matrix_(9,9, + Matrix covariance = 1e-6*Matrix_(9, 9, 15.0, -6.2, 0.0, 0.0, 0.0, 0.0, 0.0, 63.8, -0.6, -6.2, 21.9, -0.0, 0.0, 0.0, 0.0, -63.8, -0.0, -0.1, 0.0, -0.0, 100.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0, @@ -166,8 +180,15 @@ TEST( KalmanFilter, QRvsCholesky ) { 63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, -0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0); + // Create two Kalman filter of dimension 9, one using QR the other LDL + KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::LDL); + + // create corresponding initial states + KalmanFilter::State p0a = kfa.init(mean, covariance); + KalmanFilter::State p0b = kfb.init(mean, covariance); + // Set up dynamics update - Matrix Psi_k = 1e-6*Matrix_(9,9, + Matrix Psi_k = 1e-6*Matrix_(9, 9, 1000000.0, 0.0, 0.0, -19200.0, 600.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 600.0, 19200.0, 200.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, -0.0, -200.0, 19200.0, 0.0, 0.0, 0.0, @@ -177,9 +198,9 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0); - Matrix B = zeros(9,1); + Matrix B = zeros(9, 1); Vector u = zero(1); - Matrix dt_Q_k = 1e-6*Matrix_(9,9, + Matrix dt_Q_k = 1e-6*Matrix_(9, 9, 33.7, 3.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1, 126.4, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.0, -0.3, 88.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, @@ -190,19 +211,19 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 22.2); - // 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); + // Do prediction step + KalmanFilter::State pa = kfa.predictQ(p0a, Psi_k, B, u, dt_Q_k); + KalmanFilter::State pb = kfb.predictQ(p0b, Psi_k, B, u, dt_Q_k); // Check that they yield the same mean and information matrix - EXPECT(assert_equal(KFa.mean(),KFb.mean())); - EXPECT(assert_equal(KFa.information(),KFb.information(),1e-7)); + EXPECT(assert_equal(pa.mean(), pb.mean())); + EXPECT(assert_equal(pa.information(), pb.information(), 1e-7)); // and in addition attain the correct covariance Vector expectedMean = Vector_(9, 0.9814, 1.0200, 1.0190, 1., 1., 1., 1., 1., 1.); - EXPECT(assert_equal(expectedMean,KFa.mean(),1e-7)); - EXPECT(assert_equal(expectedMean,KFb.mean(),1e-7)); - Matrix expected = 1e-6*Matrix_(9,9, + EXPECT(assert_equal(expectedMean, pa.mean(), 1e-7)); + EXPECT(assert_equal(expectedMean, pb.mean(), 1e-7)); + Matrix expected = 1e-6*Matrix_(9, 9, 48.8, -3.1, -0.0, -0.4, -0.4, 0.0, 0.0, 63.8, -0.6, -3.1, 148.4, -0.3, 0.5, 1.7, 0.2, -63.8, 0.0, -0.1, -0.0, -0.3, 188.0, -0.0, 0.2, 1.2, 0.0, 0.1, 0.0, @@ -212,26 +233,31 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, 0.0, 63.8, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 647.2, 0.0, -0.6, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 647.2); - EXPECT(assert_equal(expected,KFa.covariance(),1e-7)); - EXPECT(assert_equal(expected,KFb.covariance(),1e-7)); + EXPECT(assert_equal(expected, pa.covariance(), 1e-7)); + EXPECT(assert_equal(expected, pb.covariance(), 1e-7)); - Matrix H = 1e-3*Matrix_(3,9, + // prepare update + Matrix H = 1e-3*Matrix_(3, 9, 0.0, 9795.9, 83.6, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, -9795.9, 0.0, -5.2, 0.0, 0.0, 0.0, 0.0, 1000.0, 0.0, -83.6, 5.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.); - Vector z = Vector_(3,0.2599 , 1.3327 , 0.2007); - - Vector sigmas = Vector_(3, 0.3323 ,0.2470 ,0.1904); + Vector z = Vector_(3, 0.2599 , 1.3327 , 0.2007); + Vector sigmas = Vector_(3, 0.3323 , 0.2470 , 0.1904); SharedDiagonal modelR = noiseModel::Diagonal::Sigmas(sigmas); - KalmanFilter KFa2 = KFa.update(H, z, modelR); - KalmanFilter KFb2 = KFb.update(H, z, modelR); - EXPECT(assert_equal(KFa2.mean(),KFb2.mean())); - EXPECT(assert_equal(KFa2.information(),KFb2.information(),1e-7)); + // do update + KalmanFilter::State pa2 = kfa.update(pa, H, z, modelR); + KalmanFilter::State pb2 = kfb.update(pb, H, z, modelR); + + // Check that they yield the same mean and information matrix + EXPECT(assert_equal(pa2.mean(), pb2.mean())); + EXPECT(assert_equal(pa2.information(), pb2.information(), 1e-7)); + + // and in addition attain the correct mean and covariance Vector expectedMean2 = Vector_(9, 0.9207, 0.9030, 1.0178, 1.0002, 0.9992, 0.9998, 0.9981, 1.0035, 0.9882); - EXPECT(assert_equal(expectedMean2,KFa2.mean(),1e-4)); // not happy with tolerance here ! - EXPECT(assert_equal(expectedMean2,KFb2.mean(),1e-4)); // is something still amiss? - Matrix expected2 = 1e-6*Matrix_(9,9, + EXPECT(assert_equal(expectedMean2, pa2.mean(), 1e-4));// not happy with tolerance here ! + EXPECT(assert_equal(expectedMean2, pb2.mean(), 1e-4));// is something still amiss? + Matrix expected2 = 1e-6*Matrix_(9, 9, 46.1, -2.6, -0.0, -0.4, -0.4, 0.0, 0.0, 63.9, -0.5, -2.6, 132.8, -0.5, 0.4, 1.5, 0.2, -64.0, -0.0, -0.1, -0.0, -0.5, 188.0, -0.0, 0.2, 1.2, -0.0, 0.1, 0.0, @@ -241,8 +267,8 @@ TEST( KalmanFilter, QRvsCholesky ) { 0.0, -64.0, -0.0, -0.0, -0.0, -0.0, 647.2, -0.0, 0.0, 63.9, -0.0, 0.1, -0.0, -0.0, 0.0, -0.0, 647.2, 0.1, -0.5, -0.1, 0.0, -0.0, -0.0, 0.0, 0.0, 0.1, 635.8); - EXPECT(assert_equal(expected2,KFa2.covariance(),1e-7)); - EXPECT(assert_equal(expected2,KFb2.covariance(),1e-7)); + EXPECT(assert_equal(expected2, pa2.covariance(), 1e-7)); + EXPECT(assert_equal(expected2, pb2.covariance(), 1e-7)); } /* ************************************************************************* */