diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp new file mode 100644 index 000000000..5e84961af --- /dev/null +++ b/gtsam/linear/KalmanFilter.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * testKalmanFilter.cpp + * + * Simple linear Kalman filter. + * Implemented using factor graphs, i.e., does LDL-based SRIF, really. + * + * Created on: Sep 3, 2011 + * @Author: Stephen Williams + * @Author: Frank Dellaert + */ + +#include +#include +#include + +namespace gtsam { + + // Auxiliary function to solve factor graph and return pointer to root conditional + GaussianConditional* solve(GaussianFactorGraph& factorGraph) { + + // Solve the factor graph + GaussianSequentialSolver solver(factorGraph); + GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); + + // As this is a filter, all we need is the posterior P(x_t), + // 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 + const GaussianConditional::shared_ptr& r = bayesNet->back(); + return new GaussianConditional(0, r->get_d(), r->get_R(), r->get_sigmas()); + } + + /* ************************************************************************* */ + KalmanFilter::KalmanFilter(const Vector& x, const SharedDiagonal& model) : + n_(x.size()), I_(eye(n_, n_)) { + + // Create a factor graph f(x0), eliminate it into P(x0) + GaussianFactorGraph factorGraph; + factorGraph.add(0, I_, x, model); + density_.reset(solve(factorGraph)); + } + + /* ************************************************************************* */ + Vector KalmanFilter::mean() const { + // Solve for mean + Index nVars = 1; + VectorValues x(nVars, n_); + density_->rhs(x); + density_->solveInPlace(x); + return x[0]; + } + + /* ************************************************************************* */ + Matrix KalmanFilter::information() const { + return density_->computeInformation(); + } + + /* ************************************************************************* */ + Matrix KalmanFilter::covariance() const { + return inverse(information()); + } + + /* ************************************************************************* */ + void 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) + // and factor f2 is from the motion model + GaussianFactorGraph factorGraph; + + // push back f1 + factorGraph.push_back(density_->toFactor()); + + // 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 + 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)); + } + + /* ************************************************************************* */ + void 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 + // and factor f2 is from the measurement model + GaussianFactorGraph factorGraph; + + // push back f1 + factorGraph.push_back(density_->toFactor()); + + // The factor related to the measurements would be defined as + // 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 + 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)); + } + +/* ************************************************************************* */ + +} // \namespace gtsam + diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h new file mode 100644 index 000000000..3d803fea7 --- /dev/null +++ b/gtsam/linear/KalmanFilter.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * testKalmanFilter.cpp + * + * Simple linear Kalman filter. + * Implemented using factor graphs, i.e., does LDL-based SRIF, really. + * + * Created on: Sep 3, 2011 + * @Author: Stephen Williams + * @Author: Frank Dellaert + */ + +#include + +namespace gtsam { + + class KalmanFilter { + private: + + size_t n_; /** dimensionality of state */ + Matrix I_; /** identity matrix of size n*n */ + + /** The Kalman filter posterior density is a Gaussian Conditional with no parents */ + GaussianConditional::shared_ptr density_; + + public: + + /** + * Constructor from prior density at time k=0 + * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} + * @param x estimate at time 0 + * @param P covariance at time 0, restricted to diagonal Gaussian 'model' for now + */ + KalmanFilter(const Vector& x, const SharedDiagonal& model); + + /** Return mean of posterior P(x|Z) at given all measurements Z */ + Vector mean() const; + + /** Return information matrix of posterior P(x|Z) at given all measurements Z */ + Matrix information() const; + + /** Return covariance of posterior P(x|Z) at given all measurements Z */ + Matrix covariance() const; + + /** + * Predict the state P(x_{t+1}|Z^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: + * 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, + * and w is zero-mean, Gaussian white noise with covariance Q. + * Q is normally derived as G*w*G^T where w models uncertainty of some physical property, + * such as velocity or acceleration, and G is derived from physics. + * In the current implementation, the noise model for w is restricted to a diagonal. + * TODO: allow for a G + */ + void predict(const Matrix& F, const Matrix& B, const Vector& u, + const SharedDiagonal& model); + + /** + * Update Kalman filter with a measurement + * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} + * will be of the form h(x_{t}) = H*x_{t} + v + * where H is the observation model/matrix, and v is zero-mean, + * 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); + + }; + +}// \namespace gtsam + +/* ************************************************************************* */ + diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 5de9382b4..37d38c6c3 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -32,6 +32,7 @@ check_PROGRAMS += tests/testHessianFactor tests/testGaussianFactor tests/testGau check_PROGRAMS += tests/testGaussianFactorGraph tests/testGaussianJunctionTree # Kalman Filter +sources += KalmanFilter.cpp check_PROGRAMS += tests/testKalmanFilter # Iterative Methods diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 1af6df5e1..bf771fae1 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -14,149 +14,17 @@ * * Test simple linear Kalman filter on a moving 2D point * - * Created on: Aug 19, 2011 + * Created on: Sep 3, 2011 * @Author: Stephen Williams * @Author: Frank Dellaert */ -#include -#include +#include #include using namespace std; using namespace gtsam; -class KalmanFilter { -private: - - size_t n_; /** dimensionality of state */ - Matrix I_; /** identity matrix of size n*n */ - - /** The Kalman filter posterior density is a Gaussian Conditional with no parents */ - GaussianConditional::shared_ptr density_; - - /** - * solve a factor graph fragment and store result as density_ - */ - void solve(GaussianFactorGraph& factorGraph) { - - // Solve the factor graph - GaussianSequentialSolver solver(factorGraph); - GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); - - // As this is a filter, all we need is the posterior P(x_t), - // 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 - const GaussianConditional::shared_ptr& root = bayesNet->back(); - density_.reset( - new GaussianConditional(0, root->get_d(), root->get_R(), - root->get_sigmas())); - } - -public: - - /** - * Constructor from prior density at time k=0 - * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} - * @param x estimate at time 0 - * @param P covariance at time 0, restricted to diagonal Gaussian 'model' for now - * - */ - KalmanFilter(const Vector& x, const SharedDiagonal& model) : - n_(x.size()), I_(eye(n_, n_)) { - - // Create a factor graph f(x0), eliminate it into P(x0) - GaussianFactorGraph factorGraph; - factorGraph.add(0, I_, x, model); - solve(factorGraph); - } - - /** - * Return mean of posterior P(x|Z) at given all measurements Z - */ - Vector mean() const { - // Solve for mean - Index nVars = 1; - VectorValues x(nVars, n_); - density_->rhs(x); - density_->solveInPlace(x); - return x[0]; - } - - /** - * Return information matrix of posterior P(x|Z) at given all measurements Z - */ - Matrix information() const { - return density_->computeInformation(); - } - - /** - * Return covariance of posterior P(x|Z) at given all measurements Z - */ - Matrix covariance() const { - return inverse(information()); - } - - /** - * Predict the state P(x_{t+1}|Z^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: - * 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, - * and w is zero-mean, Gaussian white noise with covariance Q. - * Q is normally derived as G*w*G^T where w models uncertainty of some physical property, - * such as velocity or acceleration, and G is derived from physics. - * In the current implementation, the noise model for w is restricted to a diagonal. - * TODO: allow for a G - */ - void 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) - // and factor f2 is from the motion model - GaussianFactorGraph factorGraph; - - // push back f1 - factorGraph.push_back(density_->toFactor()); - - // 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 - factorGraph.add(0, -F, 1, I_, B*u, model); - - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - solve(factorGraph); - } - - /** - * Update Kalman filter with a measurement - * For the Kalman Filter, the measurement function, h(x_{t}) = z_{t} - * will be of the form h(x_{t}) = H*x_{t} + v - * where H is the observation model/matrix, and v is zero-mean, - * 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) { - // We will create a small factor graph f1-(x0)-f2 - // where factor f1 is the predictive density - // and factor f2 is from the measurement model - GaussianFactorGraph factorGraph; - - // push back f1 - factorGraph.push_back(density_->toFactor()); - - // The factor related to the measurements would be defined as - // 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 - factorGraph.add(0, H, z, model); - - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - solve(factorGraph); - } - -}; -// KalmanFilter - /* ************************************************************************* */ /** Small 2D point class implemented as a Vector */