Merge pull request #2129 from borglab/feature/dynamicSizes
Allow for dynamic M/G in new EKF variantsrelease/4.3a0
Frank Dellaert
GitHub
commit
b792270960
|
|
@ -169,32 +169,34 @@ namespace internal {
|
|||
/// To use this for your gtsam type, define:
|
||||
/// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
|
||||
/// Assumes existence of: identity, dimension, localCoordinates, retract,
|
||||
/// and additionally Logmap, Expmap, compose, between, and inverse
|
||||
/// and additionally Logmap, Expmap, AdjointMap, compose, between, and inverse
|
||||
template<class Class>
|
||||
struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
|
||||
struct LieGroupTraits : public GetDimensionImpl<Class, Class::dimension> {
|
||||
using structure_category = lie_group_tag;
|
||||
|
||||
/// @name Group
|
||||
/// @{
|
||||
using group_flavor = multiplicative_group_tag;
|
||||
static Class Identity() { return Class::Identity();}
|
||||
static Class Identity() { return Class::Identity(); }
|
||||
/// @}
|
||||
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
using ManifoldType = Class;
|
||||
// Note: Class::dimension can be an int or Eigen::Dynamic.
|
||||
// GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj).
|
||||
inline constexpr static auto dimension = Class::dimension;
|
||||
using TangentVector = Eigen::Matrix<double, dimension, 1>;
|
||||
using ChartJacobian = OptionalJacobian<dimension, dimension>;
|
||||
|
||||
static TangentVector Local(const Class& origin, const Class& other,
|
||||
ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {
|
||||
return origin.localCoordinates(other, Horigin, Hother);
|
||||
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
|
||||
return origin.localCoordinates(other, H1, H2);
|
||||
}
|
||||
|
||||
static Class Retract(const Class& origin, const TangentVector& v,
|
||||
ChartJacobian Horigin = {}, ChartJacobian Hv = {}) {
|
||||
return origin.retract(v, Horigin, Hv);
|
||||
ChartJacobian H = {}, ChartJacobian Hv = {}) {
|
||||
return origin.retract(v, H, Hv);
|
||||
}
|
||||
/// @}
|
||||
|
||||
|
|
@ -222,6 +224,13 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
|
|||
ChartJacobian H = {}) {
|
||||
return m.inverse(H);
|
||||
}
|
||||
|
||||
static Eigen::Matrix<double, dimension, dimension> AdjointMap(const Class& m) {
|
||||
// This assumes that the Class itself provides a member function `AdjointMap()`
|
||||
// For dynamically-sized types (dimension == Eigen::Dynamic),
|
||||
// m.AdjointMap() must return a gtsam::Matrix of the correct runtime dimensions.
|
||||
return m.AdjointMap();
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@ struct VectorSpaceImpl {
|
|||
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
|
||||
if (H1) *H1 = - Jacobian::Identity();
|
||||
if (H2) *H2 = Jacobian::Identity();
|
||||
Class v = other-origin;
|
||||
Class v = other - origin;
|
||||
return v.vector();
|
||||
}
|
||||
|
||||
|
|
@ -82,12 +82,12 @@ struct VectorSpaceImpl {
|
|||
return -v;
|
||||
}
|
||||
|
||||
static LieAlgebra Hat(const TangentVector& v) {
|
||||
return v;
|
||||
}
|
||||
static LieAlgebra Hat(const TangentVector& v) { return v; }
|
||||
|
||||
static TangentVector Vee(const LieAlgebra& X) {
|
||||
return X;
|
||||
static TangentVector Vee(const LieAlgebra& X) { return X; }
|
||||
|
||||
static Jacobian AdjointMap(const Class& /*m*/) {
|
||||
return Jacobian::Identity();
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
|
|
@ -118,7 +118,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
|
|||
ChartJacobian H1 = {}, ChartJacobian H2 = {}) {
|
||||
if (H1) *H1 = - Eye(origin);
|
||||
if (H2) *H2 = Eye(other);
|
||||
Class v = other-origin;
|
||||
Class v = other - origin;
|
||||
return v.vector();
|
||||
}
|
||||
|
||||
|
|
@ -141,8 +141,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
|
|||
|
||||
static Class Expmap(const TangentVector& v, ChartJacobian Hv = {}) {
|
||||
Class result(v);
|
||||
if (Hv)
|
||||
*Hv = Eye(v);
|
||||
if (Hv) *Hv = Eye(v);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -165,6 +164,7 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
|
|||
return -v;
|
||||
}
|
||||
|
||||
static Eigen::MatrixXd AdjointMap(const Class& m) { return Eye(m); }
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
|
@ -273,8 +273,8 @@ struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
|
|||
if (H) (*H)[0] = 1.0;
|
||||
return v[0];
|
||||
}
|
||||
// AdjointMap for ScalarTraits is inherited from VectorSpaceImpl<Scalar, 1>
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // namespace internal
|
||||
|
|
@ -352,6 +352,9 @@ struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
|
|||
if (H) *H = Jacobian::Identity();
|
||||
return m + Eigen::Map<const Fixed>(v.data());
|
||||
}
|
||||
|
||||
// AdjointMap for fixed-size Eigen matrices is inherited from
|
||||
// internal::VectorSpaceImpl< Eigen::Matrix<double, M, N, ...> , M*N >
|
||||
/// @}
|
||||
};
|
||||
|
||||
|
|
@ -425,7 +428,7 @@ struct DynamicTraits {
|
|||
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
|
||||
if (H) *H = Eye(m);
|
||||
TangentVector result(GetDimension(m));
|
||||
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
|
||||
Eigen::Map<Dynamic>(result.data(), m.rows(), m.cols()) = m;
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -461,6 +464,7 @@ struct DynamicTraits {
|
|||
return X;
|
||||
}
|
||||
|
||||
static Jacobian AdjointMap(const Dynamic& m) { return Eye(m); }
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
|
@ -506,4 +510,3 @@ private:
|
|||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/navigation/LieGroupEKF.h> // Include the base class
|
||||
#include <gtsam/base/Lie.h> // For traits (needed for AdjointMap, Expmap)
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -36,37 +38,50 @@ namespace gtsam {
|
|||
*
|
||||
* 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)`
|
||||
* 1. Prediction via group composition: `predict(const G& U, const Covariance& Q)`
|
||||
* 2. Prediction via tangent control vector: `predict(const TangentVector& u, double dt, const Covariance& Q)`
|
||||
*
|
||||
* The state-dependent prediction methods from LieGroupEKF are hidden.
|
||||
* The update step remains the same as in ManifoldEKF/LieGroupEKF.
|
||||
* For details on how static and dynamic dimensions are handled, please refer to
|
||||
* the `ManifoldEKF` class documentation.
|
||||
*/
|
||||
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
|
||||
/// Jacobian for group-specific operations like AdjointMap. Eigen::Matrix<double, Dim, Dim>.
|
||||
using Jacobian = typename Base::Jacobian;
|
||||
/// Covariance matrix type. Eigen::Matrix<double, Dim, Dim>.
|
||||
using Covariance = typename Base::Covariance;
|
||||
|
||||
/// Constructor: forwards to LieGroupEKF constructor
|
||||
InvariantEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {}
|
||||
|
||||
/**
|
||||
* Constructor: forwards to LieGroupEKF constructor.
|
||||
* @param X0 Initial state on Lie group G.
|
||||
* @param P0 Initial covariance in the tangent space at X0.
|
||||
*/
|
||||
InvariantEKF(const G& X0, const Covariance& P0) : Base(X0, P0) {}
|
||||
|
||||
// We hide state-dependent predict methods from LieGroupEKF by only providing the
|
||||
// invariant predict methods below.
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* where Ad_{U^{-1}} is the Adjoint map of U^{-1}.
|
||||
*
|
||||
* @param U Lie group element representing the motion increment.
|
||||
* @param Q Process noise covariance in the tangent space (size nxn).
|
||||
* @param Q Process noise covariance.
|
||||
*/
|
||||
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();
|
||||
void predict(const G& U, const Covariance& Q) {
|
||||
this->X_ = traits<G>::Compose(this->X_, U);
|
||||
const G U_inv = traits<G>::Inverse(U);
|
||||
const Jacobian A = traits<G>::AdjointMap(U_inv);
|
||||
// P_ is Covariance. A is Jacobian. Q is Covariance.
|
||||
// All are Eigen::Matrix<double,Dim,Dim>.
|
||||
this->P_ = A * this->P_ * A.transpose() + Q;
|
||||
}
|
||||
|
||||
|
|
@ -77,10 +92,19 @@ namespace gtsam {
|
|||
*
|
||||
* @param u Tangent space control vector.
|
||||
* @param dt Time interval.
|
||||
* @param Q Process noise covariance matrix (size nxn).
|
||||
* @param Q Process noise covariance matrix.
|
||||
*/
|
||||
void predict(const TangentVector& u, double dt, const Matrix& Q) {
|
||||
G U = traits<G>::Expmap(u * dt);
|
||||
void predict(const TangentVector& u, double dt, const Covariance& Q) {
|
||||
G U;
|
||||
if constexpr (std::is_same_v<G, Matrix>) {
|
||||
// Specialize to Matrix case as its Expmap is not defined.
|
||||
const Matrix& X = static_cast<const Matrix&>(this->X_);
|
||||
U.resize(X.rows(), X.cols());
|
||||
Eigen::Map<Vector>(static_cast<Matrix&>(U).data(), U.size()) = u * dt;
|
||||
}
|
||||
else {
|
||||
U = traits<G>::Expmap(u * dt);
|
||||
}
|
||||
predict(U, Q); // Call the group composition predict
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,91 +37,111 @@ 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`).
|
||||
* @tparam G Lie group type (must satisfy LieGroup concept).
|
||||
*
|
||||
* This filter specializes ManifoldEKF for Lie groups, offering predict methods
|
||||
* with state-dependent dynamics functions.
|
||||
* Use the InvariantEKF class for prediction via group composition.
|
||||
* For details on how static and dynamic dimensions are handled, please refer to
|
||||
* the `ManifoldEKF` class documentation.
|
||||
*/
|
||||
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
|
||||
static constexpr int Dim = Base::Dim; ///< Compile-time dimension of G.
|
||||
using TangentVector = typename Base::TangentVector; ///< Tangent vector type for G.
|
||||
/// Jacobian for group operations (Adjoint, Expmap derivatives, F).
|
||||
using Jacobian = typename Base::Jacobian; // Eigen::Matrix<double, Dim, Dim>
|
||||
using Covariance = typename Base::Covariance; // Eigen::Matrix<double, Dim, Dim>
|
||||
|
||||
/// Constructor: initialize with state and covariance
|
||||
LieGroupEKF(const G& X0, const MatrixN& P0) : Base(X0, P0) {
|
||||
/**
|
||||
* Constructor: initialize with state and covariance.
|
||||
* @param X0 Initial state on Lie group G.
|
||||
* @param P0 Initial covariance in the tangent space at X0.
|
||||
*/
|
||||
LieGroupEKF(const G& X0, const Covariance& 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)
|
||||
* Signature: TangentVector f(const G& X, OptionalJacobian<Dim, Dim> 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>&>>;
|
||||
OptionalJacobian<Dim, Dim>&>>;
|
||||
|
||||
/**
|
||||
* 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)
|
||||
* A = Ad_{U^{-1}} + Dexp * Df * dt (Compute full state transition Jacobian)
|
||||
*
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)
|
||||
* @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.
|
||||
* @param A OptionalJacobian 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);
|
||||
G predictMean(Dynamics&& f, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
|
||||
Jacobian Df, Dexp; // Eigen::Matrix<double, Dim, Dim>
|
||||
|
||||
if constexpr (std::is_same_v<G, Matrix>) {
|
||||
// Specialize to Matrix case as its Expmap is not defined.
|
||||
TangentVector xi = f(this->X_, A ? &Df : nullptr);
|
||||
const Matrix nextX = traits<Matrix>::Retract(this->X_, xi * dt, A ? &Dexp : nullptr); // just addition
|
||||
if (A) {
|
||||
// Full state transition Jacobian for left-invariant EKF:
|
||||
const Matrix I_n = Matrix::Identity(this->n_, this->n_);
|
||||
*A = I_n + Dexp * Df * dt; // AdjointMap is always identity for Matrix
|
||||
}
|
||||
return nextX;
|
||||
}
|
||||
else {
|
||||
TangentVector xi = f(this->X_, A ? &Df : nullptr); // xi and Df = d(xi)/d(localX)
|
||||
G U = traits<G>::Expmap(xi * dt, A ? &Dexp : nullptr);
|
||||
if (A) {
|
||||
// State transition Jacobian for left-invariant EKF:
|
||||
*A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
|
||||
}
|
||||
return X_next;
|
||||
return this->X_.compose(U);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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
|
||||
* Uses predictMean to compute X_{k+1} and A, then updates covariance.
|
||||
* X_{k+1}, A = predictMean(f, dt)
|
||||
* P_{k+1} = A P_k A^T + Q
|
||||
*
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<n,n>&)
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian<Dim,Dim>&)
|
||||
* @param f Dynamics functor.
|
||||
* @param dt Time step.
|
||||
* @param Q Process noise covariance (size nxn).
|
||||
* @param Q Process noise covariance.
|
||||
*/
|
||||
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
|
||||
void predict(Dynamics&& f, double dt, const Matrix& Q) {
|
||||
void predict(Dynamics&& f, double dt, const Covariance& Q) {
|
||||
Jacobian A;
|
||||
if constexpr (Dim == Eigen::Dynamic) {
|
||||
A.resize(this->n_, this->n_);
|
||||
}
|
||||
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)
|
||||
* Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian<Dim, Dim> 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>&>
|
||||
std::is_invocable_r_v<TangentVector, Dynamics, const G&, const Control&, OptionalJacobian<Dim, Dim>&>
|
||||
>;
|
||||
|
||||
/**
|
||||
|
|
@ -130,7 +150,7 @@ namespace gtsam {
|
|||
* xi = f(X_k, u, Df)
|
||||
*
|
||||
* @tparam Control Control input type.
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<n,n>&)
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)
|
||||
* @param f Dynamics functor.
|
||||
* @param u Control input.
|
||||
* @param dt Time step.
|
||||
|
|
@ -138,8 +158,8 @@ namespace gtsam {
|
|||
* @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);
|
||||
G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
|
||||
return predictMean([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, A);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -149,15 +169,15 @@ namespace gtsam {
|
|||
* xi = f(X_k, u, Df)
|
||||
*
|
||||
* @tparam Control Control input type.
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<n,n>&)
|
||||
* @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian<Dim,Dim>&)
|
||||
* @param f Dynamics functor.
|
||||
* @param u Control input.
|
||||
* @param dt Time step.
|
||||
* @param Q Process noise covariance (size nxn).
|
||||
* @param Q Process noise covariance.
|
||||
*/
|
||||
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);
|
||||
void predict(Dynamics&& f, const Control& u, double dt, const Covariance& Q) {
|
||||
return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
|
||||
}
|
||||
|
||||
}; // LieGroupEKF
|
||||
|
|
|
|||
|
|
@ -27,9 +27,11 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Manifold.h> // Include for traits
|
||||
#include <gtsam/base/Manifold.h> // Include for traits and IsManifold
|
||||
|
||||
#include <Eigen/Dense>
|
||||
#include <string>
|
||||
#include <stdexcept>
|
||||
#include <type_traits>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -38,42 +40,78 @@ 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.
|
||||
* @tparam M Manifold type (must satisfy Manifold concept).
|
||||
*
|
||||
* 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.
|
||||
* 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.
|
||||
*
|
||||
* **Handling Static and Dynamic Dimensions:**
|
||||
* The filter supports manifolds M with either a compile-time fixed dimension or a
|
||||
* runtime dynamic dimension. This is determined by `gtsam::traits<M>::dimension`.
|
||||
* - If `dimension` is an integer (e.g., 3, 6), it's a fixed-size manifold.
|
||||
* - If `dimension` is `Eigen::Dynamic`, it's a dynamically-sized manifold. In this case,
|
||||
* `gtsam::traits<M>::GetDimension(const M&)` must be available to retrieve the
|
||||
* actual dimension at runtime.
|
||||
* The internal protected member `n_` stores this runtime dimension.
|
||||
* Covariance matrices (e.g., `P_`, method argument `Q`) and Jacobians (e.g., method argument `F`)
|
||||
* are typed using `Covariance` and `Jacobian` typedefs, which are specializations of
|
||||
* `Eigen::Matrix<double, Dim, Dim>`, where `Dim` is `traits<M>::dimension`.
|
||||
* For dynamically-sized manifolds (`Dim == Eigen::Dynamic`), these Eigen types
|
||||
* represent dynamically-sized matrices.
|
||||
*/
|
||||
template <typename M>
|
||||
class ManifoldEKF {
|
||||
public:
|
||||
/// Manifold dimension (tangent space dimension)
|
||||
static constexpr int n = traits<M>::dimension;
|
||||
/// Compile-time dimension of the manifold M.
|
||||
static constexpr int Dim = traits<M>::dimension;
|
||||
|
||||
/// Tangent vector type for the manifold M
|
||||
/// Tangent vector type for the manifold M.
|
||||
using TangentVector = typename traits<M>::TangentVector;
|
||||
/// Covariance matrix type (P, Q).
|
||||
using Covariance = Eigen::Matrix<double, Dim, Dim>;
|
||||
/// State transition Jacobian type (F).
|
||||
using Jacobian = Eigen::Matrix<double, Dim, Dim>;
|
||||
|
||||
/// 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");
|
||||
/**
|
||||
* Constructor: initialize with state and covariance.
|
||||
* @param X0 Initial state on manifold M.
|
||||
* @param P0 Initial covariance in the tangent space at X0
|
||||
*/
|
||||
ManifoldEKF(const M& X0, const Covariance& P0) : X_(X0) {
|
||||
static_assert(IsManifold<M>::value,
|
||||
"Template parameter M must be a GTSAM Manifold.");
|
||||
|
||||
if constexpr (Dim == Eigen::Dynamic) {
|
||||
n_ = traits<M>::GetDimension(X0);
|
||||
// Validate dimensions of initial covariance P0.
|
||||
if (P0.rows() != n_ || P0.cols() != n_) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF: Initial covariance P0 dimensions (" +
|
||||
std::to_string(P0.rows()) + "x" + std::to_string(P0.cols()) +
|
||||
") do not match state's tangent space dimension (" +
|
||||
std::to_string(n_) + ").");
|
||||
}
|
||||
}
|
||||
else {
|
||||
n_ = Dim;
|
||||
}
|
||||
|
||||
virtual ~ManifoldEKF() = default; // Add virtual destructor for base class
|
||||
P_ = P0;
|
||||
}
|
||||
|
||||
/// @return current state estimate
|
||||
virtual ~ManifoldEKF() = default;
|
||||
|
||||
/// @return current state estimate on manifold M.
|
||||
const M& state() const { return X_; }
|
||||
|
||||
/// @return current covariance estimate
|
||||
const MatrixN& covariance() const { return P_; }
|
||||
/// @return current covariance estimate.
|
||||
const Covariance& covariance() const { return P_; }
|
||||
|
||||
/// @return runtime dimension of the manifold.
|
||||
int dimension() const { return n_; }
|
||||
|
||||
/**
|
||||
* Basic predict step: Updates state and covariance given the predicted
|
||||
|
|
@ -83,73 +121,138 @@ namespace gtsam {
|
|||
* 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).
|
||||
* @param X_next The predicted state at time k+1 on manifold M.
|
||||
* @param F The state transition Jacobian.
|
||||
* @param Q Process noise covariance matrix.
|
||||
*/
|
||||
void predict(const M& X_next, const MatrixN& F, const Matrix& Q) {
|
||||
void predict(const M& X_next, const Jacobian& F, const Covariance& Q) {
|
||||
if constexpr (Dim == Eigen::Dynamic) {
|
||||
if (F.rows() != n_ || F.cols() != n_ || Q.rows() != n_ || Q.cols() != n_) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF::predict: Dynamic F/Q dimensions must match state dimension " +
|
||||
std::to_string(n_) + ".");
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
* Measurement update: Corrects the state and covariance using a pre-calculated
|
||||
* predicted measurement and its Jacobian.
|
||||
*
|
||||
* @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)).
|
||||
* @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).
|
||||
* @param prediction Predicted measurement.
|
||||
* @param H Jacobian of the measurement function h w.r.t. local(X), H = dh/dlocal(X).
|
||||
* @param z Observed measurement.
|
||||
* @param R Measurement noise covariance (size m x m).
|
||||
* @param R Measurement noise covariance.
|
||||
*/
|
||||
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;
|
||||
template <typename Measurement>
|
||||
void update(const Measurement& prediction,
|
||||
const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
|
||||
const Measurement& z,
|
||||
const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
|
||||
|
||||
// Predict measurement and get Jacobian H = dh/dlocal(X)
|
||||
Measurement z_pred = h(X_, H);
|
||||
static_assert(IsManifold<Measurement>::value,
|
||||
"Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
|
||||
|
||||
// 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)
|
||||
static constexpr int MeasDim = traits<Measurement>::dimension;
|
||||
|
||||
// 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)
|
||||
int m_runtime;
|
||||
if constexpr (MeasDim == Eigen::Dynamic) {
|
||||
m_runtime = traits<Measurement>::GetDimension(z);
|
||||
if (traits<Measurement>::GetDimension(prediction) != m_runtime) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions.");
|
||||
}
|
||||
if (H.rows() != m_runtime || H.cols() != n_ || R.rows() != m_runtime || R.cols() != m_runtime) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF::update: Jacobian H or Noise R dimensions mismatch for dynamic measurement.");
|
||||
}
|
||||
}
|
||||
else {
|
||||
m_runtime = MeasDim;
|
||||
if constexpr (Dim == Eigen::Dynamic) {
|
||||
if (H.cols() != n_) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF::update: Jacobian H columns must match state dimension " + std::to_string(n_) + ".");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Correction vector in tangent space
|
||||
TangentVector delta_xi = -K * innovation; // delta_xi = - K * y
|
||||
// Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z)
|
||||
typename traits<Measurement>::TangentVector innovation =
|
||||
traits<Measurement>::Local(prediction, z);
|
||||
|
||||
// Update state using retract
|
||||
X_ = traits<M>::Retract(X_, delta_xi); // X <- X.retract(delta_xi)
|
||||
// Innovation covariance: S = H P H^T + R
|
||||
// S will be Eigen::Matrix<double, MeasDim, MeasDim>
|
||||
Eigen::Matrix<double, MeasDim, MeasDim> S = H * P_ * H.transpose() + R;
|
||||
|
||||
// Update covariance using Joseph form:
|
||||
MatrixN I_KH = I_n - K * H;
|
||||
// Kalman Gain: K = P H^T S^-1
|
||||
// K will be Eigen::Matrix<double, Dim, MeasDim>
|
||||
Eigen::Matrix<double, Dim, MeasDim> K = P_ * H.transpose() * S.inverse();
|
||||
|
||||
// Correction vector in tangent space of M: delta_xi = K * innovation
|
||||
const TangentVector delta_xi = K * innovation; // delta_xi is Dim x 1 (or n_ x 1 if dynamic)
|
||||
|
||||
// Update state using retract: X_new = retract(X_old, delta_xi)
|
||||
X_ = traits<M>::Retract(X_, delta_xi);
|
||||
|
||||
// Update covariance using Joseph form for numerical stability
|
||||
Jacobian I_n; // Eigen::Matrix<double, Dim, Dim>
|
||||
if constexpr (Dim == Eigen::Dynamic) {
|
||||
I_n = Jacobian::Identity(n_, n_);
|
||||
}
|
||||
else {
|
||||
I_n = Jacobian::Identity();
|
||||
}
|
||||
|
||||
// I_KH will be Eigen::Matrix<double, Dim, Dim>
|
||||
Jacobian 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_
|
||||
/**
|
||||
* Measurement update: Corrects the state and covariance using a measurement
|
||||
* model function.
|
||||
*
|
||||
* @tparam Measurement Type of the measurement vector.
|
||||
* @tparam MeasurementFunction Functor/lambda providing measurement and its Jacobian.
|
||||
* Signature: `Measurement h(const M& x, Jac& H_jacobian)`
|
||||
* where H = d(h)/d(local(X)).
|
||||
* @param h Measurement model function.
|
||||
* @param z Observed measurement.
|
||||
* @param R Measurement noise covariance.
|
||||
*/
|
||||
template <typename Measurement, typename MeasurementFunction>
|
||||
void update(MeasurementFunction&& h, const Measurement& z,
|
||||
const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
|
||||
static_assert(IsManifold<Measurement>::value,
|
||||
"Template parameter Measurement must be a GTSAM Manifold.");
|
||||
|
||||
private:
|
||||
/// Identity matrix of size n
|
||||
static const MatrixN I_n;
|
||||
static constexpr int MeasDim = traits<Measurement>::dimension;
|
||||
|
||||
int m_runtime;
|
||||
if constexpr (MeasDim == Eigen::Dynamic) {
|
||||
m_runtime = traits<Measurement>::GetDimension(z);
|
||||
}
|
||||
else {
|
||||
m_runtime = MeasDim;
|
||||
}
|
||||
|
||||
// Predict measurement and get Jacobian H = dh/dlocal(X)
|
||||
Matrix H(m_runtime, n_);
|
||||
Measurement prediction = h(X_, H);
|
||||
|
||||
// Call the other update function
|
||||
update(prediction, H, z, R);
|
||||
}
|
||||
|
||||
protected:
|
||||
M X_; ///< Manifold state estimate.
|
||||
Covariance P_; ///< Covariance (Eigen::Matrix<double, Dim, Dim>).
|
||||
int n_; ///< Runtime tangent space dimension of M.
|
||||
};
|
||||
|
||||
// Define static identity I_n
|
||||
template <typename M>
|
||||
const typename ManifoldEKF<M>::MatrixN ManifoldEKF<M>::I_n = ManifoldEKF<M>::MatrixN::Identity();
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -119,6 +119,68 @@ TEST(IEKF_Pose2, PredictUpdateSequence) {
|
|||
|
||||
}
|
||||
|
||||
// Define simple dynamics and measurement for a 2x2 Matrix state
|
||||
namespace exampleDynamicMatrix {
|
||||
Matrix f(const Matrix& p, const Vector& vTangent, double dt) {
|
||||
return traits<Matrix>::Retract(p, vTangent * dt);
|
||||
}
|
||||
double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
|
||||
if (H) {
|
||||
H->resize(1, p.size());
|
||||
(*H) << 1.0, 0.0, 0.0, 1.0; // Assuming 2x2
|
||||
}
|
||||
return p(0, 0) + p(1, 1); // trace !
|
||||
}
|
||||
} // namespace exampleDynamicMatrix
|
||||
|
||||
TEST(InvariantEKF_DynamicMatrix, PredictAndUpdate) {
|
||||
// --- Setup ---
|
||||
Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
|
||||
Matrix p0Covariance = I_4x4 * 0.01;
|
||||
Vector velocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished();
|
||||
double dt = 0.1;
|
||||
Matrix Q = I_4x4 * 0.001;
|
||||
Matrix R = Matrix::Identity(1, 1) * 0.005;
|
||||
|
||||
InvariantEKF<Matrix> ekf(p0Matrix, p0Covariance);
|
||||
EXPECT_LONGS_EQUAL(4, ekf.state().size());
|
||||
EXPECT_LONGS_EQUAL(4, ekf.dimension());
|
||||
|
||||
// --- Predict ---
|
||||
ekf.predict(velocityTangent, dt, Q);
|
||||
|
||||
// Verification for Predict
|
||||
Matrix pPredictedExpected = exampleDynamicMatrix::f(p0Matrix, velocityTangent, dt);
|
||||
Matrix pCovariancePredictedExpected = p0Covariance + Q;
|
||||
EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9));
|
||||
|
||||
// --- Update ---
|
||||
// Use the state after prediction for the update step
|
||||
Matrix pStateBeforeUpdate = ekf.state();
|
||||
Matrix pCovarianceBeforeUpdate = ekf.covariance();
|
||||
|
||||
double zTrue = exampleDynamicMatrix::h(pStateBeforeUpdate);
|
||||
double zObserved = zTrue - 0.03; // Simulated measurement with some error
|
||||
|
||||
ekf.update(exampleDynamicMatrix::h, zObserved, R);
|
||||
|
||||
// Verification for Update (Manual Kalman Steps)
|
||||
Matrix H(1, 4);
|
||||
double zPredictionManual = exampleDynamicMatrix::h(pStateBeforeUpdate, H);
|
||||
double innovationY_tangent = zObserved - zPredictionManual;
|
||||
Matrix S = H * pCovarianceBeforeUpdate * H.transpose() + R;
|
||||
Matrix kalmanGainK = pCovarianceBeforeUpdate * H.transpose() * S.inverse();
|
||||
Vector deltaXiTangent = kalmanGainK * innovationY_tangent;
|
||||
Matrix pUpdatedExpected = traits<Matrix>::Retract(pStateBeforeUpdate, deltaXiTangent);
|
||||
Matrix I_KH = I_4x4 - kalmanGainK * H;
|
||||
Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + kalmanGainK * R * kalmanGainK.transpose();
|
||||
|
||||
EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9));
|
||||
}
|
||||
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
|
|
|
|||
|
|
@ -108,6 +108,92 @@ TEST(GroupeEKF, StateAndControl) {
|
|||
EXPECT(assert_equal(expectedH, actualH));
|
||||
}
|
||||
|
||||
// Namespace for dynamic Matrix LieGroupEKF test
|
||||
namespace exampleLieGroupDynamicMatrix {
|
||||
// Constant tangent vector for dynamics (same as "velocityTangent" in IEKF test)
|
||||
const Vector kFixedVelocityTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished();
|
||||
|
||||
// Dynamics function: xi = f(X, H_X)
|
||||
// Returns a constant tangent vector, so Df_DX = 0.
|
||||
// H_X is D(xi)/D(X_local), where X_local is the tangent space perturbation of X.
|
||||
Vector f(const Matrix& X, OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> H_X = {}) {
|
||||
if (H_X) {
|
||||
size_t state_dim = X.size();
|
||||
size_t tangent_dim = kFixedVelocityTangent.size();
|
||||
// Ensure Jacobian dimensions are consistent even if state or tangent is 0-dim
|
||||
H_X->setZero(tangent_dim, state_dim);
|
||||
}
|
||||
return kFixedVelocityTangent;
|
||||
}
|
||||
|
||||
// Measurement function h(X, H)
|
||||
double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
|
||||
// Specialized for a 2x2 matrix!
|
||||
if (p.rows() != 2 || p.cols() != 2) {
|
||||
throw std::invalid_argument("Matrix must be 2x2.");
|
||||
}
|
||||
if (H) {
|
||||
H->resize(1, p.size());
|
||||
*H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
|
||||
}
|
||||
return p(0, 0) + p(1, 1); // Trace of the matrix
|
||||
}
|
||||
} // namespace exampleLieGroupDynamicMatrix
|
||||
|
||||
TEST(LieGroupEKF_DynamicMatrix, PredictAndUpdate) {
|
||||
// --- Setup ---
|
||||
Matrix p0Matrix = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
|
||||
Matrix p0Covariance = I_4x4 * 0.01;
|
||||
double dt = 0.1;
|
||||
Matrix process_noise_Q = I_4x4 * 0.001;
|
||||
Matrix measurement_noise_R = Matrix::Identity(1, 1) * 0.005;
|
||||
|
||||
LieGroupEKF<Matrix> ekf(p0Matrix, p0Covariance);
|
||||
EXPECT_LONGS_EQUAL(4, ekf.state().size());
|
||||
EXPECT_LONGS_EQUAL(4, ekf.dimension());
|
||||
|
||||
// --- Predict ---
|
||||
// ekf.predict takes f(X, H_X), dt, process_noise_Q
|
||||
ekf.predict(exampleLieGroupDynamicMatrix::f, dt, process_noise_Q);
|
||||
|
||||
// Verification for Predict
|
||||
// For f, Df_DXk = 0 (Jacobian of xi w.r.t X_local is Zero).
|
||||
// State transition Jacobian A = Ad_Uinv + Dexp * Df_DXk * dt.
|
||||
// For Matrix (VectorSpace): Ad_Uinv = I, Dexp = I.
|
||||
// So, A = I + I * 0 * dt = I.
|
||||
// Covariance update: P_next = A * P_current * A.transpose() + Q = I * P_current * I + Q = P_current + Q.
|
||||
Matrix pPredictedExpected = traits<Matrix>::Retract(p0Matrix, exampleLieGroupDynamicMatrix::kFixedVelocityTangent * dt);
|
||||
Matrix pCovariancePredictedExpected = p0Covariance + process_noise_Q;
|
||||
|
||||
EXPECT(assert_equal(pPredictedExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pCovariancePredictedExpected, ekf.covariance(), 1e-9));
|
||||
|
||||
// --- Update ---
|
||||
Matrix pStateBeforeUpdate = ekf.state();
|
||||
Matrix pCovarianceBeforeUpdate = ekf.covariance();
|
||||
|
||||
double zTrue = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate);
|
||||
double zObserved = zTrue - 0.03; // Simulated measurement with some error
|
||||
|
||||
ekf.update(exampleLieGroupDynamicMatrix::h, zObserved, measurement_noise_R);
|
||||
|
||||
// Verification for Update (Manual Kalman Steps)
|
||||
Matrix H_update(1, 4); // Measurement Jacobian: 1x4 for 2x2 matrix, trace measurement
|
||||
double zPredictionManual = exampleLieGroupDynamicMatrix::h(pStateBeforeUpdate, H_update);
|
||||
// Innovation: y_tangent = traits<Measurement>::Local(prediction, observation)
|
||||
// For double (scalar), Local(A,B) is B-A.
|
||||
double innovationY_tangent = zObserved - zPredictionManual;
|
||||
Matrix S_innovation_cov = H_update * pCovarianceBeforeUpdate * H_update.transpose() + measurement_noise_R;
|
||||
Matrix K_gain = pCovarianceBeforeUpdate * H_update.transpose() * S_innovation_cov.inverse();
|
||||
Vector deltaXiTangent = K_gain * innovationY_tangent; // Tangent space correction for Matrix state
|
||||
Matrix pUpdatedExpected = traits<Matrix>::Retract(pStateBeforeUpdate, deltaXiTangent);
|
||||
Matrix I_KH = I_4x4 - K_gain * H_update; // I_4x4 because state dimension is 4
|
||||
Matrix pUpdatedCovarianceExpected = I_KH * pCovarianceBeforeUpdate * I_KH.transpose() + K_gain * measurement_noise_R * K_gain.transpose();
|
||||
|
||||
EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9));
|
||||
}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
|
|
|
|||
|
|
@ -30,20 +30,20 @@ using namespace gtsam;
|
|||
namespace exampleUnit3 {
|
||||
|
||||
// Predicts the next state given current state, tangent velocity, and dt
|
||||
Unit3 predictNextState(const Unit3& p, const Vector2& v, double dt) {
|
||||
Unit3 f(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) {
|
||||
double 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);
|
||||
auto h = [](const Unit3& p_) { return p_.point3().z(); };
|
||||
*H = numericalDerivative11<double, Unit3, 2>(h, p);
|
||||
}
|
||||
return Vector1(p.point3().z());
|
||||
return p.point3().z();
|
||||
}
|
||||
|
||||
} // namespace exampleUnit3
|
||||
|
|
@ -76,13 +76,13 @@ TEST(ManifoldEKF_Unit3, Predict) {
|
|||
|
||||
// --- Prepare inputs for ManifoldEKF::predict ---
|
||||
// 1. Compute expected next state
|
||||
Unit3 p_next_expected = exampleUnit3::predictNextState(data.p0, data.velocity, data.dt);
|
||||
Unit3 p_next_expected = exampleUnit3::f(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.
|
||||
// We can compute this numerically using the f function.
|
||||
// GTSAM's numericalDerivative handles derivatives *between* manifolds.
|
||||
auto predict_wrapper = [&](const Unit3& p) -> Unit3 {
|
||||
return exampleUnit3::predictNextState(p, data.velocity, data.dt);
|
||||
return exampleUnit3::f(p, data.velocity, data.dt);
|
||||
};
|
||||
Matrix2 F = numericalDerivative11<Unit3, Unit3>(predict_wrapper, data.p0);
|
||||
|
||||
|
|
@ -100,7 +100,7 @@ TEST(ManifoldEKF_Unit3, Predict) {
|
|||
// 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);
|
||||
return exampleUnit3::f(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));
|
||||
|
|
@ -116,8 +116,8 @@ TEST(ManifoldEKF_Unit3, Update) {
|
|||
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
|
||||
double z_true = exampleUnit3::measureZ(p_start, {});
|
||||
double z_observed = z_true + 0.02; // Add some noise
|
||||
|
||||
// --- Perform EKF update ---
|
||||
ekf.update(exampleUnit3::measureZ, z_observed, data.R);
|
||||
|
|
@ -125,10 +125,10 @@ TEST(ManifoldEKF_Unit3, Update) {
|
|||
// --- 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);
|
||||
double z_pred = exampleUnit3::measureZ(p_start, H);
|
||||
|
||||
// 2. Innovation and Covariance
|
||||
Vector1 y = z_pred - z_observed; // Innovation (using vector subtraction for z)
|
||||
double 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
|
||||
|
|
@ -147,6 +147,87 @@ TEST(ManifoldEKF_Unit3, Update) {
|
|||
EXPECT(assert_equal(P_updated_expected, ekf.covariance(), 1e-8));
|
||||
}
|
||||
|
||||
// Define simple dynamics and measurement for a 2x2 Matrix state
|
||||
namespace exampleDynamicMatrix {
|
||||
|
||||
// Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt.
|
||||
Matrix f(const Matrix& p, const Vector& vTangent, double dt) {
|
||||
return traits<Matrix>::Retract(p, vTangent * dt); // +
|
||||
}
|
||||
|
||||
// Define a measurement model: measure the trace of the Matrix (assumed 2x2 here)
|
||||
double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
|
||||
// Specialized for a 2x2 matrix!
|
||||
if (p.rows() != 2 || p.cols() != 2) {
|
||||
throw std::invalid_argument("Matrix must be 2x2.");
|
||||
}
|
||||
if (H) {
|
||||
H->resize(1, p.size());
|
||||
*H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
|
||||
}
|
||||
return p(0, 0) + p(1, 1); // Trace of the matrix
|
||||
}
|
||||
|
||||
} // namespace exampleDynamicMatrix
|
||||
|
||||
TEST(ManifoldEKF_DynamicMatrix, CombinedPredictAndUpdate) {
|
||||
Matrix pInitial = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
|
||||
Matrix pInitialCovariance = I_4x4 * 0.01; // Covariance for 2x2 matrix (4x4)
|
||||
Vector vTangent = (Vector(4) << 0.5, 0.1, -0.1, -0.5).finished(); // [dp00, dp10, dp01, dp11]/sec
|
||||
double deltaTime = 0.1;
|
||||
Matrix processNoiseCovariance = I_4x4 * 0.001; // Process noise covariance (4x4)
|
||||
Matrix measurementNoiseCovariance = Matrix::Identity(1, 1) * 0.005; // Measurement noise covariance (1x1)
|
||||
|
||||
ManifoldEKF<Matrix> ekf(pInitial, pInitialCovariance);
|
||||
// For a 2x2 Matrix, tangent space dimension is 2*2=4.
|
||||
EXPECT_LONGS_EQUAL(4, ekf.state().size());
|
||||
EXPECT_LONGS_EQUAL(pInitial.rows() * pInitial.cols(), ekf.state().size());
|
||||
|
||||
// Predict Step
|
||||
Matrix pPredictedMean = exampleDynamicMatrix::f(pInitial, vTangent, deltaTime);
|
||||
|
||||
// For this linear prediction model (pNext = pCurrent + V*dt in tangent space),
|
||||
// Derivative w.r.t deltaXi is Identity.
|
||||
Matrix fJacobian = I_4x4;
|
||||
|
||||
ekf.predict(pPredictedMean, fJacobian, processNoiseCovariance);
|
||||
|
||||
EXPECT(assert_equal(pPredictedMean, ekf.state(), 1e-9));
|
||||
Matrix pPredictedCovarianceExpected = fJacobian * pInitialCovariance * fJacobian.transpose() + processNoiseCovariance;
|
||||
EXPECT(assert_equal(pPredictedCovarianceExpected, ekf.covariance(), 1e-9));
|
||||
|
||||
// Update Step
|
||||
Matrix pCurrentForUpdate = ekf.state();
|
||||
Matrix pCurrentCovarianceForUpdate = ekf.covariance();
|
||||
|
||||
// True trace of pCurrentForUpdate (which is pPredictedMean)
|
||||
double zTrue = exampleDynamicMatrix::h(pCurrentForUpdate);
|
||||
EXPECT_DOUBLES_EQUAL(5.0, zTrue, 1e-9);
|
||||
double zObserved = zTrue - 0.03;
|
||||
|
||||
ekf.update(exampleDynamicMatrix::h, zObserved, measurementNoiseCovariance);
|
||||
|
||||
// Manual Kalman Update Steps for Verification
|
||||
Matrix hJacobian(1, 4); // Measurement Jacobian H (1x4 for 2x2 matrix, trace measurement)
|
||||
double zPredictionManual = exampleDynamicMatrix::h(pCurrentForUpdate, hJacobian);
|
||||
Matrix hJacobianExpected = (Matrix(1, 4) << 1.0, 0.0, 0.0, 1.0).finished();
|
||||
EXPECT(assert_equal(hJacobianExpected, hJacobian, 1e-9));
|
||||
|
||||
// Innovation: y = zObserved - zPredictionManual (since measurement is double)
|
||||
double yInnovation = zObserved - zPredictionManual;
|
||||
Matrix innovationCovariance = hJacobian * pCurrentCovarianceForUpdate * hJacobian.transpose() + measurementNoiseCovariance;
|
||||
|
||||
Matrix kalmanGain = pCurrentCovarianceForUpdate * hJacobian.transpose() * innovationCovariance.inverse(); // K is 4x1
|
||||
|
||||
// State Correction (in tangent space of Matrix)
|
||||
Vector deltaXiTangent = kalmanGain * yInnovation; // deltaXi is 4x1 Vector
|
||||
|
||||
Matrix pUpdatedManualExpected = traits<Matrix>::Retract(pCurrentForUpdate, deltaXiTangent);
|
||||
Matrix pUpdatedCovarianceManualExpected = (I_4x4 - kalmanGain * hJacobian) * pCurrentCovarianceForUpdate;
|
||||
|
||||
EXPECT(assert_equal(pUpdatedManualExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pUpdatedCovarianceManualExpected, ekf.covariance(), 1e-9));
|
||||
}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue