diff --git a/examples/LIEKF_NavstateExample.cpp b/examples/LIEKF_NavstateExample.cpp index 6c6d594a7..22639acb1 100644 --- a/examples/LIEKF_NavstateExample.cpp +++ b/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::MeasurementFunction h_func = - [](const NavState& X, OptionalJacobian<3, 9> H) { return h_gps(X, H); }; - - // Create the dynamics function dynamics_func - GeneralLIEKF::Dynamics dynamics_func = dynamics; - // Create the filter with the initial state, covariance, and dynamics and // measurement functions. - GeneralLIEKF ekf(X0, P0, dynamics_func, h_func); + LIEKF 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; diff --git a/examples/LIEKF_SE2_SimpleGPSExample.cpp b/examples/LIEKF_SE2_SimpleGPSExample.cpp index c2d0acdb6..1f0a01212 100644 --- a/examples/LIEKF_SE2_SimpleGPSExample.cpp +++ b/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)> - measurement_function = h_gps; // Create the filter with the initial state, covariance, and measurement // function. - LIEKF ekf(X0, P0, measurement_function); + LIEKF 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() diff --git a/gtsam/nonlinear/LIEKF.h b/gtsam/nonlinear/LIEKF.h index 598a6300a..93fe5c8b5 100644 --- a/gtsam/nonlinear/LIEKF.h +++ b/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 +template class LIEKF { public: - static constexpr int n = - traits::dimension; ///< Dimension of the state. - static constexpr int m = - traits::dimension; ///< Dimension of the measurement. + static constexpr int n = traits::dimension; ///< Dimension of the state. - using MeasurementFunction = std::function)>; ///< Typedef for the measurement function. using MatrixN = Eigen::Matrix; ///< 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_ - : X(X0), P(P0), h_(h) {} + LIEKF(const G& X0, const Matrix& P0) : X(X0), P(P0) {} /** * @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) { - LieGroup::Jacobian A; + void predict(const G& U, const Matrix& Q) { + 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) -> 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 + void predict(Dynamics&& f, const Eigen::Matrix& 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) -> Measurement * @param z Measurement * @param R Measurement noise covariance matrix. */ - void update(const Measurement& z, const Matrix& R) { - Matrix H(m, n); - Vector y = h_(X, H) - z; + 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); @@ -115,79 +132,16 @@ class LIEKF { } protected: - LieGroup X; ///< Current state estimate. - Matrix P; ///< Current covariance estimate. + G X; ///< Current state 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. }; -/** - * @brief Create the static identity matrix I_n of size nxn for use in the - * update stage. - * @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 -const typename LIEKF::MatrixN - LIEKF::I_n = - typename LIEKF::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 -class GeneralLIEKF : public LIEKF { - public: - using Control = - Eigen::Matrix; ///< Typedef for the control vector. - using TangentVector = - typename traits::TangentVector; ///< Typedef for the tangent - ///< vector. - using Dynamics = std::function)>; - - /** - * @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. -}; +/// Create the static identity matrix I_n of size nxn for use in update +template +const typename LIEKF::MatrixN LIEKF::I_n = LIEKF::MatrixN::Identity(); } // namespace gtsam