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

removed checking const variable each time and created appropriate Eliminate function in constructor instead.

release/4.3a0
Frank Dellaert 2013-12-10 05:52:38 +00:00
parent 4815f9cf5b
commit 597d617808
2 changed files with 22 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam/linear/KalmanFilter.cpp
View File

@ -34,14 +34,15 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Auxiliary function to solve factor graph and return pointer to root conditional
static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
const GaussianFactorGraph::Eliminate& function) {
KalmanFilter::State //
KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
// Eliminate the graph using the provided Eliminate function
Ordering ordering(factorGraph.keys());
GaussianBayesNet::shared_ptr bayesNet = //
factorGraph.eliminateSequential(ordering, function);
factorGraph.eliminateSequential(ordering, function_);
// As this is a filter, all we need is the posterior P(x_t).
// This is the last GaussianConditional in the resulting BayesNet
@ -50,22 +51,16 @@ static KalmanFilter::State solve(const GaussianFactorGraph& factorGraph,
}
/* ************************************************************************* */
static KalmanFilter::State fuse(const KalmanFilter::State& p,
GaussianFactor::shared_ptr newFactor,
const GaussianFactorGraph::Eliminate& function) {
// Auxiliary function to create a small graph for predict or update and solve
KalmanFilter::State //
KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
// Create a factor graph
GaussianFactorGraph factorGraph;
factorGraph += p, newFactor;
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph, function);
}
/* ************************************************************************* */
GaussianFactorGraph::Eliminate KalmanFilter::factorization() const {
return
method_ == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky);
return solve(factorGraph);
}
/* ************************************************************************* */
@ -75,7 +70,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0,
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph, factorization());
return solve(factorGraph);
}
/* ************************************************************************* */
@ -84,7 +79,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
return solve(factorGraph, factorization());
return solve(factorGraph);
}
/* ************************************************************************* */
@ -100,8 +95,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
// 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);
return fuse(p,
boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model),
factorization());
boost::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
}
/* ************************************************************************* */
@ -127,8 +121,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
double f = dot(b, g2);
Key k = step(p);
return fuse(p,
boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f),
factorization());
boost::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
}
/* ************************************************************************* */
@ -137,8 +130,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
// 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
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));
}
/* ************************************************************************* */
@ -148,8 +140,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
// 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
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));
}
/* ************************************************************************* */
@ -160,7 +151,7 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
Matrix G = Ht * M * H;
Vector g = Ht * 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));
}
/* ************************************************************************* */

9
gtsam/linear/KalmanFilter.h
View File

@ -59,16 +59,19 @@ private:
const size_t n_; /** dimensionality of state */
const Matrix I_; /** identity matrix of size n*n */
const Factorization method_; /** algorithm */
const GaussianFactorGraph::Eliminate function_; /** algorithm */
GaussianFactorGraph::Eliminate factorization() const;
State solve(const GaussianFactorGraph& factorGraph) const;
State fuse(const State& p, GaussianFactor::shared_ptr newFactor);
public:
// Constructor
KalmanFilter(size_t n, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION) :
n_(n), I_(eye(n_, n_)), method_(method) {
n_(n), I_(eye(n_, n_)), function_(
method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky)) {
}
/**