From 16f650c16c91d2923cab26ead7a9dc95de8b55ce Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 9 May 2025 00:48:07 -0400 Subject: [PATCH] Retain efficiency in static case --- gtsam/navigation/InvariantEKF.h | 29 ++-- gtsam/navigation/LieGroupEKF.h | 70 +++++---- gtsam/navigation/ManifoldEKF.h | 165 ++++++++++++--------- gtsam/navigation/tests/testManifoldEKF.cpp | 31 ++-- 4 files changed, 167 insertions(+), 128 deletions(-) diff --git a/gtsam/navigation/InvariantEKF.h b/gtsam/navigation/InvariantEKF.h index d2c6c2cb8..c0d95db84 100644 --- a/gtsam/navigation/InvariantEKF.h +++ b/gtsam/navigation/InvariantEKF.h @@ -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`. */ template class InvariantEKF : public LieGroupEKF { public: using Base = LieGroupEKF; ///< 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. using Jacobian = typename Base::Jacobian; + /// Covariance matrix type. Eigen::Matrix. + 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). */ - 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::AdjointMap should exist const Jacobian A = traits::Inverse(U).AdjointMap(); - // P_ is Matrix. A is Eigen::Matrix. Q is Matrix. + // P_ is Covariance. A is Jacobian. Q is Covariance. + // All are Eigen::Matrix. 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). */ - void predict(const TangentVector& u, double dt, const Matrix& Q) { + void predict(const TangentVector& u, double dt, const Covariance& Q) { const G U = traits::Expmap(u * dt); predict(U, Q); // Call the group composition predict } diff --git a/gtsam/navigation/LieGroupEKF.h b/gtsam/navigation/LieGroupEKF.h index 47c715531..da41a1e3d 100644 --- a/gtsam/navigation/LieGroupEKF.h +++ b/gtsam/navigation/LieGroupEKF.h @@ -48,17 +48,16 @@ namespace gtsam { class LieGroupEKF : public ManifoldEKF { public: using Base = ManifoldEKF; ///< 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::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; + 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 + using Covariance = typename Base::Covariance; // Eigen::Matrix /** * 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::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 Df) + * Signature: TangentVector f(const G& X, OptionalJacobian Df) * Df = d(xi)/d(local(X)) - * If n is Eigen::Dynamic, OptionalJacobian is OptionalJacobian. */ template using enable_if_dynamics = std::enable_if_t< !std::is_convertible_v&& std::is_invocable_r_v&>>; + OptionalJacobian&>>; /** * 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&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @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 > - G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { - Jacobian Df, Dexp; + G predictMean(Dynamics&& f, double dt, OptionalJacobian A = {}) const { + Jacobian Df, Dexp; // Eigen::Matrix + TangentVector xi = f(this->X_, Df); // xi and Df = d(xi)/d(localX) G U = traits::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) *A = traits::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&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, OptionalJacobian&) * @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). */ template > - void predict(Dynamics&& f, double dt, const Matrix& Q) { - Jacobian A; + void predict(Dynamics&& f, double dt, const Covariance& Q) { + Jacobian A; // Eigen::Matrix + if constexpr (Dim == Eigen::Dynamic) { + A.resize(this->n_, this->n_); + } this->X_ = predictMean(std::forward(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 Df) + * Signature: TangentVector f(const G& X, const Control& u, OptionalJacobian Df) */ template using enable_if_full_dynamics = std::enable_if_t< - std::is_invocable_r_v&> + std::is_invocable_r_v&> >; /** @@ -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&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @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 > - G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { - return predictMean([&](const G& X, OptionalJacobian Df) { return f(X, u, Df); }, dt, A); + G predictMean(Dynamics&& f, const Control& u, double dt, OptionalJacobian A = {}) const { + return predictMean([&](const G& X, OptionalJacobian 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&) + * @tparam Dynamics Functor signature: TangentVector f(const G&, const Control&, OptionalJacobian&) * @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). */ template > - void predict(Dynamics&& f, const Control& u, double dt, const Matrix& Q) { - return predict([&](const G& X, OptionalJacobian 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 Df) { return f(X, u, Df); }, dt, Q); } }; // LieGroupEKF diff --git a/gtsam/navigation/ManifoldEKF.h b/gtsam/navigation/ManifoldEKF.h index 68eaaa793..0f405c3ae 100644 --- a/gtsam/navigation/ManifoldEKF.h +++ b/gtsam/navigation/ManifoldEKF.h @@ -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`, + * 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 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::dimension; + + /// Tangent vector type for the manifold M. using TangentVector = typename traits::TangentVector; + /// Covariance matrix type (P, Q). + using Covariance = Eigen::Matrix; + /// State transition Jacobian type (F). + using Jacobian = Eigen::Matrix; + /** * 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::value, "Template parameter M must be a GTSAM Manifold."); - // Determine tangent space dimension n_ at runtime. - if constexpr (traits::dimension == Eigen::Dynamic) { - // If M::dimension is dynamic, traits::GetDimension(M) must exist. - n_ = traits::GetDimension(X0); + if constexpr (Dim == Eigen::Dynamic) { + n_ = traits::GetDimension(X0); // Runtime dimension for dynamic case } else { - n_ = traits::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 } 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). + 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). + * @param Q Process noise covariance matrix (Eigen::Matrix). */ - 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, 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. * @param z Observed measurement. - * @param R Measurement noise covariance (size m x m). + * @param R Measurement noise covariance (Eigen::Matrix). */ template void update(const Measurement& prediction, - const Matrix& H, + const Eigen::Matrix::dimension, Dim>& H, const Measurement& z, - const Matrix& R) { + const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold for LocalCoordinates."); - int m; // Measurement dimension - if constexpr (traits::dimension == Eigen::Dynamic) { - m = traits::GetDimension(z); - if (traits::GetDimension(prediction) != m) { + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); + if (traits::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::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::TangentVector innovation = traits::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 + Eigen::Matrix 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 + Eigen::Matrix 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::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 + if constexpr (Dim == Eigen::Dynamic) { + I_n = Jacobian::Identity(n_, n_); + } + else { + I_n = Jacobian::Identity(); + } + + // I_KH will be Eigen::Matrix + 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, Vector). * @tparam MeasurementFunction Functor/lambda with signature compatible with: * `Measurement h(const M& x, Jac& H_jacobian)` - * where `Jac` can be `Matrix&` or `OptionalJacobian&`. + * where `Jac` can be `Eigen::Matrix&` or + * `OptionalJacobian&`. * 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). */ template - void update(MeasurementFunction&& h, const Measurement& z, const Matrix& R) { + void update(MeasurementFunction&& h_func, const Measurement& z, + const Eigen::Matrix::dimension, traits::dimension>& R) { static_assert(IsManifold::value, "Template parameter Measurement must be a GTSAM Manifold."); - int m; // Measurement dimension - if constexpr (traits::dimension == Eigen::Dynamic) { - m = traits::GetDimension(z); + static constexpr int MeasDim = traits::dimension; + + int m_runtime; // Measurement dimension at runtime + if constexpr (MeasDim == Eigen::Dynamic) { + m_runtime = traits::GetDimension(z); } else { - m = traits::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). + int n_; ///< Runtime tangent space dimension of M. }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp index 50730a8d4..7516cd1cc 100644 --- a/gtsam/navigation/tests/testManifoldEKF.cpp +++ b/gtsam/navigation/tests/testManifoldEKF.cpp @@ -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(h, p); + auto h = [](const Unit3& p_) { return p_.point3().z(); }; + *H = numericalDerivative11(h, p); } - return Vector1(p.point3().z()); + return p.point3().z(); } } // namespace exampleUnit3 @@ -116,8 +116,8 @@ TEST(ManifoldEKF_Unit3, Update) { ManifoldEKF 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::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