From f0e35aecea7edf61f74a4f110b9c85ce418d7160 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 12:24:00 -0400 Subject: [PATCH] Comments with o4-mini --- gtsam/nonlinear/LIEKF.h | 166 ++++++++++++++++++++++------------------ 1 file changed, 90 insertions(+), 76 deletions(-) diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 3d71337fa..840d004a7 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/gtsam/nonlinear/LIEKF.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- -* GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -10,82 +10,82 @@ * -------------------------------------------------------------------------- */ /** - * @file LIEKF.h - * @brief Base and classes for Left Invariant Extended Kalman Filters + * @file LIEKF.h + * @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation * - * Templates are implemented for a Left Invariant Extended Kalman Filter - * operating on Lie Groups. + * This file defines the LIEKF class template for performing prediction and + * update steps of an Extended Kalman Filter on states residing in a Lie group. + * The class supports state evolution via group composition and dynamics + * functions, along with measurement updates using tangent-space corrections. * - * - * @date April 24, 2025 - * @author Scott Baker - * @author Matt Kielo - * @author Frank Dellaert + * @date April 24, 2025 + * @authors Scott Baker, Matt Kielo, Frank Dellaert */ #pragma once + #include #include #include #include -#include + + namespace gtsam { /** - * @brief Base class for Left Invariant Extended Kalman Filter (LIEKF) + * @class LIEKF + * @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G * - * This class provides the prediction and update structure based on control - * inputs and a measurement function. + * @tparam G Lie group type providing: + * - static int dimension = tangent dimension + * - using TangentVector = Eigen::Vector... + * - using Jacobian = Eigen::Matrix... + * - methods: Expmap(), expmap(), compose(), inverse().AdjointMap() * - * @tparam G Lie group used for state representation (e.g., Pose2, - * Pose3, NavState) - * @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement - * for 3D position) + * This filter maintains a state X in the group G and covariance P in the + * tangent space. Prediction steps are performed via group composition or a + * user-supplied dynamics function. Updates apply a measurement function h + * returning both predicted measurement and its Jacobian H, and correct state + * using the left-invariant error in the tangent space. */ - template class LIEKF { public: - static constexpr int n = traits::dimension; ///< Dimension of the state. + /// Tangent-space dimension + static constexpr int n = traits::dimension; - using MatrixN = - Eigen::Matrix; ///< Typedef for the identity matrix. + /// Square matrix of size n for covariance and Jacobians + using MatrixN = Eigen::Matrix; + + /// Constructor: initialize with state and covariance + LIEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} + + /// @return current state estimate + const G& state() const { return X_; } + + /// @return current covariance estimate + const Matrix& covariance() const { return P_; } /** - * @brief Construct with a measurement function - * @param X0 Initial State - * @param P0 Initial Covariance - * @param h Measurement function - */ - LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} - - /** - * @brief Get current state estimate. - * @return Const reference to the state estimate. - */ - const G& state() const { return X; } - - /** - * @brief Get current covariance estimate. - * @return Const reference to the covariance estimate. - */ - const Matrix& covariance() const { return P; } - - /** - * @brief Prediction stage with a Lie group element U. - * @param U Lie group control input - * @param Q Process noise covariance matrix. + * Predict step via group composition: + * X_{k+1} = X_k * U + * P_{k+1} = A P_k A^T + Q + * where A = Ad_{U^{-1}}. i.e., d(X.compose(U))/dX evaluated at X_k. + * + * @param U Lie group increment (e.g., Expmap of control * dt) + * @param Q process noise covariance in tangent space */ void predict(const G& U, const Matrix& Q) { typename G::Jacobian A; - X = X.compose(U, A); - P = A * P * A.transpose() + Q; + X_ = X_.compose(U, A); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Prediction stage with a tangent vector xi and a time interval dt. - * @param u Control vector element + * Predict step via tangent control vector: + * U = Expmap(u * dt) + * @param u tangent control vector * @param dt Time interval * @param Q Process noise covariance matrix. * @@ -97,52 +97,66 @@ class LIEKF { } /** - * @brief Prediction stage with a dynamics function that calculates the - * tangent vector xi that *depends on the state*. - * @tparam Control The control input type - * @tparam Dynamics : (G, Control, OptionalJacobian) -> TangentVector - * @param f Dynamics function that depends on state and control input - * @param u Control input - * @param dt Time interval - * @param Q Process noise covariance matrix. + * Predict step with state-dependent dynamics: + * xi = f(X, u, F) + * U = Expmap(xi * dt) + * A = Ad_{U^{-1}} * F + * + * @tparam Control control input type + * @tparam Dynamics signature: G f(const G&, const Control&, + * OptionalJacobian&) + * + * @param f dynamics functor depending on state and control + * @param u control input + * @param dt time step + * @param Q process noise covariance */ template void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { typename G::Jacobian F; - const typename G::TangentVector xi = f(X, u, F); + auto xi = f(X_, u, F); G U = G::Expmap(xi * dt); - auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f - X = X.compose(U); - P = A * P * A.transpose() + Q; + auto A = U.inverse().AdjointMap() * F; + X_ = X_.compose(U); + P_ = A * P_ * A.transpose() + Q; } /** - * @brief Update stage using a measurement and measurement covariance. - * @tparam Measurement The measurement output type - * @tparam Prediction : (G, OptionalJacobian) -> Measurement - * @param z Measurement - * @param R Measurement noise covariance matrix. + * Measurement update: + * z_pred, H = h(X) + * K = P H^T (H P H^T + R)^{-1} + * X <- Expmap(-K (z_pred - z)) * X + * P <- (I - K H) P + * + * @tparam Measurement measurement type (e.g., Vector) + * @tparam Prediction functor signature: Measurement h(const G&, + * OptionalJacobian&) + * + * @param h measurement model returning predicted z and Jacobian H + * @param z observed measurement + * @param R measurement noise covariance */ template void update(Prediction&& h, const Measurement& z, const Matrix& R) { Eigen::Matrix::dimension, n> H; - Vector y = h(X, H) - z; - Matrix S = H * P * H.transpose() + R; - Matrix K = P * H.transpose() * S.inverse(); - X = X.expmap(-K * y); - P = (I_n - K * H) * P; // move Identity to be a constant. + auto z_pred = h(X_, H); + auto y = z_pred - z; + Matrix S = H * P_ * H.transpose() + R; + Matrix K = P_ * H.transpose() * S.inverse(); + X_ = X_.expmap(-K * y); + P_ = (I_n - K * H) * P_; } protected: - G X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X_; ///< group state estimate + Matrix P_; ///< covariance in tangent space private: - static const MatrixN - I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. + /// Identity matrix of size n + static const MatrixN I_n; }; -/// Create the static identity matrix I_n of size nxn for use in update +// Define static identity I_n template const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity();