12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed bug in Kalman filter when using LDL.

release/4.3a0
Frank Dellaert 2012-01-20 22:28:53 +00:00
parent 1dc669d463
commit 852a1c0a0f
3 changed files with 46 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/linear/KalmanFilter.cpp
View File

@ -25,11 +25,15 @@
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/SharedGaussian.h> #include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h>
namespace gtsam { namespace gtsam {
using namespace std;
/// Auxiliary function to solve factor graph and return pointer to root conditional /// 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 // Solve the factor graph
GaussianSequentialSolver solver(factorGraph, useQR); GaussianSequentialSolver solver(factorGraph, useQR);
@ -37,14 +41,12 @@ namespace gtsam {
// As this is a filter, all we need is the posterior P(x_t), // As this is a filter, all we need is the posterior P(x_t),
// so we just keep the root of the Bayes net // 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 return bayesNet->back();
const GaussianConditional::shared_ptr& r = bayesNet->back();
return new GaussianConditional(0, r->get_d(), r->get_R(), r->get_sigmas());
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density, KalmanFilter::KalmanFilter(size_t n,
Factorization method) : const GaussianConditional::shared_ptr& density, Factorization method) :
n_(n), I_(eye(n_, n_)), method_(method), density_(density) { 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) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma 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) // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0));
factorGraph.push_back(factor); 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 { Vector KalmanFilter::mean() const {
// Solve for mean // Solve for mean
Index nVars = 1; VectorValues x;
VectorValues x(nVars, n_); Index k = step();
// a VectorValues that only has a value for k: cannot be printed
x.insert(k, Vector(n_));
density_->rhs(x); density_->rhs(x);
density_->solveInPlace(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 // 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 // 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; Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
Vector b = B * u, g2 = M * b, g1 = -Ft * g2; Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
double f = dot(b, 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) { const Vector& b, const SharedDiagonal& model) {
// Nhe factor related to the motion model is defined as // 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 // 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 // The factor related to the measurements would be defined as
// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // 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 // = (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));
} }
/* ************************************************************************* */ /* ************************************************************************* */

15
gtsam/linear/KalmanFilter.h
View File

@ -57,8 +57,8 @@ namespace gtsam {
GaussianConditional::shared_ptr density_; GaussianConditional::shared_ptr density_;
/// private constructor /// private constructor
KalmanFilter(size_t n, GaussianConditional* density, Factorization method = KalmanFilter(size_t n, const GaussianConditional::shared_ptr& density,
KALMANFILTER_DEFAULT_FACTORIZATION); Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION);
/// add a new factor and marginalize to new Kalman filter /// add a new factor and marginalize to new Kalman filter
KalmanFilter add(GaussianFactor* newFactor); KalmanFilter add(GaussianFactor* newFactor);
@ -84,13 +84,10 @@ namespace gtsam {
KALMANFILTER_DEFAULT_FACTORIZATION); KALMANFILTER_DEFAULT_FACTORIZATION);
/// print /// print
void print(const std::string& s = "") const { void print(const std::string& s = "") const;
std::cout << s << "\n";
Vector m = mean(); /** Return step index k, starts at 0, incremented at each predict. */
Matrix P = covariance(); Index step() const { return density_->firstFrontalKey();}
gtsam::print(m, "mean: ");
gtsam::print(P, "covariance: ");
}
/** Return mean of posterior P(x|Z) at given all measurements Z */ /** Return mean of posterior P(x|Z) at given all measurements Z */
Vector mean() const; Vector mean() const;

11
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -18,10 +18,9 @@
*/ */
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h> #include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/SharedGaussian.h> #include <gtsam/linear/SharedGaussian.h>
#include <gtsam/linear/SharedNoiseModel.h> #include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; using namespace std;
@ -115,9 +114,11 @@ TEST( KalmanFilter, linear1 ) {
// Run iteration 3 // Run iteration 3
KalmanFilter KF3p = KF2.predict(F, B, u, modelQ); KalmanFilter KF3p = KF2.predict(F, B, u, modelQ);
EXPECT(assert_equal(expected3,KF3p.mean())); EXPECT(assert_equal(expected3,KF3p.mean()));
LONGS_EQUAL(3,KF3p.step());
KalmanFilter KF3 = KF3p.update(H,z3,modelR); KalmanFilter KF3 = KF3p.update(H,z3,modelR);
EXPECT(assert_equal(expected3,KF3.mean())); 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 // 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 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); 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.mean(),KFb.mean()));
// EXPECT(assert_equal(KFa.information(),KFb.information())); EXPECT(assert_equal(KFa.information(),KFb.information(),1e-7));
// EXPECT(assert_equal(KFa.covariance(),KFb.covariance())); EXPECT(assert_equal(KFa.covariance(),KFb.covariance(),1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */