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

EKF hierarchy

release/4.3a0
Frank Dellaert 2025-04-29 00:20:41 -04:00
parent 4cc2e22b59
commit 3bcc5563eb
7 changed files with 771 additions and 291 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/IEKF_Rot3Example.cpp → examples/GEKF_Rot3Example.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/GroupEKF.h>
#include <iostream>
@ -53,7 +53,7 @@ int main() {
// Initial estimate (identity) and covariance
const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
const Matrix3 P0 = Matrix3::Identity() * 0.1;
InvariantEKF<Rot3> ekf(R0, P0);
GroupEKF<Rot3> ekf(R0, P0);
// Timestep, process noise, measurement noise
double dt = 0.1;

196
gtsam/navigation/GroupEKF.h Normal file
View File

@ -0,0 +1,196 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file GroupEKF.h
* @brief Extended Kalman Filter derived class for Lie groups G.
*
* This file defines the GroupEKF class template, inheriting from ManifoldEKF,
* for performing EKF steps specifically on states residing in a Lie group.
* It provides predict methods utilizing group composition, tangent space
* controls (via exponential map), and state-dependent dynamics functions.
*
* @date April 24, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#pragma once
#include <gtsam/navigation/ManifoldEKF.h> // Include the base class
#include <gtsam/base/Lie.h> // Include for Lie group traits and operations
#include <Eigen/Dense>
#include <type_traits>
#include <functional> // For std::function
namespace gtsam {
/**
* @class GroupEKF
* @brief Extended Kalman Filter on a Lie group G, derived from ManifoldEKF
*
* @tparam G Lie group type providing group operations and Expmap/AdjointMap.
* Must satisfy LieGroup concept (`gtsam::IsLieGroup<G>::value`).
*
* This filter specializes ManifoldEKF for Lie groups, offering convenient
* prediction methods based on group composition or dynamics functions defining
* motion in the tangent space.
*/
template <typename G>
class GroupEKF : public ManifoldEKF<G> {
public:
using Base = ManifoldEKF<G>; ///< Base class type
static constexpr int n = Base::n; ///< Group dimension (tangent space dimension)
using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G
using MatrixN = typename Base::MatrixN; ///< Square matrix of size n for covariance and Jacobians
using Jacobian = Eigen::Matrix<double, n, n>; ///< Jacobian matrix type specific to the group G
/// Constructor: initialize with state and covariance
GroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {
static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
}
/**
* Predict step via group composition (Left-Invariant):
* X_{k+1} = X_k * U
* P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
* where Ad_{U^{-1}} is the Adjoint map of U^{-1}. This corresponds to
* F = Ad_{U^{-1}} in the base class predict method.
*
* @param U Lie group element representing the motion increment.
* @param Q Process noise covariance in the tangent space (size nxn).
*/
void predict(const G& U, const Matrix& Q) {
G X_next = this->X_.compose(U);
// TODO(dellaert): traits<G>::AdjointMap should exist
Jacobian A = traits<G>::Inverse(U).AdjointMap(); // A = Adjoint(U.inverse())
Base::predict(X_next, A, Q); // Call base class predict
}
/**
* Predict step via tangent control vector:
* U = Expmap(u * dt)
* Then calls predict(U, Q).
*
* @param u Tangent space control vector.
* @param dt Time interval.
* @param Q Process noise covariance matrix (size nxn).
*/
void predict(const TangentVector& u, double dt, const Matrix& Q) {
G U = traits<G>::Expmap(u * dt);
predict(U, Q); // Call the group composition predict
}
/**
* SFINAE check for correctly typed state-dependent dynamics function.
* Signature: TangentVector f(const G& X, OptionalJacobian<n, n> Df)
* Df = d(xi)/d(local(X))
*/
template <typename Dynamics>
using enable_if_dynamics = std::enable_if_t<
!std::is_convertible_v<Dynamics, TangentVector>&&
std::is_invocable_r_v<TangentVector, Dynamics, const G&,
OptionalJacobian<n, n>&>>;
/**
* Predict mean and Jacobian A with state-dependent dynamics:
* xi = f(X_k, Df) (Compute tangent vector dynamics and Jacobian Df)
* U = Expmap(xi * dt, Dexp) (Compute motion increment U and Expmap Jacobian Dexp)
* X_{k+1} = X_k * U (Predict next state)
* F = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian)
*
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
* @param f Dynamics functor returning tangent vector xi and its Jacobian Df w.r.t. local(X).
* @param dt Time step.
* @param A Optional pointer to store the computed state transition Jacobian A.
* @return Predicted state X_{k+1}.
*/
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
G predictMean(Dynamics&& f, double dt, OptionalJacobian<n, n> A = {}) const {
Jacobian Df, Dexp;
TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX)
G U = traits<G>::Expmap(xi * dt, Dexp); // U and Dexp = d(Log(Exp(v)))/dv | v=xi*dt
G X_next = this->X_.compose(U);
if (A) {
// Full state transition Jacobian for left-invariant EKF:
*A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
}
return X_next;
}
/**
* Predict step with state-dependent dynamics:
* Uses predictMean to compute X_{k+1} and F, then calls base predict.
* X_{k+1}, F = predictMean(f, dt)
* P_{k+1} = F P_k F^T + Q
*
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
* @param f Dynamics functor.
* @param dt Time step.
* @param Q Process noise covariance (size nxn).
*/
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
void predict(Dynamics&& f, double dt, const Matrix& Q) {
Jacobian A;
G X_next = predictMean(std::forward<Dynamics>(f), dt, A);
Base::predict(X_next, A, Q); // Call base class predict
}
/**
* SFINAE check for state- and control-dependent dynamics function.
* Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian<n, n> Df)
*/
template<typename Control, typename Dynamics>
using enable_if_full_dynamics = std::enable_if_t<
std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<n, n>&>
>;
/**
* Predict mean and Jacobian A with state and control input dynamics:
* Wraps the dynamics function and calls the state-only predictMean.
* xi = f(X_k, u, Df)
*
* @tparam Control Control input type.
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<n,n>&)
* @param f Dynamics functor.
* @param u Control input.
* @param dt Time step.
* @param A Optional pointer to store the computed state transition Jacobian A.
* @return Predicted state X_{k+1}.
*/
template <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian<n, n> A = {}) const {
return predictMean([&](const G& X, OptionalJacobian<n, n> Df) { return f(X, u, Df); }, dt, A);
}
/**
* Predict step with state and control input dynamics:
* Wraps the dynamics function and calls the state-only predict.
* xi = f(X_k, u, Df)
*
* @tparam Control Control input type.
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<n,n>&)
* @param f Dynamics functor.
* @param u Control input.
* @param dt Time step.
* @param Q Process noise covariance (size nxn).
*/
template <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
return predict([&](const G& X, OptionalJacobian<n, n> Df) { return f(X, u, Df); }, dt, Q);
}
}; // GroupEKF
} // namespace gtsam

267
gtsam/navigation/InvariantEKF.h
View File

@ -9,226 +9,77 @@
* -------------------------------------------------------------------------- */
/**
* @file InvariantEKF.h
* @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation
*
* This file defines the InvariantEKF 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
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
/**
* @file InvariantEKF.h
* @brief Left-Invariant Extended Kalman Filter implementation.
*
* This file defines the InvariantEKF class template, inheriting from GroupEKF,
* which specifically implements the Left-Invariant EKF formulation. It restricts
* prediction methods to only those based on group composition (state-independent
* motion models), hiding the state-dependent prediction variants from GroupEKF.
*
* @date April 24, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <Eigen/Dense>
#include <type_traits>
#include <gtsam/navigation/GroupEKF.h> // Include the base class
namespace gtsam {
/**
* @class InvariantEKF
* @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G
*
* @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()
*
* 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 <typename G>
class InvariantEKF {
public:
/// Tangent-space dimension
static constexpr int n = traits<G>::dimension;
/// Square matrix of size n for covariance and Jacobians
using MatrixN = Eigen::Matrix<double, n, n>;
/// Constructor: initialize with state and covariance
InvariantEKF(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_; }
/**
* 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.
* @class InvariantEKF
* @brief Left-Invariant Extended Kalman Filter on a Lie group G.
*
* @param U Lie group increment (e.g., Expmap of control * dt)
* @param Q process noise covariance in tangent space
* @tparam G Lie group type (must satisfy LieGroup concept).
*
* This filter inherits from GroupEKF but restricts the prediction interface
* to only the left-invariant prediction methods:
* 1. Prediction via group composition: `predict(const G& U, const Matrix& Q)`
* 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Matrix& Q)`
*
* The state-dependent prediction methods from GroupEKF are hidden.
* The update step remains the same as in ManifoldEKF/GroupEKF.
*/
void predict(const G& U, const Matrix& Q) {
typename G::Jacobian A;
X_ = X_.compose(U, A);
P_ = A * P_ * A.transpose() + Q;
}
template <typename G>
class InvariantEKF : public GroupEKF<G> {
public:
using Base = GroupEKF<G>; ///< Base class type
using TangentVector = typename Base::TangentVector; ///< Tangent vector type
using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc.
/**
* 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.
*
* @note Use this if your dynamics does not depend on the state, e.g.
* predict(f(u), dt, q) where u is a control input
*/
void predict(const typename G::TangentVector& u, double dt, const Matrix& Q) {
predict(G::Expmap(u * dt), Q);
}
/// Constructor: forwards to GroupEKF constructor
InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {}
/**
* SFINAE check for correctly typed state-dependent dynamics function.
* xi = f(X, F)
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
*/
template <typename Dynamics>
using enable_if_dynamics = std::enable_if_t<
!std::is_convertible_v<Dynamics, typename G::TangentVector> &&
std::is_invocable_r_v<typename G::TangentVector, Dynamics, const G&,
OptionalJacobian<n, n>&>>;
/**
* Predict mean only with state-dependent dynamics:
* xi = f(X, F)
* X_.expmap(xi * dt)
*
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
* @param f dynamics functor depending on state and control
* @param dt time step
* @param Q process noise covariance
*/
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
G predictMean(Dynamics&& f, double dt, const Matrix& Q,
OptionalJacobian<n, n> A = {}) const {
typename G::Jacobian Df, Dexp;
typename G::TangentVector xi = f(X_, Df);
G U = G::Expmap(xi * dt, Dexp);
// derivative of mean = d[X.compose(U)]/dLog(X)
// = Ad_{U⁻¹} (the “left‐invariant” part)
// plus the effect of X→U via Expmap:
// Dexp * Df * dt (how U moves when X moves through f)
if (A) *A = U.inverse().AdjointMap() + Dexp * Df * dt;
return X_.compose(U);
}
// --- Expose only the Invariant Prediction Methods ---
/**
* Predict step with state-dependent dynamics:
* xi = f(X, F)
* X_.expmap(xi * dt)
*
* @tparam Dynamics signature: G f(const G&, OptionalJacobian<n,n>&)
* @param f dynamics functor depending on state and control
* @param dt time step
* @param Q process noise covariance
*/
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
void predict(Dynamics&& f, double dt, const Matrix& Q) {
typename G::Jacobian A;
X_ = predictMean(f, dt, Q, &A);
P_ = A * P_ * A.transpose() + Q;
}
/**
* Predict step via group composition (Left-Invariant):
* X_{k+1} = X_k * U
* P_{k+1} = Ad_{U^{-1}} P_k Ad_{U^{-1}}^T + Q
* Calls the base class implementation.
*
* @param U Lie group element representing the motion increment.
* @param Q Process noise covariance in the tangent space (size nxn).
*/
void predict(const G& U, const Matrix& Q) {
Base::predict(U, Q);
}
/**
* Predict mean only with state and control input dynamics:
* xi = f(X, u, F)
* X_.expmap(xi * dt)
*
* @tparam Control control input type
* @tparam Dynamics signature: G f(const G&, const Control&,
* 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>
G predictMean(Dynamics&& f, const Control& u, double dt, const Matrix& Q,
OptionalJacobian<n, n> A = {}) {
auto g = [f, u](const G& X, OptionalJacobian<n, n> F) {
return f(X, u, F);
};
return predictMean(g, dt, Q, A);
}
/**
* Predict step via tangent control vector:
* U = Expmap(u * dt)
* Then calls predict(U, Q). Calls the base class implementation.
*
* @param u Tangent space control vector.
* @param dt Time interval.
* @param Q Process noise covariance matrix (size nxn).
*/
void predict(const TangentVector& u, double dt, const Matrix& Q) {
Base::predict(u, dt, Q);
}
/**
* Predict step with state and control input dynamics:
* xi = f(X, u, F)
* X_.expmap(xi * dt)
*
* @tparam Control control input type
* @tparam Dynamics signature: G f(const G&, const Control&,
* 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>
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
auto g = [f, u](const G& X, OptionalJacobian<n, n> F) {
return f(X, u, F);
};
predict(g, dt, Q);
}
}; // InvariantEKF
/**
* 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<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>
void update(Prediction&& h, const Measurement& z, const Matrix& R) {
Eigen::Matrix<double, traits<Measurement>::dimension, n> H;
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_; ///< group state estimate
Matrix P_; ///< covariance in tangent space
private:
/// Identity matrix of size n
static const MatrixN I_n;
};
// Define static identity I_n
template <typename G>
const typename InvariantEKF<G>::MatrixN InvariantEKF<G>::I_n = InvariantEKF<G>::MatrixN::Identity();
} // namespace gtsam
} // namespace gtsam

155
gtsam/navigation/ManifoldEKF.h Normal file
View File

@ -0,0 +1,155 @@
/* ----------------------------------------------------------------------------
* 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)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ManifoldEKF.h
* @brief Extended Kalman Filter base class on a generic manifold M
*
* This file defines the ManifoldEKF class template for performing prediction
* and update steps of an Extended Kalman Filter on states residing in a
* differentiable manifold. It relies on the manifold's retract and
* localCoordinates operations.
*
* @date April 24, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h> // Include for traits
#include <Eigen/Dense>
#include <type_traits>
namespace gtsam {
/**
* @class ManifoldEKF
* @brief Extended Kalman Filter on a generic manifold M
*
* @tparam M Manifold type providing:
* - static int dimension = tangent dimension
* - using TangentVector = Eigen::Vector...
* - A `retract(const TangentVector&)` method (member or static)
* - A `localCoordinates(const M&)` method (member or static)
* - `gtsam::traits<M>` specialization must exist.
*
* This filter maintains a state X in the manifold M and covariance P in the
* tangent space at X. Prediction requires providing the predicted next state
* and the state transition Jacobian F. Updates apply a measurement function h
* and correct the state using the tangent space error.
*/
template <typename M>
class ManifoldEKF {
public:
/// Manifold dimension (tangent space dimension)
static constexpr int n = traits<M>::dimension;
/// Tangent vector type for the manifold M
using TangentVector = typename traits<M>::TangentVector;
/// Square matrix of size n for covariance and Jacobians
using MatrixN = Eigen::Matrix<double, n, n>;
/// Constructor: initialize with state and covariance
ManifoldEKF(const M& X0, const MatrixN& P0) : X_(X0), P_(P0) {
static_assert(IsManifold<M>::value, "Template parameter M must be a GTSAM Manifold");
}
virtual ~ManifoldEKF() = default; // Add virtual destructor for base class
/// @return current state estimate
const M& state() const { return X_; }
/// @return current covariance estimate
const MatrixN& covariance() const { return P_; }
/**
* Basic predict step: Updates state and covariance given the predicted
* next state and the state transition Jacobian F.
* X_{k+1} = X_next
* P_{k+1} = F P_k F^T + Q
* where F = d(local(X_{k+1})) / d(local(X_k)) is the Jacobian of the
* state transition in local coordinates around X_k.
*
* @param X_next The predicted state at time k+1.
* @param F The state transition Jacobian (size nxn).
* @param Q Process noise covariance matrix in the tangent space (size nxn).
*/
void predict(const M& X_next, const MatrixN& F, const Matrix& Q) {
X_ = X_next;
P_ = F * P_ * F.transpose() + Q;
}
/**
* Measurement update: Corrects the state and covariance using a measurement.
* z_pred, H = h(X)
* y = z - z_pred (innovation, or z_pred - z depending on convention)
* S = H P H^T + R (innovation covariance)
* K = P H^T S^{-1} (Kalman gain)
* delta_xi = -K * y (correction in tangent space)
* X <- X.retract(delta_xi)
* P <- (I - K H) P
*
* @tparam Measurement Type of the measurement vector (e.g., VectorN<m>)
* @tparam Prediction Functor signature: Measurement h(const M&,
* OptionalJacobian<m,n>&)
* where m is the measurement dimension.
*
* @param h Measurement model functor returning predicted measurement z_pred
* and its Jacobian H = d(h)/d(local(X)).
* @param z Observed measurement.
* @param R Measurement noise covariance (size m x m).
*/
template <typename Measurement, typename Prediction>
void update(Prediction&& h, const Measurement& z, const Matrix& R) {
constexpr int m = traits<Measurement>::dimension;
Eigen::Matrix<double, m, n> H;
// Predict measurement and get Jacobian H = dh/dlocal(X)
Measurement z_pred = h(X_, H);
// Innovation
// Ensure consistent subtraction for manifold types if Measurement is one
Vector innovation = traits<Measurement>::Local(z, z_pred); // y = z_pred (-) z (in tangent space)
// Innovation covariance and Kalman Gain
auto S = H * P_ * H.transpose() + R;
Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m)
// Correction vector in tangent space
TangentVector delta_xi = -K * innovation; // delta_xi = - K * y
// Update state using retract
X_ = traits<M>::Retract(X_, delta_xi); // X <- X.retract(delta_xi)
// Update covariance using Joseph form:
MatrixN I_KH = I_n - K * H;
P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
}
protected:
M X_; ///< manifold state estimate
MatrixN P_; ///< covariance in tangent space at X_
private:
/// Identity matrix of size n
static const MatrixN I_n;
};
// Define static identity I_n
template <typename M>
const typename ManifoldEKF<M>::MatrixN ManifoldEKF<M>::I_n = ManifoldEKF<M>::MatrixN::Identity();
} // namespace gtsam

114
gtsam/navigation/tests/testGroupEKF.cpp Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* 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)
*
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testGroupEKF.cpp
* @brief Unit test for GroupEKF, as well as dynamics used in Rot3 example.
* @date April 26, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/GroupEKF.h>
#include <gtsam/navigation/NavState.h>
using namespace gtsam;
// Duplicate the dynamics function in GroupEKF_Rot3Example
namespace exampleSO3 {
static constexpr double k = 0.5;
Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
// φ = Logmap(R), Dφ = ∂φ/∂δR
Matrix3 D_phi;
Vector3 phi = Rot3::Logmap(X, D_phi);
// zero out yaw
phi[2] = 0.0;
D_phi.row(2).setZero();
if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR
return -k * phi; // xi ∈ 𝔰𝔬(3)
}
} // namespace exampleSO3
TEST(GroupeEKF, DynamicsJacobian) {
// Construct a nontrivial state and IMU input
Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3);
// Analytic Jacobian
Matrix3 actualH;
exampleSO3::dynamics(R, actualH);
// Numeric Jacobian w.r.t. the state X
auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); };
Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R);
// Compare
EXPECT(assert_equal(expectedH, actualH));
}
TEST(GroupeEKF, PredictNumericState) {
// GIVEN
Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
Matrix3 P0 = Matrix3::Identity() * 0.2;
double dt = 0.1;
// Analytic Jacobian
Matrix3 actualH;
GroupEKF<Rot3> ekf0(R0, P0);
ekf0.predictMean(exampleSO3::dynamics, dt, actualH);
// wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 {
GroupEKF<Rot3> ekf(R, P0);
return ekf.predictMean(exampleSO3::dynamics, dt);
};
// numeric Jacobian of g at R0
Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
EXPECT(assert_equal(expectedH, actualH));
}
TEST(GroupeEKF, StateAndControl) {
auto f = [](const Rot3& X, const Vector2& dummy_u,
OptionalJacobian<3, 3> H = {}) {
return exampleSO3::dynamics(X, H);
};
// GIVEN
Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
Matrix3 P0 = Matrix3::Identity() * 0.2;
Vector2 dummy_u(1, 2);
double dt = 0.1;
Matrix3 Q = Matrix3::Zero();
// Analytic Jacobian
Matrix3 actualH;
GroupEKF<Rot3> ekf0(R0, P0);
ekf0.predictMean(f, dummy_u, dt, actualH);
// wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 {
GroupEKF<Rot3> ekf(R, P0);
return ekf.predictMean(f, dummy_u, dt, Q);
};
// numeric Jacobian of g at R0
Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
EXPECT(assert_equal(expectedH, actualH));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}

172
gtsam/navigation/tests/testInvariantEKF.cpp
View File

@ -7,109 +7,119 @@
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testInvariantEKFNavState.cpp
* @brief Unit test for the NavState dynamics Jacobian in InvariantEKF example.
* @date April 26, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
/**
* @file testInvariantEKF_Pose2.cpp
* @brief Unit test for the InvariantEKF using Pose2 state.
* Based on the logic from IEKF_SE2Example.cpp
* @date April 26, 2025
* @authors Frank Dellaert (adapted from example by Scott Baker, Matt Kielo)
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h>
#include <iostream>
using namespace std;
using namespace gtsam;
// Duplicate the dynamics function in InvariantEKF_Rot3Example
namespace exampleSO3 {
static constexpr double k = 0.5;
Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
// φ = Logmap(R), Dφ = ∂φ/∂δR
Matrix3 D_phi;
Vector3 phi = Rot3::Logmap(X, D_phi);
// zero out yaw
phi[2] = 0.0;
D_phi.row(2).setZero();
if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR
return -k * phi; // xi ∈ 𝔰𝔬(3)
}
} // namespace exampleSO3
TEST(IEKF, dynamicsJacobian) {
// Construct a nontrivial state and IMU input
Rot3 R = Rot3::RzRyRx(0.1, -0.2, 0.3);
// Analytic Jacobian
Matrix3 actualH;
exampleSO3::dynamics(R, actualH);
// Numeric Jacobian w.r.t. the state X
auto f = [&](const Rot3& X_) { return exampleSO3::dynamics(X_); };
Matrix3 expectedH = numericalDerivative11<Vector3, Rot3>(f, R);
// Compare
EXPECT(assert_equal(expectedH, actualH));
// Create a 2D GPS measurement function that returns the predicted measurement h
// and Jacobian H. The predicted measurement h is the translation of the state X.
// (Copied from IEKF_SE2Example.cpp)
Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
return X.translation(H);
}
TEST(IEKF, PredictNumericState) {
// GIVEN
Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
Matrix3 P0 = Matrix3::Identity() * 0.2;
double dt = 0.1;
Matrix3 Q = Matrix3::Zero();
// Analytic Jacobian
Matrix3 actualH;
InvariantEKF<Rot3> iekf0(R0, P0);
iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH);
TEST(IEKF_Pose2, PredictUpdateSequence) {
// GIVEN: Initial state, covariance, noises, motions, measurements
// (from IEKF_SE2Example.cpp)
Pose2 X0(0.0, 0.0, 0.0);
Matrix3 P0 = Matrix3::Identity() * 0.1;
// wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 {
InvariantEKF<Rot3> iekf(R, P0);
return iekf.predictMean(exampleSO3::dynamics, dt, Q);
};
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
Matrix2 R = I_2x2 * 0.01;
// numeric Jacobian of g at R0
Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
EXPECT(assert_equal(expectedH, actualH));
}
Vector2 z1, z2;
z1 << 1.0, 0.0;
z2 << 1.0, 1.0;
TEST(IEKF, StateAndControl) {
auto f = [](const Rot3& X, const Vector2& dummy_u,
OptionalJacobian<3, 3> H = {}) {
return exampleSO3::dynamics(X, H);
};
// Create the filter
InvariantEKF<Pose2> ekf(X0, P0);
EXPECT(assert_equal(X0, ekf.state()));
EXPECT(assert_equal(P0, ekf.covariance()));
// GIVEN
Rot3 R0 = Rot3::RzRyRx(0.2, -0.1, 0.3);
Matrix3 P0 = Matrix3::Identity() * 0.2;
Vector2 dummy_u(1, 2);
double dt = 0.1;
Matrix3 Q = Matrix3::Zero();
// --- First Prediction ---
ekf.predict(U1, Q);
// Analytic Jacobian
Matrix3 actualH;
InvariantEKF<Rot3> iekf0(R0, P0);
iekf0.predictMean(f, dummy_u, dt, Q, actualH);
// Calculate expected state and covariance
Pose2 X1_expected = X0.compose(U1);
Matrix3 Ad_U1_inv = U1.inverse().AdjointMap();
Matrix3 P1_expected = Ad_U1_inv * P0 * Ad_U1_inv.transpose() + Q;
// wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 {
InvariantEKF<Rot3> iekf(R, P0);
return iekf.predictMean(f, dummy_u, dt, Q);
};
// Verify
EXPECT(assert_equal(X1_expected, ekf.state(), 1e-9));
EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9));
// numeric Jacobian of g at R0
Matrix3 expectedH = numericalDerivative11<Rot3, Rot3>(g, R0);
EXPECT(assert_equal(expectedH, actualH));
// --- First Update ---
ekf.update(h_gps, z1, R);
// Calculate expected state and covariance (manual Kalman steps)
Matrix H1; // H = dh/dlocal(X) -> 2x3
Vector2 z_pred1 = h_gps(X1_expected, H1);
Vector2 y1 = z_pred1 - z1; // Innovation
Matrix S1 = H1 * P1_expected * H1.transpose() + R;
Matrix K1 = P1_expected * H1.transpose() * S1.inverse(); // Gain (3x2)
Vector3 delta_xi1 = -K1 * y1; // Correction (tangent space)
Pose2 X1_updated_expected = X1_expected.retract(delta_xi1);
Matrix3 I_KH1 = Matrix3::Identity() - K1 * H1;
Matrix3 P1_updated_expected = I_KH1 * P1_expected; // Standard form P = (I-KH)P
// Verify
EXPECT(assert_equal(X1_updated_expected, ekf.state(), 1e-9));
EXPECT(assert_equal(P1_updated_expected, ekf.covariance(), 1e-9));
// --- Second Prediction ---
ekf.predict(U2, Q);
// Calculate expected state and covariance
Pose2 X2_expected = X1_updated_expected.compose(U2);
Matrix3 Ad_U2_inv = U2.inverse().AdjointMap();
Matrix3 P2_expected = Ad_U2_inv * P1_updated_expected * Ad_U2_inv.transpose() + Q;
// Verify
EXPECT(assert_equal(X2_expected, ekf.state(), 1e-9));
EXPECT(assert_equal(P2_expected, ekf.covariance(), 1e-9));
// --- Second Update ---
ekf.update(h_gps, z2, R);
// Calculate expected state and covariance (manual Kalman steps)
Matrix H2; // 2x3
Vector2 z_pred2 = h_gps(X2_expected, H2);
Vector2 y2 = z_pred2 - z2; // Innovation
Matrix S2 = H2 * P2_expected * H2.transpose() + R;
Matrix K2 = P2_expected * H2.transpose() * S2.inverse(); // Gain (3x2)
Vector3 delta_xi2 = -K2 * y2; // Correction (tangent space)
Pose2 X2_updated_expected = X2_expected.retract(delta_xi2);
Matrix3 I_KH2 = Matrix3::Identity() - K2 * H2;
Matrix3 P2_updated_expected = I_KH2 * P2_expected; // Standard form
// Verify
EXPECT(assert_equal(X2_updated_expected, ekf.state(), 1e-9));
EXPECT(assert_equal(P2_updated_expected, ekf.covariance(), 1e-9));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
}

154
gtsam/navigation/tests/testManifoldEKF.cpp Normal file
View File

@ -0,0 +1,154 @@
/* ----------------------------------------------------------------------------
* 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)
*
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testManifoldEKF.cpp
* @brief Unit test for the ManifoldEKF base class using Unit3.
* @date April 26, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/navigation/ManifoldEKF.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace gtsam;
// Define simple dynamics for Unit3: constant velocity in the tangent space
namespace exampleUnit3 {
// Predicts the next state given current state, tangent velocity, and dt
Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) {
return p.retract(v * dt);
}
// Define a measurement model: measure the z-component of the Unit3 direction
// H is the Jacobian dh/d(local(p))
Vector1 measureZ(const Unit3& p, OptionalJacobian<1, 2> H) {
if (H) {
// H = d(p.point3().z()) / d(local(p))
// Calculate numerically for simplicity in test
auto h = [](const Unit3& p_) { return Vector1(p_.point3().z()); };
*H = numericalDerivative11<Vector1, Unit3, 2>(h, p);
}
return Vector1(p.point3().z());
}
} // namespace exampleUnit3
// Test fixture for ManifoldEKF with Unit3
struct Unit3EKFTest {
Unit3 p0;
Matrix2 P0;
Vector2 velocity;
double dt;
Matrix2 Q; // Process noise
Matrix1 R; // Measurement noise
Unit3EKFTest() :
p0(Unit3(Point3(1, 0, 0))), // Start pointing along X-axis
P0(I_2x2 * 0.01),
velocity((Vector2() << 0.0, M_PI / 4.0).finished()), // Rotate towards +Z axis
dt(0.1),
Q(I_2x2 * 0.001),
R(Matrix1::Identity() * 0.01) {
}
};
TEST(ManifoldEKF_Unit3, Predict) {
Unit3EKFTest data;
// Initialize the EKF
ManifoldEKF<Unit3> ekf(data.p0, data.P0);
// --- Prepare inputs for ManifoldEKF::predict ---
// 1. Compute expected next state
Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt);
// 2. Compute state transition Jacobian F = d(local(p_next)) / d(local(p))
// We can compute this numerically using the predictNextState function.
// GTSAM's numericalDerivative handles derivatives *between* manifolds.
auto predict_wrapper = [&](const Unit3& p) -> Unit3 {
return exampleUnit3::predictNextState(p, data.velocity, data.dt);
};
Matrix2 F = numericalDerivative11<Unit3, Unit3>(predict_wrapper, data.p0);
// --- Perform EKF prediction ---
ekf.predict(p_next_expected, F, data.Q);
// --- Verification ---
// Check state
EXPECT(assert_equal(p_next_expected, ekf.state(), 1e-8));
// Check covariance
Matrix2 P_expected = F * data.P0 * F.transpose() + data.Q;
EXPECT(assert_equal(P_expected, ekf.covariance(), 1e-8));
// Check F manually for a simple case (e.g., zero velocity should give Identity)
Vector2 zero_velocity = Vector2::Zero();
auto predict_wrapper_zero = [&](const Unit3& p) -> Unit3 {
return exampleUnit3::predictNextState(p, zero_velocity, data.dt);
};
Matrix2 F_zero = numericalDerivative11<Unit3, Unit3>(predict_wrapper_zero, data.p0);
EXPECT(assert_equal<Matrix2>(I_2x2, F_zero, 1e-8));
}
TEST(ManifoldEKF_Unit3, Update) {
Unit3EKFTest data;
// Use a slightly different starting point and covariance for variety
Unit3 p_start = Unit3(Point3(0, 1, 0)).retract((Vector2() << 0.1, 0).finished()); // Perturb pointing along Y
Matrix2 P_start = I_2x2 * 0.05;
ManifoldEKF<Unit3> ekf(p_start, P_start);
// Simulate a measurement (e.g., true value + noise)
Vector1 z_true = exampleUnit3::measureZ(p_start, {});
Vector1 z_observed = z_true + Vector1(0.02); // Add some noise
// --- Perform EKF update ---
ekf.update(exampleUnit3::measureZ, z_observed, data.R);
// --- Verification (Manual Kalman Update Steps) ---
// 1. Predict measurement and get Jacobian H
Matrix12 H; // Note: Jacobian is 1x2 for Unit3
Vector1 z_pred = exampleUnit3::measureZ(p_start, H);
// 2. Innovation and Covariance
Vector1 y = z_pred - z_observed; // Innovation (using vector subtraction for z)
Matrix1 S = H * P_start * H.transpose() + data.R; // 1x1 matrix
// 3. Kalman Gain K
Matrix K = P_start * H.transpose() * S.inverse(); // 2x1 matrix
// 4. State Correction (in tangent space)
Vector2 delta_xi = -K * y; // 2x1 vector
// 5. Expected Updated State and Covariance
Unit3 p_updated_expected = p_start.retract(delta_xi);
Matrix2 I_KH = I_2x2 - K * H;
Matrix2 P_updated_expected = I_KH * P_start;
// --- Compare EKF result with manual calculation ---
EXPECT(assert_equal(p_updated_expected, ekf.state(), 1e-8));
EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8));
}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}