diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 6291c90b6b..ebc4d583eb 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,29 +20,44 @@ * @author Frank Dellaert */ + #include + +#include #include #include -#include -#include #ifndef NDEBUG #include #endif -using namespace std; +// In the code below we often get a covariance matrix Q, and we need to create a +// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as +// Q = L L^T +// and hence +// Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1} +// We then have +// 0.5 * |Ax - b|^T Q^{-1} |Ax - b| +// = 0.5 * |Ax - b|^T M^T M |Ax - b| +// = 0.5 * |MAx - Mb|^T |MAx - Mb| +// The functor below efficiently multiplies with M by calling L.solve(). +namespace { +using Matrix = gtsam::Matrix; +struct InverseL : public Eigen::LLT { + InverseL(const Matrix& Q) : Eigen::LLT(Q) {} + Matrix operator*(const Matrix& A) const { return matrixL().solve(A); } +}; +} // namespace namespace gtsam { - /* ************************************************************************* */ -// Auxiliary function to solve factor graph and return pointer to root conditional -KalmanFilter::State // -KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { - +// Auxiliary function to solve factor graph and return pointer to root +// conditional +KalmanFilter::State KalmanFilter::solve( + const GaussianFactorGraph& factorGraph) const { // Eliminate the graph using the provided Eliminate function Ordering ordering(factorGraph.keys()); - const auto bayesNet = // - factorGraph.eliminateSequential(ordering, function_); + const auto bayesNet = 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 @@ -52,9 +67,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { /* ************************************************************************* */ // Auxiliary function to create a small graph for predict or update and solve -KalmanFilter::State // -KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { - +KalmanFilter::State KalmanFilter::fuse( + const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; factorGraph.push_back(p); @@ -66,45 +80,54 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { /* ************************************************************************* */ KalmanFilter::State KalmanFilter::init(const Vector& x0, - const SharedDiagonal& P0) const { - + const SharedDiagonal& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, + P0); // |x-x0|^2_P0 return solve(factorGraph); } /* ************************************************************************* */ -KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const { - +KalmanFilter::State KalmanFilter::init(const Vector& x0, + const Matrix& P0) const { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + + // Perform Cholesky decomposition of P0 = LL^T + InverseL M(P0); + + // Premultiply I and x0 with M=L^{-1} + const Matrix A = M * I_; // = M + const Vector b = M * x0; // = M*x0 + factorGraph.emplace_shared(0, A, b); return solve(factorGraph); } /* ************************************************************************* */ -void KalmanFilter::print(const string& s) const { - cout << "KalmanFilter " << s << ", dim = " << n_ << endl; +void KalmanFilter::print(const std::string& s) const { + std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl; } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const SharedDiagonal& model) const { - + const Matrix& B, const Vector& u, + const SharedDiagonal& model) const { // 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_{k},x_{k+1}) = + // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T Key k = step(p); return fuse(p, - std::make_shared(k, -F, k + 1, I_, B * u, model)); + std::make_shared(k, -F, k + 1, I_, B * u, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, - const Matrix& B, const Vector& u, const Matrix& Q) const { - + const Matrix& B, const Vector& u, + const Matrix& Q) const { #ifndef NDEBUG - DenseIndex n = F.cols(); + const DenseIndex n = dim(); + assert(F.cols() == n); assert(F.rows() == n); assert(B.rows() == n); assert(B.cols() == u.size()); @@ -112,50 +135,52 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // 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 - // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: - // TODO: starts to seem more elaborate than straight-up KF equations? - Matrix M = Q.inverse(), Ft = trans(F); - Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; - Vector b = B * u, g2 = M * b, g1 = -Ft * g2; - double f = dot(b, g2); - Key k = step(p); - return fuse(p, - std::make_shared(k, k + 1, G11, G12, g1, G22, g2, f)); + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); + + // Premultiply -F, I, and B * u with M=L^{-1} + const Matrix A1 = -(M * F); + const Matrix A2 = M * I_; + const Vector b = M * (B * u); + return predict2(p, A1, A2, b); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, - const Matrix& A1, const Vector& b, const SharedDiagonal& model) const { + const Matrix& A1, const Vector& b, + const SharedDiagonal& model) const { // 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_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2 Key k = step(p); return fuse(p, std::make_shared(k, A0, k + 1, A1, b, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, - const Vector& z, const SharedDiagonal& model) const { + const Vector& z, + const SharedDiagonal& model) const { // 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 + // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T + // = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T Key k = step(p); return fuse(p, std::make_shared(k, H, z, model)); } /* ************************************************************************* */ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, - const Vector& z, const Matrix& Q) const { + const Vector& z, + const Matrix& Q) const { Key k = step(p); - Matrix M = Q.inverse(), Ht = trans(H); - Matrix G = Ht * M * H; - Vector g = Ht * M * z; - double f = dot(z, M * z); - return fuse(p, std::make_shared(k, G, g, f)); + + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); + + // Pre-multiply H and z with M=L^{-1}, respectively + const Matrix A = M * H; + const Vector b = M * z; + return fuse(p, std::make_shared(k, A, b)); } /* ************************************************************************* */ -} // \namespace gtsam - +} // namespace gtsam diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 65fac877ab..2bf0549fae 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -11,10 +11,10 @@ /** * @file KalmanFilter.h - * @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. + * @brief Simple linear Kalman filter implemented using factor graphs, i.e., + * performs Cholesky or QR-based SRIF (Square-Root Information Filter). * @date Sep 3, 2011 - * @author Stephen Williams - * @author Frank Dellaert + * @authors Stephen Williams, Frank Dellaert */ #pragma once @@ -32,120 +32,186 @@ namespace gtsam { /** * Kalman Filter class * - * 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. + * Maintains a Gaussian density under linear-Gaussian motion and + * measurement models using the square-root information form. * - * 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 an old state. + * The filter is functional; it does not maintain internal state. Instead: + * - Use `init()` to create an initial filter state, + * - Call `predict()` and `update()` to create new states. */ class GTSAM_EXPORT KalmanFilter { - -public: - + public: /** - * This Kalman filter is a Square-root Information filter - * The type below allows you to specify the factorization variant. + * @enum Factorization + * @brief Specifies the factorization variant to use. */ - enum Factorization { - QR, CHOLESKY - }; + enum Factorization { QR, CHOLESKY }; /** - * The Kalman filter state is simply a GaussianDensity + * @typedef State + * @brief The Kalman filter state, represented as a shared pointer to a + * GaussianDensity. */ typedef GaussianDensity::shared_ptr State; -private: - - const size_t n_; /** dimensionality of state */ - const Matrix I_; /** identity matrix of size n*n */ - const GaussianFactorGraph::Eliminate function_; /** algorithm */ + private: + const size_t n_; ///< Dimensionality of the state. + const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$. + const GaussianFactorGraph::Eliminate + function_; ///< Elimination algorithm used. + /** + * Solve the factor graph. + * @param factorGraph The Gaussian factor graph to solve. + * @return The resulting Kalman filter state. + */ State solve(const GaussianFactorGraph& factorGraph) const; - State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; -public: + /** + * Fuse two states. + * @param p The prior state. + * @param newFactor The new factor to incorporate. + * @return The resulting fused state. + */ + State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; - // Constructor - KalmanFilter(size_t n, Factorization method = - KALMANFILTER_DEFAULT_FACTORIZATION) : - n_(n), I_(Matrix::Identity(n_, n_)), function_( - method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) : - GaussianFactorGraph::Eliminate(EliminateCholesky)) { - } + public: + /** + * Constructor. + * @param n Dimensionality of the state. + * @param method Factorization method (default: QR unless compile-flag set). + */ + KalmanFilter(size_t n, + Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION) + : n_(n), + I_(Matrix::Identity(n_, n_)), + function_(method == QR + ? GaussianFactorGraph::Eliminate(EliminateQR) + : GaussianFactorGraph::Eliminate(EliminateCholesky)) {} /** - * Create initial state, i.e., prior density at time k=0 - * In Kalman Filter notation, these are x_{0|0} and P_{0|0} - * @param x0 estimate at time 0 - * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' + * Create the initial state (prior density at time \f$ k=0 \f$). + * + * In Kalman Filter notation: + * - \f$ x_{0|0} \f$: Initial state estimate. + * - \f$ P_{0|0} \f$: Initial covariance matrix. + * + * @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$). + * @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian + * model. + * @return Initial Kalman filter state. */ State init(const Vector& x0, const SharedDiagonal& P0) const; - /// version of init with a full covariance matrix + /** + * Create the initial state with a full covariance matrix. + * @param x0 Initial state estimate. + * @param P0 Full covariance matrix. + * @return Initial Kalman filter state. + */ State init(const Vector& x0, const Matrix& P0) const; - /// print + /** + * Print the Kalman filter details. + * @param s Optional string prefix. + */ void print(const std::string& s = "") const; - /** Return step index k, starts at 0, incremented at each predict. */ - static Key step(const State& p) { - return p->firstFrontalKey(); - } + /** + * Return the step index \f$ k \f$ (starts at 0, incremented at each predict + * step). + * @param p The current state. + * @return Step index. + */ + static Key step(const State& p) { return p->firstFrontalKey(); } /** - * Predict the state P(x_{t+1}|Z^t) - * In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t} - * 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. + * Predict the next state \f$ P(x_{k+1}|Z^k) \f$. + * + * In Kalman Filter notation: + * - \f$ x_{k+1|k} \f$: Predicted state. + * - \f$ P_{k+1|k} \f$: Predicted covariance. + * + * Motion model: + * \f[ + * x_{k+1} = F \cdot x_k + B \cdot u_k + w + * \f] + * where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$. + * + * @param p Previous state (\f$ x_k \f$). + * @param F State transition matrix (\f$ F \f$). + * @param B Control input matrix (\f$ B \f$). + * @param u Control vector (\f$ u_k \f$). + * @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian). + * @return Predicted state (\f$ x_{k+1|k} \f$). */ State predict(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const SharedDiagonal& modelQ) const; + const Vector& u, const SharedDiagonal& modelQ) const; - /* - * Version of predict with full covariance - * 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. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Predict the next state with a full covariance matrix. + * + *@note 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. This version allows more realistic models than a diagonal matrix. + * + * @param p Previous state. + * @param F State transition matrix. + * @param B Control input matrix. + * @param u Control vector. + * @param Q Full covariance matrix (\f$ Q \f$). + * @return Predicted state. */ State predictQ(const State& p, const Matrix& F, const Matrix& B, - const Vector& u, const Matrix& Q) const; + const Vector& u, const Matrix& Q) 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: - * This version of predict takes GaussianFactor motion model [A0 A1 b] - * with an optional noise model. + * Predict the next state using a GaussianFactor motion model. + * @param p Previous state. + * @param A0 Factor matrix. + * @param A1 Factor matrix. + * @param b Constant term vector. + * @param model Noise model (optional). + * @return Predicted state. */ State predict2(const State& p, const Matrix& A0, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) const; + const Vector& b, const SharedDiagonal& model = nullptr) const; /** - * 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. + * Update the Kalman filter with a measurement. + * + * Observation model: + * \f[ + * z_k = H \cdot x_k + v + * \f] + * where \f$ v \f$ is zero-mean Gaussian noise with covariance R. * In this version, R is restricted to diagonal Gaussians (model parameter) + * + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param model Noise model (diagonal Gaussian). + * @return Updated state. */ State update(const State& p, const Matrix& H, const Vector& z, - const SharedDiagonal& model) const; + const SharedDiagonal& model) const; - /* - * Version of update with full covariance - * 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. - * This version allows more realistic models than a diagonal covariance matrix. + /** + * Update the Kalman filter with a measurement using a full covariance matrix. + * @param p Previous state. + * @param H Observation matrix. + * @param z Measurement vector. + * @param R Full covariance matrix. + * @return Updated state. */ State updateQ(const State& p, const Matrix& H, const Vector& z, - const Matrix& Q) const; -}; - -} // \namespace gtsam + const Matrix& R) const; -/* ************************************************************************* */ + /** + * Return the dimensionality of the state. + * @return Dimensionality of the state. + */ + size_t dim() const { return n_; } +}; +} // namespace gtsam