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

Kalman filter now can work in QR or LDL mode

release/4.3a0
Frank Dellaert 2012-01-20 18:01:56 +00:00
parent 099f170bf2
commit 24292d6c13
3 changed files with 93 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/linear/KalmanFilter.cpp
View File

@ -29,10 +29,9 @@
namespace gtsam {
/// Auxiliary function to solve factor graph and return pointer to root conditional
GaussianConditional* solve(GaussianFactorGraph& factorGraph) {
GaussianConditional* solve(GaussianFactorGraph& factorGraph, bool useQR) {
// Solve the factor graph
const bool useQR = true; // make sure we use QR (numerically stable)
GaussianSequentialSolver solver(factorGraph, useQR);
GaussianBayesNet::shared_ptr bayesNet = solver.eliminate();
@ -44,8 +43,9 @@ namespace gtsam {
}
/* ************************************************************************* */
KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density) :
n_(n), I_(eye(n_, n_)), density_(density) {
KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density,
Factorization method) :
n_(n), I_(eye(n_, n_)), method_(method), density_(density) {
}
/* ************************************************************************* */
@ -59,29 +59,31 @@ namespace gtsam {
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));
return KalmanFilter(n_, solve(factorGraph, method_ == QR), method_);
}
/* ************************************************************************* */
KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0) :
n_(x0.size()), I_(eye(n_, n_)) {
KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0,
Factorization method) :
n_(x0.size()), I_(eye(n_, n_)), method_(method) {
// 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));
density_.reset(solve(factorGraph, method_ == QR));
}
/* ************************************************************************* */
KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0) :
n_(x.size()), I_(eye(n_, n_)) {
KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0,
Factorization method) :
n_(x.size()), I_(eye(n_, n_)), method_(method) {
// 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_.reset(solve(factorGraph));
density_.reset(solve(factorGraph, method_ == QR));
}
/* ************************************************************************* */

44
gtsam/linear/KalmanFilter.h
View File

@ -23,6 +23,10 @@
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianConditional.h>
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION QR
#endif
namespace gtsam {
class SharedDiagonal;
@ -32,16 +36,29 @@ namespace gtsam {
* Linear Kalman Filter
*/
class KalmanFilter {
public:
/**
* This Kalman filter is a Square-root Information filter
* The type below allows you to specify the factorization variant.
*/
enum Factorization {
QR, LDL
};
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, GaussianConditional* density);
KalmanFilter(size_t n, GaussianConditional* density, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION);
/// add a new factor and marginalize to new Kalman filter
KalmanFilter add(GaussianFactor* newFactor);
@ -54,7 +71,8 @@ namespace gtsam {
* @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);
KalmanFilter(const Vector& x0, const SharedDiagonal& P0,
Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION);
/**
* Constructor from prior density at time k=0
@ -62,16 +80,17 @@ namespace gtsam {
* @param x0 estimate at time 0
* @param P0 covariance at time 0, full Gaussian
*/
KalmanFilter(const Vector& x0, const Matrix& P0);
KalmanFilter(const Vector& x0, const Matrix& P0, Factorization method =
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 {
std::cout << s << "\n";
Vector m = mean();
Matrix P = covariance();
gtsam::print(m, "mean: ");
gtsam::print(P, "covariance: ");
}
/** Return mean of posterior P(x|Z) at given all measurements Z */
Vector mean() const;
@ -122,11 +141,12 @@ 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, const SharedDiagonal& model);
KalmanFilter update(const Matrix& H, const Vector& z,
const SharedDiagonal& model);
};
}// \namespace gtsam
} // \namespace gtsam
/* ************************************************************************* */

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

@ -149,6 +149,54 @@ TEST( KalmanFilter, predict ) {
EXPECT(assert_equal(KFa.covariance(),KFb.covariance()));
}
/* ************************************************************************* */
// Test both QR and LDL versions in case of a realistic (AHRS) dynamics update
TEST( KalmanFilter, QRvsCholesky ) {
Vector mean = zero(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, 10000000.0, 0.0, 0.0, 0.0, 0.0, 0.1, -0.0,
0.0, 0.0, 0.0, 23.4, 24.5, -0.6, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 24.5, 87.9, 10.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, -0.6, 10.1, 61.1, 0.0, 0.0, 0.0,
0.0, -63.8, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, 0.0,
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);
// Set up dynamics update
Matrix Psi_k = 1e-6*Matrix_(9,9,
1000000.0, 0.0, 0.0, -19189.0, 277.7, -13.0, 0.0, 0.0, 0.0,
0.0, 1000000.0, 0.0, 277.6, 19188.3, 164.1, 0.0, 0.0, 0.0,
0.0, 0.0, 1000000.0, -15.4, -163.9, 19190.3, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 999973.7, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 999973.7, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 999973.7, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999973.7, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999973.7, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999973.7);
Matrix B = zeros(9,1);
Vector u = zero(1);
Matrix dt_Q_k = 1e-6*Matrix_(9,9,
33.6, 1.3, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1.3, 126.5, -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,
0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 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, 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);
EXPECT(assert_equal(KFa.mean(),KFb.mean()));
// EXPECT(assert_equal(KFa.information(),KFb.information()));
// EXPECT(assert_equal(KFa.covariance(),KFb.covariance()));
}
/* ************************************************************************* */
int main() {
TestResult tr;