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

Comments and formatting, and much cleaner solve

release/4.3a0
Frank Dellaert 2013-12-10 05:23:28 +00:00
parent abbbd02979
commit f2da19e4de
2 changed files with 229 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
gtsam/linear/KalmanFilter.cpp
View File

@ -21,7 +21,7 @@
*/ */
#include <gtsam/linear/KalmanFilter.h> #include <gtsam/linear/KalmanFilter.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
@ -34,82 +34,89 @@ using namespace std;
namespace gtsam { namespace gtsam {
/// Auxiliary function to solve factor graph and return pointer to root conditional // Auxiliary function to solve factor graph and return pointer to root conditional
KalmanFilter::State solve(const GaussianFactorGraph& factorGraph, const GaussianFactorGraph::Eliminate& function) static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
{ const GaussianFactorGraph::Eliminate& function) {
// Do a dense solve, e.g. don't use the multifrontal framework
// Eliminate the keys in increasing order so that the last key is the one we want to keep.
FastSet<Key> keysToEliminate = factorGraph.keys();
FastSet<Key>::const_iterator lastKeyPos = keysToEliminate.end();
-- lastKeyPos;
Key remainingKey = *lastKeyPos;
keysToEliminate.erase(lastKeyPos);
GaussianFactorGraph::EliminationResult result = function(factorGraph, Ordering(keysToEliminate));
// As this is a filter, all we need is the posterior P(x_t). Eliminate it to be // We will eliminate the keys in increasing order, except the last key
// upper-triangular. Ordering ordering1(factorGraph.keys());
Key lastKey = ordering1.back();
ordering1.pop_back();
// Eliminate the graph using the provided Eliminate function
GaussianConditional::shared_ptr conditional;
GaussianFactor::shared_ptr factor;
boost::tie(conditional, factor) = function(factorGraph, ordering1);
// As this is a filter, all we need is the posterior P(x_t).
GaussianFactorGraph graphOfRemainingFactor; GaussianFactorGraph graphOfRemainingFactor;
graphOfRemainingFactor += result.second; graphOfRemainingFactor += factor;
GaussianDensity::shared_ptr state = boost::make_shared<GaussianDensity>(
*function(graphOfRemainingFactor, Ordering(cref_list_of<1>(remainingKey))).first);
return state; // Eliminate it to be upper-triangular.
} Ordering ordering2;
ordering2 += lastKey;
boost::tie(conditional, factor) = function(graphOfRemainingFactor, ordering2);
/* ************************************************************************* */ return boost::make_shared<GaussianDensity>(*conditional);
KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor, }
const GaussianFactorGraph::Eliminate& function)
{ /* ************************************************************************* */
static KalmanFilter::State fuse(const KalmanFilter::State& p,
GaussianFactor::shared_ptr newFactor,
const GaussianFactorGraph::Eliminate& function) {
// Create a factor graph // Create a factor graph
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
factorGraph += p, newFactor; factorGraph += p, newFactor;
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph, function); return solve(factorGraph, function);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::Eliminate KalmanFilter::factorization() const { GaussianFactorGraph::Eliminate KalmanFilter::factorization() const {
return method_ == QR ? return
GaussianFactorGraph::Eliminate(EliminateQR) : method_ == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky); GaussianFactorGraph::Eliminate(EliminateCholesky);
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0, const SharedDiagonal& P0) { KalmanFilter::State KalmanFilter::init(const Vector& x0,
const SharedDiagonal& P0) {
// 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 += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph, factorization()); return solve(factorGraph, factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
// 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 += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
return solve(factorGraph, factorization()); return solve(factorGraph, factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */
void KalmanFilter::print(const string& s) const { void KalmanFilter::print(const string& s) const {
cout << "KalmanFilter " << s << ", dim = " << n_ << endl; cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const SharedDiagonal& model) { const Matrix& B, const Vector& u, const SharedDiagonal& model) {
// 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
Key k = step(p); Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model), factorization()); return fuse(p,
} boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model),
factorization());
}
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const Matrix& Q) { const Matrix& B, const Vector& u, const Matrix& Q) {
#ifndef NDEBUG #ifndef NDEBUG
@ -130,38 +137,42 @@ namespace gtsam {
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);
Key k = step(p); Key k = step(p);
return fuse(p, boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f), factorization()); return fuse(p,
} boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f),
factorization());
}
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
const Matrix& A1, const Vector& b, const SharedDiagonal& model) { const Matrix& A1, 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
Key k = step(p); Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model), factorization()); return fuse(p, boost::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model),
} factorization());
}
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
const Vector& z, const SharedDiagonal& model) { const Vector& z, const SharedDiagonal& model) {
// 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
Key k = step(p); Key k = step(p);
return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model), factorization()); return fuse(p, boost::make_shared<JacobianFactor>(k, H, z, model),
} factorization());
}
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
const Matrix& Q) { const Vector& z, const Matrix& Q) {
Key k = step(p); Key k = step(p);
Matrix M = inverse(Q), Ht = trans(H); Matrix M = inverse(Q), Ht = trans(H);
Matrix G = Ht * M * H; Matrix G = Ht * M * H;
Vector g = Ht * M * z; Vector g = Ht * M * z;
double f = dot(z, M * z); double f = dot(z, M * z);
return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f), factorization()); return fuse(p, boost::make_shared<HessianFactor>(k, G, g, f), factorization());
} }
/* ************************************************************************* */ /* ************************************************************************* */

21
gtsam/linear/KalmanFilter.h
View File

@ -29,18 +29,18 @@
namespace gtsam { namespace gtsam {
/** /**
* Kalman Filter class * Kalman Filter class
* *
* Knows how to maintain a Gaussian density under linear-Gaussian motion and * 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. * 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 * 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. * an initial state, then predict() and update() that create new states out of an old state.
*/ */
class GTSAM_EXPORT KalmanFilter { class GTSAM_EXPORT KalmanFilter {
public: public:
/** /**
* This Kalman filter is a Square-root Information filter * This Kalman filter is a Square-root Information filter
@ -55,7 +55,7 @@ namespace gtsam {
*/ */
typedef GaussianDensity::shared_ptr State; typedef GaussianDensity::shared_ptr State;
private: private:
const size_t n_; /** dimensionality of state */ const size_t n_; /** dimensionality of state */
const Matrix I_; /** identity matrix of size n*n */ const Matrix I_; /** identity matrix of size n*n */
@ -63,9 +63,9 @@ namespace gtsam {
GaussianFactorGraph::Eliminate factorization() const; GaussianFactorGraph::Eliminate factorization() const;
public: public:
// private constructor // Constructor
KalmanFilter(size_t n, Factorization method = KalmanFilter(size_t n, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION) : KALMANFILTER_DEFAULT_FACTORIZATION) :
n_(n), I_(eye(n_, n_)), method_(method) { n_(n), I_(eye(n_, n_)), method_(method) {
@ -73,7 +73,7 @@ namespace gtsam {
/** /**
* Create initial state, i.e., prior density at time k=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} * In Kalman Filter notation, these are x_{0|0} and P_{0|0}
* @param x0 estimate at time 0 * @param x0 estimate at time 0
* @param P0 covariance at time 0, given as a diagonal Gaussian 'model' * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
*/ */
@ -93,7 +93,6 @@ namespace gtsam {
/** /**
* Predict the state P(x_{t+1}|Z^t) * Predict the state P(x_{t+1}|Z^t)
* In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
* After the call, that is the density that can be queried.
* Details and parameters: * Details and parameters:
* In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w * In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
* where F is the state transition model/matrix, B is the control input model, * where F is the state transition model/matrix, B is the control input model,
@ -128,7 +127,7 @@ namespace gtsam {
* will be of the form h(x_{t}) = H*x_{t} + v * will be of the form h(x_{t}) = H*x_{t} + v
* where H is the observation model/matrix, and v is zero-mean, * where H is the observation model/matrix, and v is zero-mean,
* Gaussian white noise with covariance R. * Gaussian white noise with covariance R.
* Currently, R is restricted to diagonal Gaussians (model parameter) * In this version, R is restricted to diagonal Gaussians (model parameter)
*/ */
State update(const State& p, const Matrix& H, const Vector& z, State update(const State& p, const Matrix& H, const Vector& z,
const SharedDiagonal& model); const SharedDiagonal& model);
@ -141,7 +140,7 @@ namespace gtsam {
*/ */
State updateQ(const State& p, const Matrix& H, const Vector& z, State updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q); const Matrix& Q);
}; };
} // \namespace gtsam } // \namespace gtsam