From a425e6e3d4c80f5553d045e54d4e7d7f60a7d258 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:36:43 -0500 Subject: [PATCH 1/3] better docs --- gtsam/linear/KalmanFilter.h | 226 +++++++++++++++++++++++------------- 1 file changed, 146 insertions(+), 80 deletions(-) diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 65fac877a..7fde9975d 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 */ - - State solve(const GaussianFactorGraph& factorGraph) const; - State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const; - -public: - - // 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)) { - } + 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. /** - * 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' + * Solve the factor graph. + * @param factorGraph The Gaussian factor graph to solve. + * @return The resulting Kalman filter state. + */ + State solve(const GaussianFactorGraph& factorGraph) const; + + /** + * 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; + + 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 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; + const Matrix& R) const; + + /** + * Return the dimensionality of the state. + * @return Dimensionality of the state. + */ + size_t dim() const { return n_; } }; -} // \namespace gtsam - -/* ************************************************************************* */ - +} // namespace gtsam \ No newline at end of file From 1ae3fc2ad80ff27e499ea49ac942eff6ffef0910 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 9 Dec 2024 16:38:15 -0500 Subject: [PATCH 2/3] Use LLT rather than inverse --- gtsam/linear/KalmanFilter.cpp | 120 ++++++++++++++++++++-------------- 1 file changed, 70 insertions(+), 50 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..12a43093c 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,26 +20,35 @@ * @author Frank Dellaert */ -#include +#include #include #include -#include -#include - +#include using namespace std; namespace gtsam { -/* ************************************************************************* */ -// Auxiliary function to solve factor graph and return pointer to root conditional -KalmanFilter::State // -KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const { +// In the code below we often need to multiply matrices with R, the Cholesky +// factor of the information matrix Q^{-1}, when given a covariance matrix Q. +// We can do this efficiently using Eigen, as we have: +// Q = L L^T +// => Q^{-1} = L^{-T}L^{-1} = R^T R +// => L^{-1} = R +// Rather than explicitly calculate R and do R*A, we should use L.solve(A) +// The following macro makes L available in the scope where it is used: +#define FACTORIZE_Q_INTO_L(Q) \ + Eigen::LLT llt(Q); \ + auto L = llt.matrixL(); +/* ************************************************************************* */ +// 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 @@ -49,9 +58,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); @@ -63,20 +71,27 @@ 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 + FACTORIZE_Q_INTO_L(P0) + + // Premultiply I and x0 with R + const Matrix R = L.solve(I_); // = R + const Vector b = L.solve(x0); // = R*x0 + factorGraph.emplace_shared(0, R, b); return solve(factorGraph); } @@ -87,21 +102,23 @@ void KalmanFilter::print(const string& s) const { /* ************************************************************************* */ 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()); @@ -109,50 +126,53 @@ 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 + FACTORIZE_Q_INTO_L(Q) + + // Premultiply A1 = -F and A2 = I, b = B * u with R + const Matrix A1 = -L.solve(F); // = -R*F + const Matrix A2 = L.solve(I_); // = R + const Vector b = L.solve(B * u); // = R * (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 + FACTORIZE_Q_INTO_L(Q) + + // Pre-multiply H and z with R, respectively + const Matrix RtH = L.solve(H); // = R * H + const Vector b = L.solve(z); // = R * z + + return fuse(p, std::make_shared(k, RtH, b)); } /* ************************************************************************* */ -} // \namespace gtsam - +} // namespace gtsam From af9db89ea9ce9e18dbcbfbb3791501ddaf8e6724 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 11 Dec 2024 13:10:31 -0500 Subject: [PATCH 3/3] Fixed math, use functor not macro --- gtsam/linear/KalmanFilter.cpp | 74 ++++++++++++++++++----------------- gtsam/linear/KalmanFilter.h | 2 +- 2 files changed, 40 insertions(+), 36 deletions(-) diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 12a43093c..7c6e78527 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -25,22 +25,27 @@ #include #include -using namespace std; +#include "gtsam/base/Matrix.h" + +// 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 { - -// In the code below we often need to multiply matrices with R, the Cholesky -// factor of the information matrix Q^{-1}, when given a covariance matrix Q. -// We can do this efficiently using Eigen, as we have: -// Q = L L^T -// => Q^{-1} = L^{-T}L^{-1} = R^T R -// => L^{-1} = R -// Rather than explicitly calculate R and do R*A, we should use L.solve(A) -// The following macro makes L available in the scope where it is used: -#define FACTORIZE_Q_INTO_L(Q) \ - Eigen::LLT llt(Q); \ - auto L = llt.matrixL(); - /* ************************************************************************* */ // Auxiliary function to solve factor graph and return pointer to root // conditional @@ -85,19 +90,19 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - // Perform Cholesky decomposition of P0 - FACTORIZE_Q_INTO_L(P0) + // Perform Cholesky decomposition of P0 = LL^T + InverseL M(P0); - // Premultiply I and x0 with R - const Matrix R = L.solve(I_); // = R - const Vector b = L.solve(x0); // = R*x0 - factorGraph.emplace_shared(0, R, b); + // 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; } /* ************************************************************************* */ @@ -126,13 +131,13 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, assert(Q.cols() == n); #endif - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Premultiply A1 = -F and A2 = I, b = B * u with R - const Matrix A1 = -L.solve(F); // = -R*F - const Matrix A2 = L.solve(I_); // = R - const Vector b = L.solve(B * u); // = R * (B * u) + // 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); } @@ -163,14 +168,13 @@ KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Matrix& Q) const { Key k = step(p); - // Perform Cholesky decomposition of Q - FACTORIZE_Q_INTO_L(Q) + // Perform Cholesky decomposition of Q = LL^T + InverseL M(Q); - // Pre-multiply H and z with R, respectively - const Matrix RtH = L.solve(H); // = R * H - const Vector b = L.solve(z); // = R * z - - return fuse(p, std::make_shared(k, RtH, b)); + // 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)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 7fde9975d..2bf0549fa 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -214,4 +214,4 @@ class GTSAM_EXPORT KalmanFilter { size_t dim() const { return n_; } }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam