working commit with nees for gal3imuekf and navstateimuekf with proof and visualizations to support

eval/Dataset.cpp

@@ -41,9 +41,17 @@ void Dataset::setImuCovariances(const std::shared_ptr<PreintegrationCombinedPara
     params->setBiasOmegaCovariance(Matrix3::Identity() * std::pow(noise.sigmaGyroBias, 2));
 }
 
-std::shared_ptr<PreintegrationCombinedParams> Dataset::configureImuParams(double alpha) const {
-    auto params = PreintegrationCombinedParams::MakeSharedD(getGravity());
-    params->n_gravity = Vector3(0, 0, -getGravity());
+std::shared_ptr<PreintegrationCombinedParams> Dataset::configureImuParams(double alpha, 
+    const std::shared_ptr<PreintegrationCombinedParams>& customParams) const {
+
+    std::shared_ptr<PreintegrationCombinedParams> params;
+    if (customParams) {
+        params = customParams;
+    } else {
+        params = PreintegrationCombinedParams::MakeSharedD(getGravity());
+        params->n_gravity = Vector3(0, 0, -getGravity());
+    }
+
     setImuCovariances(params, computeNoiseParams(alpha));
     return params;
 }

eval/Dataset.h

@@ -84,9 +84,11 @@ class Dataset {
     /**
      * @brief Configure IMU preintegration parameters for this dataset
      * @param alpha Scaling factor for noise parameters (default: 3.0)
+     * @param customParams Optional custom parameters to use as base
      * @return Configured preintegration parameters
      */
-    std::shared_ptr<PreintegrationCombinedParams> configureImuParams(double alpha = 3.0) const;
+    std::shared_ptr<PreintegrationCombinedParams> configureImuParams(double alpha = 3.0, 
+        const std::shared_ptr<PreintegrationCombinedParams>& customParams = nullptr) const;
 
 private:
     std::vector<StateMeasurement> states_;

eval/EKFNEESEvaluator.h

@@ -0,0 +1,148 @@
+/* ----------------------------------------------------------------------------
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+ *
+ * See LICENSE for the license information
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   EKFNEESEvaluator.h
+ * @brief  NEES evaluator for EKF comparison
+ * @author Alec Kain
+ */
+
+#pragma once
+
+#include "NEESEvaluator.h"
+#include <gtsam/navigation/Gal3ImuEKF.h>
+#include <gtsam/navigation/NavStateImuEKF.h>
+#include "TrajectoryValidator.h"
+
+namespace gtsam {
+
+/// Extended NEES evaluator for EKF comparison
+class EKFNEESEvaluator {
+public:
+    /// Window indices for processing
+    struct WindowIndices {
+        int startIndex;
+        int endIndex;
+    };
+
+    /// Constructor
+    explicit EKFNEESEvaluator(const Dataset& dataset);
+
+    /// Compute timestep from dataset
+    double computeTimestep() const;
+
+    /// Run Gal3ImuEKF evaluation (uses default dataset name)
+    NEESEvaluator::NEESResults runGal3ImuEKF(double interval, double alpha) const;
+
+    /// Run Gal3ImuEKF evaluation with custom dataset name for file output
+    NEESEvaluator::NEESResults runGal3ImuEKF(double interval, double alpha, 
+                                             const std::string& datasetName) const;
+
+    /// Run NavStateImuEKF evaluation
+    NEESEvaluator::NEESResults runNavStateImuEKF(double interval, double alpha) const;
+
+    /// Run NavStateImuEKF evaluation with custom dataset name for file output
+    NEESEvaluator::NEESResults runNavStateImuEKF(double interval, double alpha,
+                                                  const std::string& datasetName) const;
+
+private:
+    const Dataset& dataset_;
+
+    /// Compute NEES value
+    std::optional<double> computeNEES(const Vector& error, 
+                                      const Matrix& covarianceMatrix) const;
+
+    /// Compute window indices
+    WindowIndices computeWindowIndices(int windowNumber, int windowSize) const;
+
+    /// Check if window is valid
+    bool isWindowValid(const WindowIndices& window) const;
+
+    /// Gal3 helper functions
+    Gal3 convertToGal3(const NavState& navState, double time) const;
+    Matrix initializeGal3Covariance() const;
+    Gal3ImuEKF initializeGal3EKF(const NavState& initialState,
+                                  const std::shared_ptr<PreintegrationCombinedParams>& params) const;
+    void propagateGal3EKF(Gal3ImuEKF& ekf, const WindowIndices& window,
+                         const imuBias::ConstantBias& bias, double dt) const;
+    Vector9 computeGal3Error(const Gal3& predicted, const Gal3& groundTruth) const;
+    Matrix9 extractNavigationCovariance(const Gal3ImuEKF& ekf) const;
+
+    /// Trajectory data management
+    TrajectoryValidator::TrajectoryPoint createGroundTruthPoint(
+        const NavState& navState, double timestamp) const;
+    TrajectoryValidator::TrajectoryPoint createPredictedPointFromGal3(
+        const Gal3& gal3State, const Matrix9& covariance, double timestamp) const;
+    TrajectoryValidator::TrajectoryPoint createPredictedPointFromNavState(
+        const NavState& navState, const Matrix9& covariance, double timestamp) const;
+    void storeTrajectoryData(
+        std::vector<TrajectoryValidator::TrajectoryPoint>& groundTruthTrajectory,
+        std::vector<TrajectoryValidator::TrajectoryPoint>& predictedTrajectory,
+        std::vector<Vector9>& errorTrajectory,
+        const Gal3& predictedGal3, const NavState& groundTruthNavState,
+        const Matrix9& navigationCovariance, const Vector9& navigationError,
+        double timestamp) const;
+
+    /// Process single Gal3 window
+    std::optional<double> processGal3Window(
+        const WindowIndices& window,
+        const std::shared_ptr<PreintegrationCombinedParams>& params,
+        double dt,
+        std::vector<TrajectoryValidator::TrajectoryPoint>& groundTruthTrajectory,
+        std::vector<TrajectoryValidator::TrajectoryPoint>& predictedTrajectory,
+        std::vector<Vector9>& errorTrajectory) const;
+
+    /// Export trajectory results with dataset name and preintegration time
+    void exportTrajectoryResults(
+        const std::vector<TrajectoryValidator::TrajectoryPoint>& groundTruthTrajectory,
+        const std::vector<TrajectoryValidator::TrajectoryPoint>& predictedTrajectory,
+        const std::vector<Vector9>& errorTrajectory,
+        const std::string& filterName,
+        const std::string& datasetName,
+        const std::string& preintegrationTime) const;
+
+    /// Export NEES values with dataset name and preintegration time
+    void exportNEESValues(
+        const std::string& filterName,
+        const std::string& datasetName,
+        const std::string& preintegrationTime,
+        const std::vector<TrajectoryValidator::TrajectoryPoint>& trajectory,
+        const std::vector<double>& neesValues) const;
+
+    /// Process time window with Gal3ImuEKF (with dataset name)
+    NEESEvaluator::NEESResults processTimeWindowWithGal3EKF(
+        const std::shared_ptr<PreintegrationCombinedParams>& params,
+        double preintegrationTime, double dt,
+        const std::string& datasetName) const;
+
+    /// NavState helper functions
+    NavStateImuEKF initializeNavStateEKF(const NavState& initialState,
+                                          const std::shared_ptr<PreintegrationCombinedParams>& params) const;
+    void propagateNavStateEKF(NavStateImuEKF& ekf, const WindowIndices& window,
+                              const imuBias::ConstantBias& bias, double dt) const;
+    Vector9 computeNavStateError(const NavState& predicted, 
+                                 const NavState& groundTruth) const;
+
+    /// Process single NavState window
+    std::optional<double> processNavStateWindow(
+        const WindowIndices& window,
+        const std::shared_ptr<PreintegrationCombinedParams>& params,
+        double dt,
+        std::vector<TrajectoryValidator::TrajectoryPoint>& groundTruthTrajectory,
+        std::vector<TrajectoryValidator::TrajectoryPoint>& predictedTrajectory,
+        std::vector<Vector9>& errorTrajectory) const;
+
+    /// Process time window with NavStateImuEKF
+    NEESEvaluator::NEESResults processTimeWindowWithNavStateEKF(
+        const std::shared_ptr<PreintegrationCombinedParams>& params,
+        double preintegrationTime, double dt,
+        const std::string& datasetName) const;
+};
+
+} // namespace gtsam

eval/NEESEvaluator.cpp

@@ -30,7 +30,7 @@ Vector NEESEvaluator::computeError(const NavState& predicted,
                                  const imuBias::ConstantBias& biasPred,
                                  const imuBias::ConstantBias& biasActual) const {
     Vector15 error;
-    error << predicted.localCoordinates(actual),
+    error << predicted.logmap(actual),
              biasActual.vector() - biasPred.vector();
     return error;
 }

eval/NEESEvaluator.h

@@ -23,6 +23,7 @@
 
 namespace gtsam {
 
+/// NEES evaluation for IMU preintegration
 class NEESEvaluator {
 public:
     /**
@@ -36,28 +37,31 @@ class NEESEvaluator {
         double preintTime;
     };
 
-    // Evaluates NEES values given a dataset (Dataset)
+    /// Evaluates NEES values given a dataset (Dataset)
     explicit NEESEvaluator(const Dataset& dataset) : dataset_(dataset) {}
 
-    // Method for running the evaluator with parameters for interval (double) and alpha size (double)
-    // Returns NEES results instead of printing
+    /// Method for running the evaluator with parameters for interval (double) and alpha size (double)
+    /// Returns NEES results instead of printing
     NEESResults run(double interval, double alpha = 3.0) const; 
 
-    // Static method for printing NEES results
+    /// Static method for printing NEES results
     static void printNeesStatistics(const NEESResults& results);
+
+    /// Compute statistics from NEES values (made public for EKF evaluator)
+    static NEESResults computeStatistics(const std::vector<double>& neesResults, double preintTime);
 
 private:
     const Dataset& dataset_;
 
-    // Helper functions ordered "up" - used functions defined before calling functions
+    /// Helper functions ordered "up" - used functions defined before calling functions
 
     bool isValidWindow(int startIdx, int endIdx) const;
 
     Vector computeError(const NavState& predicted, 
                        const NavState& actual,
                        const imuBias::ConstantBias& biasPred,
                        const imuBias::ConstantBias& biasActual) const;
-                       
+
     std::optional<double> computeNEES(const Vector& error, const Matrix& covMatrix) const;
 
     std::optional<double> calculateWindowNEES(const std::shared_ptr<PreintegrationCombinedParams>& params,
@@ -69,12 +73,10 @@ class NEESEvaluator {
     NEESResults processTimeWindow(const std::shared_ptr<PreintegrationCombinedParams>& params,
                                 double preintTime, double dt) const;
 
-    // Statistics computation helper functions
+    /// Statistics computation helper functions
     static double computeMean(const std::vector<double>& values);
     static double computeMedian(const std::vector<double>& values);
     static double computeVariance(const std::vector<double>& values, double mean);
-
-    static NEESResults computeStatistics(const std::vector<double>& neesResults, double preintTime);
 };
 
 } // namespace gtsam