Templated predict/update

examples/LIEKF_NavstateExample.cpp

@@ -11,10 +11,10 @@
 
 /**
  * @file LIEKF_NavstateExample.cpp
- * @brief A left invariant Extended Kalman Filter example using the GeneralLIEKF
+ * @brief A left invariant Extended Kalman Filter example using the LIEKF
  * on NavState using IMU/GPS measurements.
  *
- * This example uses the templated GeneralLIEKF class to estimate the state of
+ * This example uses the templated LIEKF class to estimate the state of
  * an object using IMU/GPS measurements. The prediction stage of the LIEKF uses
  * a generic dynamics function to predict the state. This simulates a navigation
  * state of (pose, velocity, position)
@@ -64,16 +64,9 @@ int main() {
   Matrix9 P0 = Matrix9::Identity() * 0.1;
   double dt = 1.0;
 
-  // Create the measurement function h_func that wraps h_gps
-  GeneralLIEKF<NavState, Vector3, 6>::MeasurementFunction h_func =
-      [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); };
-
-  // Create the dynamics function dynamics_func
-  GeneralLIEKF<NavState, Vector3, 6>::Dynamics dynamics_func = dynamics;
-
   // Create the filter with the initial state, covariance, and dynamics and
   // measurement functions.
-  GeneralLIEKF<NavState, Vector3, 6> ekf(X0, P0, dynamics_func, h_func);
+  LIEKF<NavState> ekf(X0, P0);
 
   // Create the process covariance and measurement covariance matrices Q and R.
   Matrix9 Q = Matrix9::Identity() * 0.01;
@@ -96,25 +89,25 @@ int main() {
   cout << "P0: " << ekf.covariance() << endl;
 
   // First prediction stage
-  ekf.predict(imu1, dt, Q);
+  ekf.predict<6>(dynamics, imu1, dt, Q);
   cout << "\nFirst Prediction:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;
 
   // First update stage
-  ekf.update(z1, R);
+  ekf.update(h_gps, z1, R);
   cout << "\nFirst Update:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;
 
   // Second prediction stage
-  ekf.predict(imu2, dt, Q);
+  ekf.predict<6>(dynamics, imu2, dt, Q);
   cout << "\nSecond Prediction:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;
 
   // Second update stage
-  ekf.update(z2, R);
+  ekf.update(h_gps, z2, R);
   cout << "\nSecond Update:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << ekf.covariance() << endl;

examples/LIEKF_SE2_SimpleGPSExample.cpp

@@ -53,14 +53,10 @@ int main() {
   // // Initialize the filter's state, covariance, and time interval values.
   Pose2 X0(0.0, 0.0, 0.0);
   Matrix3 P0 = Matrix3::Identity() * 0.1;
-  double dt = 1.0;
 
-  // Create the function measurement_function that wraps h_gps.
-  std::function<Vector2(const Pose2&, gtsam::OptionalJacobian<2, 3>)>
-      measurement_function = h_gps;
   // Create the filter with the initial state, covariance, and measurement
   // function.
-  LIEKF<Pose2, Vector2> ekf(X0, P0, measurement_function);
+  LIEKF<Pose2> ekf(X0, P0);
 
   // Define the process covariance and measurement covariance matrices Q and R.
   Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
@@ -100,7 +96,7 @@ int main() {
        << endl;
 
   // First update stage
-  ekf.update(z1, R);
+  ekf.update(h_gps, z1, R);
   cout << "\nFirst Update:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
@@ -114,7 +110,7 @@ int main() {
        << endl;
 
   // Second update stage
-  ekf.update(z2, R);
+  ekf.update(h_gps, z2, R);
   cout << "\nSecond Update:\n";
   cout << "X: " << ekf.state() << endl;
   cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()

gtsam/nonlinear/LIEKF.h

@@ -38,23 +38,17 @@ namespace gtsam {
  * This class provides the prediction and update structure based on control
  * inputs and a measurement function.
  *
- * @tparam LieGroup  Lie group used for state representation (e.g., Pose2,
+ * @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)
  */
 
-template <typename LieGroup, typename Measurement>
+template <typename G>
 class LIEKF {
  public:
-  static constexpr int n =
+  static constexpr int n = traits<G>::dimension;  ///< Dimension of the state.
-      traits<LieGroup>::dimension;  ///< Dimension of the state.
-  static constexpr int m =
-      traits<Measurement>::dimension;  ///< Dimension of the measurement.
 
-  using MeasurementFunction = std::function<Measurement(
-      const LieGroup&,
-      OptionalJacobian<m, n>)>;  ///< Typedef for the measurement function.
   using MatrixN =
       Eigen::Matrix<double, n, n>;  ///< Typedef for the identity matrix.
 
@@ -64,14 +58,13 @@ class LIEKF {
    * @param P0 Initial Covariance
    * @param h Measurement function
    */
-  LIEKF(const LieGroup& X0, const Matrix& P0, MeasurementFunction& h)  // X_ P_
+  LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {}
-      : X(X0), P(P0), h_(h) {}
 
   /**
    * @brief Get current state estimate.
-   * @return Const reference to the state estiamte.
+   * @return Const reference to the state estimate.
    */
-  const LieGroup& state() const { return X; }
+  const G& state() const { return X; }
 
   /**
    * @brief Get current covariance estimate.
@@ -84,8 +77,8 @@ class LIEKF {
    * @param U Lie group control input
    * @param Q Process noise covariance matrix.
    */
-  void predict(const LieGroup& U, const Matrix& Q) {
+  void predict(const G& U, const Matrix& Q) {
-    LieGroup::Jacobian A;
+    typename G::Jacobian A;
     X = X.compose(U, A);
     P = A * P * A.transpose() + Q;
   }
@@ -97,17 +90,41 @@ class LIEKF {
    * @param Q Process noise covariance matrix.
    */
   void predict(const Vector& u, double dt, const Matrix& Q) {
-    predict(LieGroup::Expmap(u * dt), Q);
+    predict(G::Expmap(u * dt), Q);
+  }
+
+  /**
+   * @brief Prediction stage with a dynamics function that calculates the
+   * tangent vector xi in the tangent space.
+   * @tparam _p The dimension of the control vector.
+   * @tparam Dynamics : (G, VectorP, OptionalJacobian<n,n>) -> TangentVector
+   * @param f Dynamics function that depends on state and control vector
+   * @param u Control vector element
+   * @param dt Time interval
+   * @param Q Process noise covariance matrix.
+   */
+  template <size_t _p, typename Dynamics>
+  void predict(Dynamics&& f, const Eigen::Matrix<double, _p, 1>& u, double dt,
+               const Matrix& Q) {
+    typename G::Jacobian F;
+    const typename G::TangentVector xi = f(X, u, F);
+    G U = G::Expmap(xi * dt);
+    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
+   * @tparam Prediction : (G, OptionalJacobian<m,n>) -> Measurement
    * @param z Measurement
    * @param R Measurement noise covariance matrix.
    */
-  void update(const Measurement& z, const Matrix& R) {
+  template <typename Measurement, typename Prediction>
-    Matrix H(m, n);
+  void update(Prediction&& h, const Measurement& z, const Matrix& R) {
-    Vector y = h_(X, H) - z;
+    Eigen::Matrix<double, traits<Measurement>::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);
@@ -115,79 +132,16 @@ class LIEKF {
   }
 
  protected:
-  LieGroup X;  ///< Current state estimate.
+  G X;       ///< Current state estimate.
-  Matrix P;    ///< Current covariance estimate.
+  Matrix P;  ///< Current covariance estimate.
 
  private:
-  MeasurementFunction h_;  ///< Measurement function
   static const MatrixN
       I_n;  ///< A nxn identity matrix used in the update stage of the LIEKF.
 };
 
-/**
+/// Create the static identity matrix I_n of size nxn for use in update
- * @brief Create the static identity matrix I_n of size nxn for use in the
+template <typename G>
- * update stage.
+const typename LIEKF<G>::MatrixN LIEKF<G>::I_n = LIEKF<G>::MatrixN::Identity();
- * @tparam LieGroup  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)
- */
-template <typename LieGroup, typename Measurement>
-const typename LIEKF<LieGroup, Measurement>::MatrixN
-    LIEKF<LieGroup, Measurement>::I_n =
-        typename LIEKF<LieGroup, Measurement>::MatrixN::Identity();
-
-/**
- * @brief General Left Invariant Extended Kalman Filter with dynamics function.
- *
- * This class extends the LIEKF class to include a dynamics function f. The
- * dynamics maps a state and control vector to a tangent vector xi.
- *
- * @tparam LieGroup The Lie group type for the state.
- * @tparam Measurement The type of the measurement.
- * @tparam _p The dimension of the control vector.
- */
-template <typename LieGroup, typename Measurement, size_t _p>
-class GeneralLIEKF : public LIEKF<LieGroup, Measurement> {
- public:
-  using Control =
-      Eigen::Matrix<double, _p, 1>;  ///< Typedef for the control vector.
-  using TangentVector =
-      typename traits<LieGroup>::TangentVector;  ///< Typedef for the tangent
-                                                 ///< vector.
-  using Dynamics = std::function<TangentVector(
-      const LieGroup&, const Control&,  ///< Typedef for the dynamics function.
-      OptionalJacobian<n, n>)>;
-
-  /**
-   * @brief Construct with general dynamics
-   * @param X0 Initial State
-   * @param P0 Initial Covariance
-   * @param f Dynamics function that depends on state and control vector
-   * @param h Measurement function
-   */
-  GeneralLIEKF(const LieGroup& X0, const Matrix& P0, Dynamics& f,
-               MeasurementFunction& h)
-      : LIEKF(X0, P0, h), f_(f) {}
-
-  /**
-   * @brief Prediction stage with a dynamics function that calculates the
-   * tangent vector xi in the tangent space.
-   * @param u Control vector element
-   * @param dt Time interval
-   * @param Q Process noise covariance matrix.
-   */
-  void predict(const Control& u, double dt, const Matrix& Q) {
-    LieGroup::Jacobian H;
-    const TangentVector xi = f_(X, u, H);
-    LieGroup U = LieGroup::Expmap(xi * dt);
-    auto A = U.inverse().AdjointMap() * H;
-    X = X.compose(U);
-    P = A * P * A.transpose() + Q;
-  }
-
- private:
-  Dynamics f_;  ///< Dynamics function.
-};
 
 }  // namespace gtsam