Frank Dellaert
GitHub
commit
e41c2eef12
|
|
@ -0,0 +1,77 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IEKF_Rot3Example.cpp
|
||||
* @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
|
||||
* and a single magnetometer update.
|
||||
* @date April 25, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/navigation/LieGroupEKF.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// --- 1) Closed‐loop dynamics f(X): xi = –k·[φx,φy,0], H = ∂xi/∂φ·Dφ ---
|
||||
static constexpr double k = 0.5;
|
||||
Vector3 dynamicsSO3(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)
|
||||
}
|
||||
|
||||
// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
|
||||
static const Vector3 m_world(0, 0, -1);
|
||||
Vector3 h_mag(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
|
||||
Vector3 z = X.inverse().rotate(m_world);
|
||||
if (H) *H = -skewSymmetric(z);
|
||||
return z;
|
||||
}
|
||||
|
||||
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;
|
||||
LieGroupEKF<Rot3> ekf(R0, P0);
|
||||
|
||||
// Timestep, process noise, measurement noise
|
||||
double dt = 0.1;
|
||||
Matrix3 Q = Matrix3::Identity() * 0.01;
|
||||
Matrix3 Rm = Matrix3::Identity() * 0.05;
|
||||
|
||||
cout << "=== Init ===\nR:\n"
|
||||
<< ekf.state().matrix() << "\nP:\n"
|
||||
<< ekf.covariance() << "\n\n";
|
||||
|
||||
// Predict using state‐dependent f
|
||||
ekf.predict(dynamicsSO3, dt, Q);
|
||||
cout << "--- After predict ---\nR:\n" << ekf.state().matrix() << "\n\n";
|
||||
|
||||
// Magnetometer measurement = body‐frame reading of m_world
|
||||
Vector3 z = h_mag(R0);
|
||||
ekf.update(h_mag, z, Rm);
|
||||
cout << "--- After update ---\nR:\n" << ekf.state().matrix() << "\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,95 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IEKF_NavstateExample.cpp
|
||||
* @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
|
||||
* @date April 25, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/navigation/InvariantEKF.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/**
|
||||
* @brief Left-invariant dynamics for NavState.
|
||||
* @param imu 6×1 vector [a; ω]: linear acceleration and angular velocity.
|
||||
* @return 9×1 tangent: [ω; 0₃; a].
|
||||
*/
|
||||
Vector9 dynamics(const Vector6& imu) {
|
||||
auto a = imu.head<3>();
|
||||
auto w = imu.tail<3>();
|
||||
Vector9 xi;
|
||||
xi << w, Vector3::Zero(), a;
|
||||
return xi;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief GPS measurement model: returns position and its Jacobian.
|
||||
* @param X Current state.
|
||||
* @param H Optional 3×9 Jacobian w.r.t. X.
|
||||
* @return 3×1 position vector.
|
||||
*/
|
||||
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
|
||||
return X.position(H);
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Initial state & covariances
|
||||
NavState X0; // R=I, v=0, t=0
|
||||
Matrix9 P0 = Matrix9::Identity() * 0.1;
|
||||
InvariantEKF<NavState> ekf(X0, P0);
|
||||
|
||||
// Noise & timestep
|
||||
double dt = 1.0;
|
||||
Matrix9 Q = Matrix9::Identity() * 0.01;
|
||||
Matrix3 R = Matrix3::Identity() * 0.5;
|
||||
|
||||
// Two IMU samples [a; ω]
|
||||
Vector6 imu1;
|
||||
imu1 << 0.1, 0, 0, 0, 0.2, 0;
|
||||
Vector6 imu2;
|
||||
imu2 << 0, 0.3, 0, 0.4, 0, 0;
|
||||
|
||||
// Two GPS fixes
|
||||
Vector3 z1;
|
||||
z1 << 0.3, 0, 0;
|
||||
Vector3 z2;
|
||||
z2 << 0.6, 0, 0;
|
||||
|
||||
cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
|
||||
<< "\n\n";
|
||||
|
||||
// --- first predict/update ---
|
||||
ekf.predict(dynamics(imu1), dt, Q);
|
||||
cout << "--- After predict 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z1, R);
|
||||
cout << "--- After update 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
|
||||
// --- second predict/update ---
|
||||
ekf.predict(dynamics(imu2), dt, Q);
|
||||
cout << "--- After predict 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z2, R);
|
||||
cout << "--- After update 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,119 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 IEKF_SE2Example.cpp
|
||||
* @brief A left invariant Extended Kalman Filter example using a Lie group
|
||||
* odometry as the prediction stage on SE(2) and
|
||||
*
|
||||
* This example uses the templated InvariantEKF class to estimate the state of
|
||||
* an object using odometry / GPS measurements The prediction stage of the
|
||||
* InvariantEKF uses a Lie Group element to propagate the stage in a discrete
|
||||
* InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that
|
||||
* is constant over the interval dt. However, if u is not constant over dt,
|
||||
* other approaches are needed to find the value of U. This approach simply
|
||||
* takes a Lie group element U, which can be found in various different ways.
|
||||
*
|
||||
* This data was compared to a left invariant EKF on SE(2) using identical
|
||||
* measurements and noise from the source of the InEKF plugin
|
||||
* https://inekf.readthedocs.io/en/latest/ Based on the paper "An Introduction
|
||||
* to the Invariant Extended Kalman Filter" by Easton R. Potokar, Randal W.
|
||||
* Beard, and Joshua G. Mangelson
|
||||
*
|
||||
* @date Apr 25, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/navigation/InvariantEKF.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// 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.
|
||||
Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
|
||||
return X.translation(H);
|
||||
}
|
||||
|
||||
// Define a InvariantEKF class that uses the Pose2 Lie group as the state and
|
||||
// the Vector2 measurement type.
|
||||
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;
|
||||
|
||||
// Create the filter with the initial state, covariance, and measurement
|
||||
// function.
|
||||
InvariantEKF<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();
|
||||
Matrix2 R = I_2x2 * 0.01;
|
||||
|
||||
// Define odometry movements.
|
||||
// U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.
|
||||
// U2: Move 1 unit in X, 1 unit in Y, 0 radians in theta.
|
||||
Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
|
||||
|
||||
// Create GPS measurements.
|
||||
// z1: Measure robot at (1, 0)
|
||||
// z2: Measure robot at (1, 1)
|
||||
Vector2 z1, z2;
|
||||
z1 << 1.0, 0.0;
|
||||
z2 << 1.0, 1.0;
|
||||
|
||||
// Define a transformation matrix to convert the covariance into (theta, x, y)
|
||||
// form. The paper and data mentioned above uses a (theta, x, y) notation,
|
||||
// whereas GTSAM uses (x, y, theta). The transformation matrix is used to
|
||||
// directly compare results of the covariance matrix.
|
||||
Matrix3 TransformP;
|
||||
TransformP << 0, 0, 1, 1, 0, 0, 0, 1, 0;
|
||||
|
||||
// Propagating/updating the filter
|
||||
// Initial state and covariance
|
||||
cout << "\nInitialization:\n";
|
||||
cout << "X0: " << ekf.state() << endl;
|
||||
cout << "P0: " << TransformP * ekf.covariance() * TransformP.transpose()
|
||||
<< endl;
|
||||
|
||||
// First prediction stage
|
||||
ekf.predict(U1, Q);
|
||||
cout << "\nFirst Prediction:\n";
|
||||
cout << "X: " << ekf.state() << endl;
|
||||
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
|
||||
<< endl;
|
||||
|
||||
// First update stage
|
||||
ekf.update(h_gps, z1, R);
|
||||
cout << "\nFirst Update:\n";
|
||||
cout << "X: " << ekf.state() << endl;
|
||||
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
|
||||
<< endl;
|
||||
|
||||
// Second prediction stage
|
||||
ekf.predict(U2, Q);
|
||||
cout << "\nSecond Prediction:\n";
|
||||
cout << "X: " << ekf.state() << endl;
|
||||
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
|
||||
<< endl;
|
||||
|
||||
// Second update stage
|
||||
ekf.update(h_gps, z2, R);
|
||||
cout << "\nSecond Update:\n";
|
||||
cout << "X: " << ekf.state() << endl;
|
||||
cout << "P: " << TransformP * ekf.covariance() * TransformP.transpose()
|
||||
<< endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -27,8 +27,6 @@
|
|||
#include <gtsam/slam/dataset.h>
|
||||
#include <time.h>
|
||||
|
||||
#include <boost/algorithm/string/classification.hpp>
|
||||
#include <boost/algorithm/string/split.hpp>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
|
|
|||
|
|
@ -277,6 +277,10 @@ inline Class expmap_default(const Class& t, const Vector& d) {
|
|||
template<typename T>
|
||||
class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
|
||||
public:
|
||||
// Concept marker: allows checking IsLieGroup<T>::value in templates
|
||||
static constexpr bool value =
|
||||
std::is_base_of<lie_group_tag, typename traits<T>::structure_category>::value;
|
||||
|
||||
typedef typename traits<T>::structure_category structure_category_tag;
|
||||
typedef typename traits<T>::ManifoldType ManifoldType;
|
||||
typedef typename traits<T>::TangentVector TangentVector;
|
||||
|
|
@ -284,7 +288,7 @@ public:
|
|||
|
||||
GTSAM_CONCEPT_USAGE(IsLieGroup) {
|
||||
static_assert(
|
||||
(std::is_base_of<lie_group_tag, structure_category_tag>::value),
|
||||
value,
|
||||
"This type's trait does not assert it is a Lie group (or derived)");
|
||||
|
||||
// group operations with Jacobians
|
||||
|
|
|
|||
|
|
@ -138,10 +138,13 @@ public:
|
|||
static const int dim = traits<T>::dimension;
|
||||
typedef typename traits<T>::ManifoldType ManifoldType;
|
||||
typedef typename traits<T>::TangentVector TangentVector;
|
||||
// Concept marker: allows checking IsManifold<T>::value in templates
|
||||
static constexpr bool value =
|
||||
std::is_base_of<manifold_tag, structure_category_tag>::value;
|
||||
|
||||
GTSAM_CONCEPT_USAGE(IsManifold) {
|
||||
static_assert(
|
||||
(std::is_base_of<manifold_tag, structure_category_tag>::value),
|
||||
value,
|
||||
"This type's structure_category trait does not assert it as a manifold (or derived)");
|
||||
static_assert(TangentVector::SizeAtCompileTime == dim);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,89 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 InvariantEKF.h
|
||||
* @brief Left-Invariant Extended Kalman Filter implementation.
|
||||
*
|
||||
* This file defines the InvariantEKF class template, inheriting from LieGroupEKF,
|
||||
* 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 LieGroupEKF.
|
||||
*
|
||||
* @date April 24, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/LieGroupEKF.h> // Include the base class
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @class InvariantEKF
|
||||
* @brief Left-Invariant Extended Kalman Filter on a Lie group G.
|
||||
*
|
||||
* @tparam G Lie group type (must satisfy LieGroup concept).
|
||||
*
|
||||
* This filter inherits from LieGroupEKF 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 LieGroupEKF are hidden.
|
||||
* The update step remains the same as in ManifoldEKF/LieGroupEKF.
|
||||
*/
|
||||
template <typename G>
|
||||
class InvariantEKF : public LieGroupEKF<G> {
|
||||
public:
|
||||
using Base = LieGroupEKF<G>; ///< Base class type
|
||||
using TangentVector = typename Base::TangentVector; ///< Tangent vector type
|
||||
using MatrixN = typename Base::MatrixN; ///< Square matrix type for covariance etc.
|
||||
using Jacobian = typename Base::Jacobian; ///< Jacobian matrix type specific to the group G
|
||||
|
||||
/// Constructor: forwards to LieGroupEKF constructor
|
||||
InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {}
|
||||
|
||||
/**
|
||||
* 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) {
|
||||
this->X_ = this->X_.compose(U);
|
||||
// TODO(dellaert): traits<G>::AdjointMap should exist
|
||||
Jacobian A = traits<G>::Inverse(U).AdjointMap();
|
||||
this->P_ = A * this->P_ * A.transpose() + Q;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
}
|
||||
|
||||
}; // InvariantEKF
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,165 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 LieGroupEKF.h
|
||||
* @brief Extended Kalman Filter derived class for Lie groups G.
|
||||
*
|
||||
* This file defines the LieGroupEKF class template, inheriting from ManifoldEKF,
|
||||
* for performing EKF steps specifically on states residing in a Lie group.
|
||||
* It provides predict methods with state-dependent dynamics functions.
|
||||
* Please use the InvariantEKF class for prediction via group composition.
|
||||
*
|
||||
* @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 LieGroupEKF
|
||||
* @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 predict methods
|
||||
* with state-dependent dynamics functions.
|
||||
* Use the InvariantEKF class for prediction via group composition.
|
||||
*/
|
||||
template <typename G>
|
||||
class LieGroupEKF : 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
|
||||
LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {
|
||||
static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
this->X_ = predictMean(std::forward<Dynamics>(f), dt, A);
|
||||
this->P_ = A * this->P_ * A.transpose() + Q;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
}
|
||||
|
||||
}; // LieGroupEKF
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -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
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
* 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 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/Pose2.h>
|
||||
#include <gtsam/navigation/InvariantEKF.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// 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_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;
|
||||
|
||||
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
|
||||
Matrix2 R = I_2x2 * 0.01;
|
||||
|
||||
Pose2 U1(1.0, 1.0, 0.5), U2(1.0, 1.0, 0.0);
|
||||
|
||||
Vector2 z1, z2;
|
||||
z1 << 1.0, 0.0;
|
||||
z2 << 1.0, 1.0;
|
||||
|
||||
// Create the filter
|
||||
InvariantEKF<Pose2> ekf(X0, P0);
|
||||
EXPECT(assert_equal(X0, ekf.state()));
|
||||
EXPECT(assert_equal(P0, ekf.covariance()));
|
||||
|
||||
// --- First Prediction ---
|
||||
ekf.predict(U1, Q);
|
||||
|
||||
// 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;
|
||||
|
||||
// Verify
|
||||
EXPECT(assert_equal(X1_expected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(P1_expected, ekf.covariance(), 1e-9));
|
||||
|
||||
|
||||
// --- 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);
|
||||
}
|
||||
|
|
@ -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 LieGroupEKF, 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/LieGroupEKF.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
// Duplicate the dynamics function in GEKF_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;
|
||||
LieGroupEKF<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 {
|
||||
LieGroupEKF<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;
|
||||
LieGroupEKF<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 {
|
||||
LieGroupEKF<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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue