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

Kalman Filter now functional (i.e., not imperative)

release/4.3a0
Frank Dellaert 2012-01-20 05:07:06 +00:00
parent b60de0f03e
commit 4aa4b8b938
3 changed files with 50 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/linear/KalmanFilter.cpp
View File

@ -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));
}
/* ************************************************************************* */

17
gtsam/linear/KalmanFilter.h
View File

@ -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);
};

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

@ -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()));
}
/* ************************************************************************* */