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

Retain efficiency in static case

release/4.3a0
Frank Dellaert 2025-05-09 00:48:07 -04:00
parent eacdf1c7fa
commit 16f650c16c
4 changed files with 167 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/InvariantEKF.h
View File

@ -38,30 +38,32 @@ 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.
* Covariances (P, Q) are `Matrix`.
* Covariances (P, Q) are `Eigen::Matrix<double, Dim, Dim>`.
*/
template <typename G>
class InvariantEKF : public LieGroupEKF<G> {
public:
using Base = LieGroupEKF<G>; ///< Base class type
using TangentVector = typename Base::TangentVector; ///< Tangent vector type
// Jacobian for group-specific operations like AdjointMap.
// Becomes Matrix if G has dynamic dimension.
/// 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.
* @param X0 Initial state on Lie group G.
* @param P0 Initial covariance in the tangent space at X0 (must be Matrix).
* @param P0_input Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix).
*/
InvariantEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {}
InvariantEKF(const G& X0, const Matrix& P0_input) : Base(X0, P0_input) {}
// We hide state-dependent predict methods from LieGroupEKF by only providing the
// We hide state-dependent predict methods from LieGroupEKF by only providing the
// invariant predict methods below.
/**
@ -71,13 +73,14 @@ namespace gtsam {
* 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 (must be Matrix, size n_ x n_).
* @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
*/
void predict(const G& U, const Matrix& Q) {
void predict(const G& U, const Covariance& Q) {
this->X_ = this->X_.compose(U);
// TODO(dellaert): traits<G>::AdjointMap should exist
const Jacobian A = traits<G>::Inverse(U).AdjointMap();
// P_ is Matrix. A is Eigen::Matrix<double,n,n>. Q is Matrix.
// P_ is Covariance. A is Jacobian. Q is Covariance.
// All are Eigen::Matrix<double,Dim,Dim>.
this->P_ = A * this->P_ * A.transpose() + Q;
}
@ -88,9 +91,9 @@ namespace gtsam {
*
* @param u Tangent space control vector.
* @param dt Time interval.
* @param Q Process noise covariance matrix (Matrix, size n_ x n_).
* @param Q Process noise covariance matrix (Eigen::Matrix<double, Dim, Dim>).
*/
void predict(const TangentVector& u, double dt, const Matrix& Q) {
void predict(const TangentVector& u, double dt, const Covariance& Q) {
const G U = traits<G>::Expmap(u * dt);
predict(U, Q); // Call the group composition predict
}

70
gtsam/navigation/LieGroupEKF.h
View File

@ -48,17 +48,16 @@ namespace gtsam {
class LieGroupEKF : public ManifoldEKF<G> {
public:
using Base = ManifoldEKF<G>; ///< Base class type
// n is the tangent space dimension of G. If G::dimension is Eigen::Dynamic, n is Eigen::Dynamic.
static constexpr int n = traits<G>::dimension;
using TangentVector = typename Base::TangentVector; ///< Tangent vector type for the group G
// Jacobian for group-specific operations like Adjoint, Expmap derivatives.
// Becomes Matrix if n is Eigen::Dynamic.
using Jacobian = Eigen::Matrix<double, n, n>;
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.
* @param X0 Initial state on Lie group G.
* @param P0 Initial covariance in the tangent space at X0 (must be Matrix).
* @param P0 Initial covariance in the tangent space at X0 (dynamic gtsam::Matrix).
*/
LieGroupEKF(const G& X0, const Matrix& P0) : Base(X0, P0) {
static_assert(IsLieGroup<G>::value, "Template parameter G must be a GTSAM Lie Group");
@ -66,38 +65,39 @@ namespace gtsam {
/**
* 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))
* If n is Eigen::Dynamic, OptionalJacobian<n,n> is OptionalJacobian<Dynamic,Dynamic>.
*/
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)
* 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)
* X_{k+1} = X_k * U (Predict next state)
* 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 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;
G predictMean(Dynamics&& f, double dt, OptionalJacobian<Dim, Dim> A = {}) const {
Jacobian Df, Dexp; // Eigen::Matrix<double, Dim, Dim>
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:
// AdjointMap returns Jacobian (Eigen::Matrix<double,Dim,Dim>)
*A = traits<G>::Inverse(U).AdjointMap() + Dexp * Df * dt;
}
return X_next;
@ -106,29 +106,32 @@ namespace gtsam {
/**
* Predict step with state-dependent dynamics:
* Uses predictMean to compute X_{k+1} and F, then updates covariance.
* 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 (Matrix, size n_ x n_).
* @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
*/
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
void predict(Dynamics&& f, double dt, const Matrix& Q) {
Jacobian A;
void predict(Dynamics&& f, double dt, const Covariance& Q) {
Jacobian A; // Eigen::Matrix<double, Dim, Dim>
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>&>
>;
/**
@ -137,7 +140,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.
@ -145,8 +148,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);
}
@ -156,15 +159,18 @@ 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 (must be Matrix, size n_ x n_).
* @param Q Process noise covariance (Eigen::Matrix<double, Dim, Dim>).
*/
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) {
// Note: The lambda below captures f by reference. Ensure f's lifetime.
// If f is an rvalue, std::forward or std::move might be needed if the lambda is stored.
// Here, the lambda is temporary, so [&] is fine.
return predict([&](const G& X, OptionalJacobian<Dim, Dim> Df) { return f(X, u, Df); }, dt, Q);
}
}; // LieGroupEKF

165
gtsam/navigation/ManifoldEKF.h
View File

@ -19,7 +19,7 @@
* localCoordinates operations.
*
* Works with manifolds M that may have fixed or dynamic tangent space dimensions.
* Covariances and Jacobians are handled as `Matrix` (dynamic-size Eigen matrices).
* Covariances and Jacobians leverage Eigen's static or dynamic matrices for efficiency.
*
* @date April 24, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
@ -51,33 +51,41 @@ namespace gtsam {
* must be provided by traits.
*
* This filter maintains a state X in the manifold M and covariance P in the
* tangent space at X. The covariance P is always stored as a gtsam::Matrix.
* tangent space at X. The covariance P is `Eigen::Matrix<double, Dim, Dim>`,
* where Dim is the compile-time dimension of M (or Eigen::Dynamic).
* 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:
/// Tangent vector type for the manifold M, as defined by its traits.
/// Compile-time dimension of the manifold M.
static constexpr int Dim = traits<M>::dimension;
/// 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>;
/**
* Constructor: initialize with state and covariance.
* @param X0 Initial state on manifold M.
* @param P0 Initial covariance in the tangent space at X0. Must be a square
* Matrix whose dimensions match the tangent space dimension of X0.
* @param P0 Initial covariance in the tangent space at X0.
* Provided as a dynamic gtsam::Matrix for flexibility,
* but will be stored internally as Covariance.
*/
ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) {
ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0) {
static_assert(IsManifold<M>::value,
"Template parameter M must be a GTSAM Manifold.");
// Determine tangent space dimension n_ at runtime.
if constexpr (traits<M>::dimension == Eigen::Dynamic) {
// If M::dimension is dynamic, traits<M>::GetDimension(M) must exist.
n_ = traits<M>::GetDimension(X0);
if constexpr (Dim == Eigen::Dynamic) {
n_ = traits<M>::GetDimension(X0); // Runtime dimension for dynamic case
}
else {
n_ = traits<M>::dimension;
n_ = Dim; // Runtime dimension is fixed compile-time dimension
}
// Validate dimensions of initial covariance P0.
@ -88,6 +96,7 @@ namespace gtsam {
") do not match state's tangent space dimension (" +
std::to_string(n_) + ").");
}
P_ = P0; // Assigns MatrixXd to Eigen::Matrix<double,Dim,Dim>
}
virtual ~ManifoldEKF() = default;
@ -95,8 +104,11 @@ namespace gtsam {
/// @return current state estimate on manifold M.
const M& state() const { return X_; }
/// @return current covariance estimate in the tangent space (always a Matrix).
const Matrix& covariance() const { return P_; }
/// @return current covariance estimate (Eigen::Matrix<double, Dim, Dim>).
const Covariance& covariance() const { return P_; }
/// @return runtime dimension of the tangent space.
int dimension() const { return n_; }
/**
* Basic predict step: Updates state and covariance given the predicted
@ -107,22 +119,19 @@ namespace gtsam {
* state transition in local coordinates around X_k.
*
* @param X_next The predicted state at time k+1 on manifold M.
* @param F The state transition Jacobian (size nxn).
* @param Q Process noise covariance matrix in the tangent space (size nxn).
* @param F The state transition Jacobian (Eigen::Matrix<double, Dim, Dim>).
* @param Q Process noise covariance matrix (Eigen::Matrix<double, Dim, Dim>).
*/
void predict(const M& X_next, const Matrix& F, const Matrix& Q) {
if (F.rows() != n_ || F.cols() != n_) {
throw std::invalid_argument(
"ManifoldEKF::predict: Jacobian F dimensions (" +
std::to_string(F.rows()) + "x" + std::to_string(F.cols()) +
") must be " + std::to_string(n_) + "x" + std::to_string(n_) + ".");
}
if (Q.rows() != n_ || Q.cols() != n_) {
throw std::invalid_argument(
"ManifoldEKF::predict: Noise Q dimensions (" +
std::to_string(Q.rows()) + "x" + std::to_string(Q.cols()) +
") must be " + std::to_string(n_) + "x" + std::to_string(n_) + ".");
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_) + ".");
}
}
// For fixed Dim, types enforce dimensions.
X_ = X_next;
P_ = F * P_ * F.transpose() + Q;
}
@ -134,64 +143,75 @@ namespace gtsam {
* @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).
* Its dimensions must be m x n.
* Type: Eigen::Matrix<double, MeasDim, Dim>.
* @param z Observed measurement.
* @param R Measurement noise covariance (size m x m).
* @param R Measurement noise covariance (Eigen::Matrix<double, MeasDim, MeasDim>).
*/
template <typename Measurement>
void update(const Measurement& prediction,
const Matrix& H,
const Eigen::Matrix<double, traits<Measurement>::dimension, Dim>& H,
const Measurement& z,
const Matrix& R) {
const Eigen::Matrix<double, traits<Measurement>::dimension, traits<Measurement>::dimension>& R) {
static_assert(IsManifold<Measurement>::value,
"Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
int m; // Measurement dimension
if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
m = traits<Measurement>::GetDimension(z);
if (traits<Measurement>::GetDimension(prediction) != m) {
static constexpr int MeasDim = traits<Measurement>::dimension;
int m_runtime; // Measurement dimension at 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.");
}
// Runtime check for H and R if they involve dynamic 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 = traits<Measurement>::dimension;
}
if (H.rows() != m || H.cols() != n_) {
throw std::invalid_argument(
"ManifoldEKF::update: Jacobian H dimensions (" +
std::to_string(H.rows()) + "x" + std::to_string(H.cols()) +
") must be " + std::to_string(m) + "x" + std::to_string(n_) + ".");
}
if (R.rows() != m || R.cols() != m) {
throw std::invalid_argument(
"ManifoldEKF::update: Noise R dimensions (" +
std::to_string(R.rows()) + "x" + std::to_string(R.cols()) +
") must be " + std::to_string(m) + "x" + std::to_string(m) + ".");
m_runtime = MeasDim;
// Types enforce dimensions for H and R if MeasDim and Dim are fixed.
// If Dim is dynamic but MeasDim is fixed, H.cols() needs check.
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_) + ".");
}
}
}
// Innovation: y = z - h(x_pred). In tangent space: local(h(x_pred), z)
// This is `log(prediction.inverse() * z)` if Measurement is a Lie group.
typename traits<Measurement>::TangentVector innovation =
traits<Measurement>::Local(prediction, z);
// Innovation covariance: S = H P H^T + R
const Matrix S = H * P_ * H.transpose() + R; // S is m x m
// S will be Eigen::Matrix<double, MeasDim, MeasDim>
Eigen::Matrix<double, MeasDim, MeasDim> S = H * P_ * H.transpose() + R;
// Kalman Gain: K = P H^T S^-1
const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m
// 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 n_ x 1
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
const auto I_n = Matrix::Identity(n_, n_);
const Matrix I_KH = I_n - K * H; // I_KH is n x n
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();
}
@ -202,37 +222,48 @@ namespace gtsam {
* @tparam Measurement Type of the measurement vector (e.g., VectorN<m>, Vector).
* @tparam MeasurementFunction Functor/lambda with signature compatible with:
* `Measurement h(const M& x, Jac& H_jacobian)`
* where `Jac` can be `Matrix&` or `OptionalJacobian<m, n_>&`.
* where `Jac` can be `Eigen::Matrix<double, MeasDim, Dim>&` or
* `OptionalJacobian<MeasDim, Dim>&`.
* The Jacobian H should be d(h)/d(local(X)).
* @param h Measurement model function.
* @param z Observed measurement.
* @param R Measurement noise covariance (must be an m x m Matrix).
* @param R Measurement noise covariance (Eigen::Matrix<double, MeasDim, MeasDim>).
*/
template <typename Measurement, typename MeasurementFunction>
void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) {
void update(MeasurementFunction&& h_func, 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.");
int m; // Measurement dimension
if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
m = traits<Measurement>::GetDimension(z);
static constexpr int MeasDim = traits<Measurement>::dimension;
int m_runtime; // Measurement dimension at runtime
if constexpr (MeasDim == Eigen::Dynamic) {
m_runtime = traits<Measurement>::GetDimension(z);
}
else {
m = traits<Measurement>::dimension;
m_runtime = MeasDim;
}
// Prepare Jacobian H for the measurement function
Matrix H(m_runtime, n_);
if constexpr (MeasDim == Eigen::Dynamic || Dim == Eigen::Dynamic) {
// If H involves any dynamic dimension, it needs explicit resizing.
H.resize(m_runtime, n_);
}
// If H is fully fixed-size, its dimensions are set at compile time.
// Predict measurement and get Jacobian H = dh/dlocal(X)
Matrix H(m, n_);
Measurement prediction = h(X_, H);
Measurement prediction = h_func(X_, H);
// Call the other update function
update(prediction, H, z, R);
}
protected:
M X_; ///< Manifold state estimate.
Matrix P_; ///< Covariance in tangent space at X_ (always a dynamic Matrix).
int n_; ///< Tangent space dimension of M, determined at construction.
M X_; ///< Manifold state estimate.
Covariance P_; ///< Covariance (Eigen::Matrix<double, Dim, Dim>).
int n_; ///< Runtime tangent space dimension of M.
};
} // namespace gtsam

31
gtsam/navigation/tests/testManifoldEKF.cpp
View File

@ -36,14 +36,14 @@ namespace exampleUnit3 {
// 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
@ -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
@ -156,16 +156,15 @@ namespace exampleDynamicMatrix {
}
// Define a measurement model: measure the trace of the Matrix (assumed 2x2 here)
Vector1 measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
double measureTrace(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
if (H) {
// p_flat (col-major for Eigen) for a 2x2 matrix p = [[p00,p01],[p10,p11]] is [p00, p10, p01, p11]
// trace = p(0,0) + p(1,1)
// H = d(trace)/d(p_flat) = [1, 0, 0, 1]
// The Jacobian H will be 1x4 for a 2x2 matrix.
H->resize(1, 4);
*H << 1.0, 0.0, 0.0, 1.0;
}
return Vector1(p(0, 0) + p(1, 1));
return p(0, 0) + p(1, 1);
}
} // namespace exampleDynamicMatrix
@ -223,8 +222,8 @@ TEST(ManifoldEKF_DynamicMatrix, Update) {
EXPECT_LONGS_EQUAL(4, ekf.state().size());
// Simulate a measurement (true trace of pStartMatrix is 1.5 + 2.5 = 4.0)
Vector1 zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here
Vector1 zObserved = zTrue - Vector1(0.03); // Add some "error"
double zTrue = exampleDynamicMatrix::measureTrace(pStartMatrix); // No Jacobian needed here
double zObserved = zTrue - 0.03; // Add some "error"
// --- Perform EKF update ---
ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance);
@ -232,12 +231,12 @@ TEST(ManifoldEKF_DynamicMatrix, Update) {
// --- Verification (Manual Kalman Update Steps) ---
// 1. Predict measurement and get Jacobian H
Matrix H(1, 4); // This will be 1x4 for a 2x2 matrix measurement
Vector1 zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H);
double zPredictionManual = exampleDynamicMatrix::measureTrace(pStartMatrix, H);
// 2. Innovation and Innovation Covariance
// EKF calculates innovation_tangent = traits<Measurement>::Local(prediction, zObserved)
// For Vector1 (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual.
Vector1 innovationY = zObserved - zPredictionManual;
// For double (a VectorSpace), Local(A,B) is B-A. So, zObserved - zPredictionManual.
double innovationY = zObserved - zPredictionManual;
Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance;
// 3. Kalman Gain K