Now handle dynamic types
Frank Dellaert
parent
ac295ebcac
commit
eacdf1c7fa
|
|
@ -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 {
|
||||
|
||||
|
|
@ -41,32 +43,41 @@ namespace gtsam {
|
|||
*
|
||||
* The state-dependent prediction methods from LieGroupEKF are hidden.
|
||||
* The update step remains the same as in ManifoldEKF/LieGroupEKF.
|
||||
* Covariances (P, Q) are `Matrix`.
|
||||
*/
|
||||
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.
|
||||
// Becomes Matrix if G has dynamic dimension.
|
||||
using Jacobian = typename Base::Jacobian;
|
||||
|
||||
/// 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 (must be Matrix).
|
||||
*/
|
||||
InvariantEKF(const G& X0, const Matrix& 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 in the tangent space (must be Matrix, size n_ x n_).
|
||||
*/
|
||||
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();
|
||||
const Jacobian A = traits<G>::Inverse(U).AdjointMap();
|
||||
// P_ is Matrix. A is Eigen::Matrix<double,n,n>. Q is Matrix.
|
||||
this->P_ = A * this->P_ * A.transpose() + Q;
|
||||
}
|
||||
|
||||
|
|
@ -77,10 +88,10 @@ 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 (Matrix, size n_ x n_).
|
||||
*/
|
||||
void predict(const TangentVector& u, double dt, const Matrix& Q) {
|
||||
G U = traits<G>::Expmap(u * dt);
|
||||
const G U = traits<G>::Expmap(u * dt);
|
||||
predict(U, Q); // Call the group composition predict
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
|||
* @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`).
|
||||
* Must satisfy LieGroup concept (`IsLieGroup<G>::value`).
|
||||
*
|
||||
* This filter specializes ManifoldEKF for Lie groups, offering predict methods
|
||||
* with state-dependent dynamics functions.
|
||||
|
|
@ -48,13 +48,19 @@ namespace gtsam {
|
|||
class LieGroupEKF : public ManifoldEKF<G> {
|
||||
public:
|
||||
using Base = ManifoldEKF<G>; ///< Base class type
|
||||
static constexpr int n = Base::n; ///< Group dimension (tangent space dimension)
|
||||
// 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
|
||||
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
|
||||
// Jacobian for group-specific operations like Adjoint, Expmap derivatives.
|
||||
// Becomes Matrix if n is Eigen::Dynamic.
|
||||
using Jacobian = Eigen::Matrix<double, n, n>;
|
||||
|
||||
/// 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 (must be 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");
|
||||
}
|
||||
|
||||
|
|
@ -62,6 +68,7 @@ namespace gtsam {
|
|||
* 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))
|
||||
* If n is Eigen::Dynamic, OptionalJacobian<n,n> is OptionalJacobian<Dynamic,Dynamic>.
|
||||
*/
|
||||
template <typename Dynamics>
|
||||
using enable_if_dynamics = std::enable_if_t<
|
||||
|
|
@ -79,7 +86,7 @@ namespace gtsam {
|
|||
* @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.
|
||||
* @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>>
|
||||
|
|
@ -99,14 +106,14 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* Predict step with state-dependent dynamics:
|
||||
* Uses predictMean to compute X_{k+1} and F, then calls base predict.
|
||||
* 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
|
||||
*
|
||||
* @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).
|
||||
* @param Q Process noise covariance (Matrix, size n_ x n_).
|
||||
*/
|
||||
template <typename Dynamics, typename = enable_if_dynamics<Dynamics>>
|
||||
void predict(Dynamics&& f, double dt, const Matrix& Q) {
|
||||
|
|
@ -153,7 +160,7 @@ namespace gtsam {
|
|||
* @param f Dynamics functor.
|
||||
* @param u Control input.
|
||||
* @param dt Time step.
|
||||
* @param Q Process noise covariance (size nxn).
|
||||
* @param Q Process noise covariance (must be Matrix, size n_ x n_).
|
||||
*/
|
||||
template <typename Control, typename Dynamics, typename = enable_if_full_dynamics<Control, Dynamics>>
|
||||
void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) {
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@
|
|||
* differentiable manifold. It relies on the manifold's retract and
|
||||
* 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).
|
||||
*
|
||||
* @date April 24, 2025
|
||||
* @authors Scott Baker, Matt Kielo, Frank Dellaert
|
||||
*/
|
||||
|
|
@ -27,9 +30,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 {
|
||||
|
|
@ -39,41 +44,59 @@ namespace gtsam {
|
|||
* @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.
|
||||
* - `traits<M>` specialization must exist, defining
|
||||
* `dimension` (compile-time or Eigen::Dynamic),
|
||||
* `TangentVector`, `Retract`, and `LocalCoordinates`.
|
||||
* If `dimension` is Eigen::Dynamic, `GetDimension(const M&)`
|
||||
* must be provided by traits.
|
||||
*
|
||||
* 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. The covariance P is always stored as a gtsam::Matrix.
|
||||
* 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
|
||||
/// Tangent vector type for the manifold M, as defined by its traits.
|
||||
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.
|
||||
* @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.
|
||||
*/
|
||||
ManifoldEKF(const M& X0, const Matrix& P0) : X_(X0), P_(P0) {
|
||||
static_assert(IsManifold<M>::value,
|
||||
"Template parameter M must be a GTSAM Manifold.");
|
||||
|
||||
/// 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");
|
||||
// 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);
|
||||
}
|
||||
else {
|
||||
n_ = traits<M>::dimension;
|
||||
}
|
||||
|
||||
virtual ~ManifoldEKF() = default; // Add virtual destructor for base class
|
||||
// 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_) + ").");
|
||||
}
|
||||
}
|
||||
|
||||
/// @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 in the tangent space (always a Matrix).
|
||||
const Matrix& covariance() const { return P_; }
|
||||
|
||||
/**
|
||||
* Basic predict step: Updates state and covariance given the predicted
|
||||
|
|
@ -83,11 +106,23 @@ 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 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).
|
||||
*/
|
||||
void predict(const M& X_next, const MatrixN& F, const Matrix& Q) {
|
||||
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_) + ".");
|
||||
}
|
||||
X_ = X_next;
|
||||
P_ = F * P_ * F.transpose() + Q;
|
||||
}
|
||||
|
|
@ -96,7 +131,7 @@ namespace gtsam {
|
|||
* 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 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.
|
||||
|
|
@ -105,56 +140,99 @@ namespace gtsam {
|
|||
*/
|
||||
template <typename Measurement>
|
||||
void update(const Measurement& prediction,
|
||||
const Eigen::Matrix<double, traits<Measurement>::dimension, n>& H,
|
||||
const Measurement& z, const Matrix& R) {
|
||||
// Innovation z \ominus prediction
|
||||
Vector innovation = traits<Measurement>::Local(z, prediction);
|
||||
const Matrix& H,
|
||||
const Measurement& z,
|
||||
const Matrix& R) {
|
||||
|
||||
// Innovation covariance and Kalman Gain
|
||||
auto S = H * P_ * H.transpose() + R; // S is m x m
|
||||
Matrix K = P_ * H.transpose() * S.inverse(); // K = P H^T S^-1 (size n x m)
|
||||
static_assert(IsManifold<Measurement>::value,
|
||||
"Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates.");
|
||||
|
||||
// Correction vector in tangent space
|
||||
TangentVector delta_xi = -K * innovation; // delta_xi is n x 1
|
||||
int m; // Measurement dimension
|
||||
if constexpr (traits<Measurement>::dimension == Eigen::Dynamic) {
|
||||
m = traits<Measurement>::GetDimension(z);
|
||||
if (traits<Measurement>::GetDimension(prediction) != m) {
|
||||
throw std::invalid_argument(
|
||||
"ManifoldEKF::update: Dynamic measurement 'prediction' and 'z' have different dimensions.");
|
||||
}
|
||||
}
|
||||
else {
|
||||
m = traits<Measurement>::dimension;
|
||||
}
|
||||
|
||||
// Update state using retract
|
||||
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) + ".");
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
// Kalman Gain: K = P H^T S^-1
|
||||
const Matrix K = P_ * H.transpose() * S.inverse(); // K is n_ x m
|
||||
|
||||
// Correction vector in tangent space of M: delta_xi = K * innovation
|
||||
const TangentVector delta_xi = K * innovation; // delta_xi is n_ x 1
|
||||
|
||||
// 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
|
||||
MatrixN I_KH = I_n - K * H; // I_KH is n x n
|
||||
const auto I_n = Matrix::Identity(n_, n_);
|
||||
const Matrix I_KH = I_n - K * H; // I_KH is n x n
|
||||
P_ = I_KH * P_ * I_KH.transpose() + K * R * K.transpose();
|
||||
}
|
||||
|
||||
/**
|
||||
* Measurement update: Corrects the state and covariance using a measurement.
|
||||
* Measurement update: Corrects the state and covariance using a measurement
|
||||
* model function.
|
||||
*
|
||||
* @tparam Measurement Type of the measurement vector (e.g., VectorN<m>)
|
||||
* @tparam Prediction Functor signature: Measurement h(const M&, OptionalJacobian<m,n>&)
|
||||
* @param h Measurement model functor returning predicted measurement prediction
|
||||
* and its Jacobian H = d(h)/d(local(X)).
|
||||
* @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_>&`.
|
||||
* The Jacobian H should be d(h)/d(local(X)).
|
||||
* @param h Measurement model function.
|
||||
* @param z Observed measurement.
|
||||
* @param R Measurement noise covariance (size m x m).
|
||||
* @param R Measurement noise covariance (must be an m x m Matrix).
|
||||
*/
|
||||
template <typename Measurement, typename Prediction>
|
||||
void update(Prediction&& h, const Measurement& z, const Matrix& R) {
|
||||
constexpr int m = traits<Measurement>::dimension;
|
||||
template <typename Measurement, typename MeasurementFunction>
|
||||
void update(MeasurementFunction&& h, const Measurement& z, const Matrix& 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);
|
||||
}
|
||||
else {
|
||||
m = traits<Measurement>::dimension;
|
||||
}
|
||||
|
||||
// Predict measurement and get Jacobian H = dh/dlocal(X)
|
||||
Eigen::Matrix<double, m, n> H;
|
||||
Matrix H(m, n_);
|
||||
Measurement prediction = h(X_, H);
|
||||
update(prediction, H, z, R); // Call the other update function
|
||||
|
||||
// Call the other update function
|
||||
update(prediction, H, z, R);
|
||||
}
|
||||
|
||||
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;
|
||||
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.
|
||||
};
|
||||
|
||||
// Define static identity I_n
|
||||
template <typename M>
|
||||
const typename ManifoldEKF<M>::MatrixN ManifoldEKF<M>::I_n = ManifoldEKF<M>::MatrixN::Identity();
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -147,6 +147,115 @@ 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 predictNextState(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)
|
||||
Vector1 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));
|
||||
}
|
||||
|
||||
} // namespace exampleDynamicMatrix
|
||||
|
||||
// Test fixture for ManifoldEKF with a 2x2 Matrix state
|
||||
struct DynamicMatrixEKFTest {
|
||||
Matrix p0Matrix; // Initial state (as 2x2 Matrix)
|
||||
Matrix p0Covariance; // Initial covariance (dynamic Matrix, 4x4)
|
||||
Vector velocityTangent; // Control input in tangent space (Vector4 for 2x2 matrix)
|
||||
double dt;
|
||||
Matrix processNoiseCovariance; // Process noise covariance (dynamic Matrix, 4x4)
|
||||
Matrix measurementNoiseCovariance; // Measurement noise covariance (dynamic Matrix, 1x1)
|
||||
|
||||
DynamicMatrixEKFTest() :
|
||||
p0Matrix((Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished()),
|
||||
p0Covariance(I_4x4 * 0.01),
|
||||
velocityTangent((Vector(4) << 0.5, 0.1, -0.1, -0.5).finished()), // [dp00, dp10, dp01, dp11]/sec
|
||||
dt(0.1),
|
||||
processNoiseCovariance(I_4x4 * 0.001),
|
||||
measurementNoiseCovariance(Matrix::Identity(1, 1) * 0.005)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
TEST(ManifoldEKF_DynamicMatrix, Predict) {
|
||||
DynamicMatrixEKFTest data;
|
||||
|
||||
ManifoldEKF<Matrix> ekf(data.p0Matrix, data.p0Covariance);
|
||||
// For a 2x2 Matrix, tangent space dimension is 2*2=4.
|
||||
EXPECT_LONGS_EQUAL(4, ekf.state().size());
|
||||
EXPECT_LONGS_EQUAL(data.p0Matrix.rows() * data.p0Matrix.cols(), ekf.state().size());
|
||||
|
||||
// --- Prepare inputs for ManifoldEKF::predict ---
|
||||
Matrix pNextExpected = exampleDynamicMatrix::predictNextState(data.p0Matrix, data.velocityTangent, data.dt);
|
||||
|
||||
// For this linear prediction model (p_next = p_current + V*dt in tangent space), F is Identity.
|
||||
Matrix jacobianF = I_4x4; // Jacobian of the state transition function
|
||||
|
||||
// --- Perform EKF prediction ---
|
||||
ekf.predict(pNextExpected, jacobianF, data.processNoiseCovariance);
|
||||
|
||||
// --- Verification ---
|
||||
EXPECT(assert_equal(pNextExpected, ekf.state(), 1e-9));
|
||||
Matrix pCovarianceExpected = jacobianF * data.p0Covariance * jacobianF.transpose() + data.processNoiseCovariance;
|
||||
EXPECT(assert_equal(pCovarianceExpected, ekf.covariance(), 1e-9));
|
||||
}
|
||||
|
||||
TEST(ManifoldEKF_DynamicMatrix, Update) {
|
||||
DynamicMatrixEKFTest data;
|
||||
|
||||
Matrix pStartMatrix = (Matrix(2, 2) << 1.5, -0.5, 0.8, 2.5).finished();
|
||||
Matrix pStartCovariance = I_4x4 * 0.02;
|
||||
ManifoldEKF<Matrix> ekf(pStartMatrix, pStartCovariance);
|
||||
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"
|
||||
|
||||
// --- Perform EKF update ---
|
||||
ekf.update(exampleDynamicMatrix::measureTrace, zObserved, data.measurementNoiseCovariance);
|
||||
|
||||
// --- 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);
|
||||
|
||||
// 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;
|
||||
Matrix innovationCovarianceS = H * pStartCovariance * H.transpose() + data.measurementNoiseCovariance;
|
||||
|
||||
// 3. Kalman Gain K
|
||||
Matrix kalmanGainK = pStartCovariance * H.transpose() * innovationCovarianceS.inverse(); // K is 4x1
|
||||
|
||||
// 4. State Correction (in tangent space of Matrix)
|
||||
Vector deltaXiTangent = kalmanGainK * innovationY; // deltaXi is 4x1 Vector
|
||||
|
||||
// 5. Expected Updated State and Covariance
|
||||
Matrix pUpdatedExpected = traits<Matrix>::Retract(pStartMatrix, deltaXiTangent);
|
||||
|
||||
// Covariance update: P_new = (I - K H) P_old
|
||||
Matrix pUpdatedCovarianceExpected = (I_4x4 - kalmanGainK * H) * pStartCovariance;
|
||||
|
||||
// --- Compare EKF result with manual calculation ---
|
||||
EXPECT(assert_equal(pUpdatedExpected, ekf.state(), 1e-9));
|
||||
EXPECT(assert_equal(pUpdatedCovarianceExpected, ekf.covariance(), 1e-9));
|
||||
}
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue