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

Comments with o4-mini

release/4.3a0
Frank Dellaert 2025-04-26 12:24:00 -04:00
parent 9352465494
commit f0e35aecea
1 changed files with 90 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
gtsam/nonlinear/LIEKF.h
View File

@ -1,6 +1,6 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -11,81 +11,81 @@
/** /**
* @file LIEKF.h * @file LIEKF.h
* @brief Base and classes for Left Invariant Extended Kalman Filters * @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 * @date April 24, 2025
* @author Scott Baker * @authors Scott Baker, Matt Kielo, Frank Dellaert
* @author Matt Kielo
* @author Frank Dellaert
*/ */
#pragma once #pragma once
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <Eigen/Dense> #include <Eigen/Dense>
#include <functional>
namespace gtsam { 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 * @tparam G Lie group type providing:
* inputs and a measurement function. * - 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, * This filter maintains a state X in the group G and covariance P in the
* Pose3, NavState) * tangent space. Prediction steps are performed via group composition or a
* @tparam Measurement Type of measurement (e.g. Vector3 for a GPS measurement * user-supplied dynamics function. Updates apply a measurement function h
* for 3D position) * returning both predicted measurement and its Jacobian H, and correct state
* using the left-invariant error in the tangent space.
*/ */
template <typename G> template <typename G>
class LIEKF { class LIEKF {
public: public:
static constexpr int n = traits<G>::dimension; ///< Dimension of the state. /// Tangent-space dimension
static constexpr int n = traits<G>::dimension;
using MatrixN = /// Square matrix of size n for covariance and Jacobians
Eigen::Matrix<double, n, n>; ///< Typedef for the identity matrix. using MatrixN = Eigen::Matrix<double, n, n>;
/// 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 * Predict step via group composition:
* @param X0 Initial State * X_{k+1} = X_k * U
* @param P0 Initial Covariance * P_{k+1} = A P_k A^T + Q
* @param h Measurement function * where A = Ad_{U^{-1}}. i.e., d(X.compose(U))/dX evaluated at X_k.
*/ *
LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} * @param U Lie group increment (e.g., Expmap of control * dt)
* @param Q process noise covariance in tangent space
/**
* @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.
*/ */
void predict(const G& U, const Matrix& Q) { void predict(const G& U, const Matrix& Q) {
typename G::Jacobian A; typename G::Jacobian A;
X = X.compose(U, A); X_ = X_.compose(U, A);
P = A * P * A.transpose() + Q; P_ = A * P_ * A.transpose() + Q;
} }
/** /**
* @brief Prediction stage with a tangent vector xi and a time interval dt. * Predict step via tangent control vector:
* @param u Control vector element * U = Expmap(u * dt)
* @param u tangent control vector
* @param dt Time interval * @param dt Time interval
* @param Q Process noise covariance matrix. * @param Q Process noise covariance matrix.
* *
@ -97,52 +97,66 @@ class LIEKF {
} }
/** /**
* @brief Prediction stage with a dynamics function that calculates the * Predict step with state-dependent dynamics:
* tangent vector xi that *depends on the state*. * xi = f(X, u, F)
* @tparam Control The control input type * U = Expmap(xi * dt)
* @tparam Dynamics : (G, Control, OptionalJacobian<n,n>) -> TangentVector * A = Ad_{U^{-1}} * F
* @param f Dynamics function that depends on state and control input *
* @param u Control input * @tparam Control control input type
* @param dt Time interval * @tparam Dynamics signature: G f(const G&, const Control&,
* @param Q Process noise covariance matrix. * OptionalJacobian<n,n>&)
*
* @param f dynamics functor depending on state and control
* @param u control input
* @param dt time step
* @param Q process noise covariance
*/ */
template <typename Control, typename Dynamics> template <typename Control, typename Dynamics>
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
typename G::Jacobian F; 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); G U = G::Expmap(xi * dt);
auto A = U.inverse().AdjointMap() * F; // chain rule for compose and f auto A = U.inverse().AdjointMap() * F;
X = X.compose(U); X_ = X_.compose(U);
P = A * P * A.transpose() + Q; P_ = A * P_ * A.transpose() + Q;
} }
/** /**
* @brief Update stage using a measurement and measurement covariance. * Measurement update:
* @tparam Measurement The measurement output type * z_pred, H = h(X)
* @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement * K = P H^T (H P H^T + R)^{-1}
* @param z Measurement * X <- Expmap(-K (z_pred - z)) * X
* @param R Measurement noise covariance matrix. * P <- (I - K H) P
*
* @tparam Measurement measurement type (e.g., Vector)
* @tparam Prediction functor signature: Measurement h(const G&,
* OptionalJacobian<m,n>&)
*
* @param h measurement model returning predicted z and Jacobian H
* @param z observed measurement
* @param R measurement noise covariance
*/ */
template <typename Measurement, typename Prediction> template <typename Measurement, typename Prediction>
void update(Prediction&& h, const Measurement& z, const Matrix& R) { void update(Prediction&& h, const Measurement& z, const Matrix& R) {
Eigen::Matrix<double, traits<Measurement>::dimension, n> H; Eigen::Matrix<double, traits<Measurement>::dimension, n> H;
Vector y = h(X, H) - z; auto z_pred = h(X_, H);
Matrix S = H * P * H.transpose() + R; auto y = z_pred - z;
Matrix K = P * H.transpose() * S.inverse(); Matrix S = H * P_ * H.transpose() + R;
X = X.expmap(-K * y); Matrix K = P_ * H.transpose() * S.inverse();
P = (I_n - K * H) * P; // move Identity to be a constant. X_ = X_.expmap(-K * y);
P_ = (I_n - K * H) * P_;
} }
protected: protected:
G X; ///< Current state estimate. G X_; ///< group state estimate
Matrix P; ///< Current covariance estimate. Matrix P_; ///< covariance in tangent space
private: private:
static const MatrixN /// Identity matrix of size n
I_n; ///< A nxn identity matrix used in the update stage of the LIEKF. 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 <typename G> template <typename G>
const typename LIEKF<G>::MatrixN LIEKF<G>::I_n = LIEKF<G>::MatrixN::Identity(); const typename LIEKF<G>::MatrixN LIEKF<G>::I_n = LIEKF<G>::MatrixN::Identity();