From 6852dd6dcfaa9534ac58a25b5379e00de86d71e3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 26 Jan 2026 11:42:00 -0500 Subject: [PATCH 1/3] Expose Manifold PIM --- gtsam/navigation/navigation.i | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index d68cd1d31e..e2ffa4ed94 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -160,15 +160,16 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { }; #include -class PreintegratedImuMeasurements { +template +class PreintegratedImuMeasurementsT { // Constructors - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); - PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params, const gtsam::imuBias::ConstantBias& bias); // Testable void print(string s = "") const; - bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + bool equals(const gtsam::PreintegratedImuMeasurementsT& expected, double tol); // Standard Interface void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, @@ -177,7 +178,7 @@ class PreintegratedImuMeasurements { void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); gtsam::Matrix preintMeasCov() const; - gtsam::Vector preintegrated() const; + // gtsam::Vector preintegrated() const; only define for TangentPreintegration double deltaTij() const; gtsam::Rot3 deltaRij() const; gtsam::Vector deltaPij() const; @@ -191,6 +192,11 @@ class PreintegratedImuMeasurements { void serialize() const; }; +typedef gtsam::PreintegratedImuMeasurementsT PreintegratedImuMeasurements; +typedef gtsam::PreintegratedImuMeasurementsT PreintegratedImuMeasurementsManifold; +// Defining below leads to multiply defined errors: as Tangent is default, use PreintegratedImuMeasurements +// typedef gtsam::PreintegratedImuMeasurementsT PreintegratedImuMeasurementsTangent; + virtual class ImuFactor: gtsam::NonlinearFactor { ImuFactor(gtsam::Key pose_i, gtsam::Key vel_i, gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias, From ea25f544114f3a5603ec721913a97f4cc9663d5f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 26 Jan 2026 11:42:22 -0500 Subject: [PATCH 2/3] Better wrapping --- gtsam/navigation/PreintegrationBase.h | 2 +- gtsam/navigation/navigation.i | 79 +++++++++++++++++---------- 2 files changed, 52 insertions(+), 29 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index de3a380cfe..d75037b69a 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -55,10 +55,10 @@ class GTSAM_EXPORT PreintegrationBase { /// Default constructor for serialization PreintegrationBase() {} + public: /// Virtual destructor for serialization virtual ~PreintegrationBase() {} - public: /// @name Constructors /// @{ diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index e2ffa4ed94..40c11c4817 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -159,9 +159,58 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { void serialize() const; }; +#include +virtual class PreintegrationBase { + // Basic utilities + void resetIntegration(); + void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); + bool matchesParamsWith(const gtsam::PreintegrationBase& other) const; + + // Access + gtsam::imuBias::ConstantBias biasHat() const; + double deltaTij() const; + gtsam::Vector deltaPij() const; + gtsam::Vector deltaVij() const; + gtsam::Rot3 deltaRij() const; + gtsam::NavState deltaXij() const; + gtsam::Vector biasHatVector() const; + + // Standard Interface + void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, + double deltaT); + gtsam::Vector biasCorrectedDelta(const gtsam::imuBias::ConstantBias& bias_i) const; + gtsam::Vector biasCorrectedDelta(const gtsam::imuBias::ConstantBias& bias_i, + Eigen::Ref H) const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias_i) const; + gtsam::NavState predict(const gtsam::NavState& state_i, + const gtsam::imuBias::ConstantBias& bias_i, + Eigen::Ref H1, + Eigen::Ref H2) const; + gtsam::Vector computeError(const gtsam::NavState& state_i, + const gtsam::NavState& state_j, + const gtsam::imuBias::ConstantBias& bias_i, + Eigen::Ref H1, + Eigen::Ref H2, + Eigen::Ref H3) const; + gtsam::Vector computeErrorAndJacobians(const gtsam::Pose3& pose_i, + gtsam::Vector vel_i, + const gtsam::Pose3& pose_j, + gtsam::Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + Eigen::Ref H1, + Eigen::Ref H2, + Eigen::Ref H3, + Eigen::Ref H4, + Eigen::Ref H5) const; + + // Testable + void print(string s = "") const; +}; + #include template -class PreintegratedImuMeasurementsT { +class PreintegratedImuMeasurementsT : gtsam::PreintegrationBase { // Constructors PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params); PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params, @@ -172,21 +221,8 @@ class PreintegratedImuMeasurementsT { bool equals(const gtsam::PreintegratedImuMeasurementsT& expected, double tol); // Standard Interface - void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega, - double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - gtsam::Matrix preintMeasCov() const; // gtsam::Vector preintegrated() const; only define for TangentPreintegration - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - gtsam::Vector deltaPij() const; - gtsam::Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - gtsam::Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; // enabling serialization functionality void serialize() const; @@ -251,7 +287,7 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { void serialize() const; }; -class PreintegratedCombinedMeasurements { +class PreintegratedCombinedMeasurements : gtsam::PreintegrationBase { // Constructors PreintegratedCombinedMeasurements( const gtsam::PreintegrationCombinedParams* params); @@ -264,20 +300,7 @@ class PreintegratedCombinedMeasurements { double tol); // Standard Interface - void integrateMeasurement(gtsam::Vector measuredAcc, - gtsam::Vector measuredOmega, double deltaT); - void resetIntegration(); - void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); - gtsam::Matrix preintMeasCov() const; - double deltaTij() const; - gtsam::Rot3 deltaRij() const; - gtsam::Vector deltaPij() const; - gtsam::Vector deltaVij() const; - gtsam::imuBias::ConstantBias biasHat() const; - gtsam::Vector biasHatVector() const; - gtsam::NavState predict(const gtsam::NavState& state_i, - const gtsam::imuBias::ConstantBias& bias) const; // enable serialization functionality void serialize() const; From a2fad09a43b1c472a4c9a5263d35dec764ac775c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 26 Jan 2026 11:42:34 -0500 Subject: [PATCH 3/3] NEES comparison --- .../NavStateImuPimCovarianceComparison.ipynb | 2249 +++++++++++++++++ 1 file changed, 2249 insertions(+) create mode 100644 python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb diff --git a/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb b/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb new file mode 100644 index 0000000000..f2c899148f --- /dev/null +++ b/python/gtsam/examples/NavStateImuPimCovarianceComparison.ipynb @@ -0,0 +1,2249 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# NavState IMU: PIM vs EKF Covariance and NEES\n", + "\n", + "This notebook compares the covariance and NEES produced by:\n", + "\n", + "- `NavStateImuEKF` (predict only, no measurement updates).\n", + "- `PreintegratedImuMeasurements` (default preintegration type, often tangent).\n", + "- `PreintegratedImuMeasurementsManifold` (manifold preintegration).\n", + "\n", + "We run a Monte Carlo simulation with synthetic IMU data and check that the two\n", + "preintegration strategies yield the same covariance and NEES, and that they\n", + "match the EKF predict step.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "8bfb7262", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab # type: ignore\n", + " %pip install --quiet gtsam-develop\n", + "except Exception:\n", + " pass # Not in Colab\n", + "\n", + "import numpy as np\n", + "import plotly.graph_objects as go\n", + "\n", + "import gtsam\n", + "from gtsam import ConstantTwistScenario, ScenarioRunner" + ] + }, + { + "cell_type": "markdown", + "id": "a18d7c45", + "metadata": {}, + "source": [ + "### NEES definition\n", + "\n", + "The Normalized Estimation Error Squared (NEES) is\n", + "\n", + "$$\\mathrm{NEES} = e^T P^{-1} e,$$\n", + "\n", + "where $e$ is the 9D NavState error (local coordinates) and $P$ is the predicted\n", + "covariance. A consistent filter should have mean NEES near the state dimension (9).\n" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "c29fbe5e", + "metadata": {}, + "outputs": [], + "source": [ + "from dataclasses import dataclass\n", + "\n", + "\n", + "@dataclass\n", + "class ErrorCov:\n", + " error: np.ndarray\n", + " covariance: np.ndarray\n", + "\n", + "\n", + "@dataclass\n", + "class TrialResult:\n", + " default: ErrorCov\n", + " manifold: ErrorCov\n", + " ekf: ErrorCov\n", + "\n", + "\n", + "def nees(error, covariance):\n", + " \"\"\"Compute NEES = e^T P^{-1} e.\"\"\"\n", + " return float(error.T @ np.linalg.solve(covariance, error))\n", + "\n", + "\n", + "def rel_frobenius(A, B):\n", + " \"\"\"Relative Frobenius norm ||A-B|| / ||B||.\"\"\"\n", + " return float(np.linalg.norm(A - B, ord=\"fro\") / np.linalg.norm(B, ord=\"fro\"))\n", + "\n", + "\n", + "def delta_error(delta_est, delta_true, exact):\n", + " \"\"\"Delta-space error: exact uses group difference of deltas.\"\"\"\n", + " if exact:\n", + " return gtsam.NavState.Expmap(delta_est).localCoordinates(\n", + " gtsam.NavState.Expmap(delta_true)\n", + " )\n", + " return delta_est - delta_true\n", + "\n", + "\n", + "def simulate_interval(interval_s, num_trials, dt, params, scenario, P0, seed=0):\n", + " \"\"\"Monte Carlo for a single preintegration interval.\n", + "\n", + " Returns per-trial errors and predicted covariances for:\n", + " - default PIM\n", + " - manifold PIM\n", + " - NavStateImuEKF\n", + " \"\"\"\n", + " steps = int(round(interval_s / dt))\n", + " if abs(steps * dt - interval_s) > 1e-12:\n", + " raise ValueError(\"interval_s must be an integer multiple of dt\")\n", + "\n", + " rng = np.random.default_rng(seed)\n", + " bias = gtsam.imuBias.ConstantBias(np.zeros(3), np.zeros(3))\n", + " runner = ScenarioRunner(scenario, params, dt, bias)\n", + "\n", + " X0_nominal = scenario.navState(0.0)\n", + " X_true = scenario.navState(interval_s)\n", + " delta_true = X0_nominal.localCoordinates(X_true)\n", + "\n", + " trials = []\n", + "\n", + " for _ in range(num_trials):\n", + " xi0 = rng.multivariate_normal(np.zeros(9), P0)\n", + " X0 = X0_nominal.retract(xi0)\n", + "\n", + " pim_default = gtsam.PreintegratedImuMeasurements(params, bias)\n", + " pim_manifold = gtsam.PreintegratedImuMeasurementsManifold(params, bias)\n", + " ekf = gtsam.NavStateImuEKF(X0, P0, params)\n", + "\n", + " for k in range(steps):\n", + " t = k * dt\n", + " omega_meas = runner.measuredAngularVelocity(t)\n", + " acc_meas = runner.measuredSpecificForce(t)\n", + "\n", + " pim_default.integrateMeasurement(acc_meas, omega_meas, dt)\n", + " pim_manifold.integrateMeasurement(acc_meas, omega_meas, dt)\n", + " ekf.predict(omega_meas, acc_meas, dt)\n", + "\n", + " X_pred_default = pim_default.predict(X0, bias)\n", + " X_pred_manifold = pim_manifold.predict(X0, bias)\n", + " X_pred_ekf = ekf.state()\n", + "\n", + " trial = TrialResult(\n", + " default=ErrorCov(\n", + " error=delta_error(X0.localCoordinates(X_pred_default), delta_true, True),\n", + " covariance=P0 + pim_default.preintMeasCov(),\n", + " ),\n", + " manifold=ErrorCov(\n", + " error=delta_error(X0.localCoordinates(X_pred_manifold), delta_true, True),\n", + " covariance=P0 + pim_manifold.preintMeasCov(),\n", + " ),\n", + " ekf=ErrorCov(\n", + " error=X_true.localCoordinates(X_pred_ekf), covariance=ekf.covariance()\n", + " ),\n", + " )\n", + "\n", + " trials.append(trial)\n", + "\n", + " return {\n", + " \"trials\": trials,\n", + " }" + ] + }, + { + "cell_type": "markdown", + "id": "42b31052", + "metadata": {}, + "source": [ + "### Scenario and IMU parameters\n", + "\n", + "We use a constant yaw-rate scenario with steady velocity. The EKF only runs\n", + "predict, and the PIMs only integrate IMU measurements.\n" + ] + }, + { + "cell_type": "markdown", + "id": "689ce1eb", + "metadata": {}, + "source": [ + "### Initial uncertainty\n", + "\n", + "We sample an initial NavState by drawing a 9D delta from `P0` and applying `retract`,\n", + "which is equivalent to using `NavState::Expmap` at the current state.\n", + "We transport `P0` through the PIM prediction Jacobian `H1` so the\n", + "PIM covariance is expressed in the NavState tangent space before NEES.\n", + "For PIM NEES we compute the error in **delta space anchored at X_i** using either\n", + "the exact group error or a simple delta subtraction (controlled by `use_exact_delta_error`).\n" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "2455e2f2", + "metadata": {}, + "outputs": [], + "source": [ + "# Scenario: steady yaw rate with constant world-frame velocity\n", + "radius = 30.0\n", + "angular_velocity = np.pi # rad/sec\n", + "w_b = np.array([0.0, 0.0, angular_velocity])\n", + "v_n = np.array([radius * angular_velocity, 0.0, 0.0])\n", + "scenario = ConstantTwistScenario(w_b, v_n)\n", + "\n", + "# IMU noise params (NavState, Z-up)\n", + "params = gtsam.PreintegrationParams.MakeSharedU(9.81)\n", + "accel_noise_sigma = 0.1\n", + "gyro_noise_sigma = 0.01\n", + "params.setAccelerometerCovariance(np.eye(3) * accel_noise_sigma**2)\n", + "params.setGyroscopeCovariance(np.eye(3) * gyro_noise_sigma**2)\n", + "params.setIntegrationCovariance(np.eye(3) * 1e-8)\n", + "\n", + "# Monte Carlo configuration\n", + "dt = 0.01 # 100 Hz IMU\n", + "intervals = np.array([50, 100, 150, 200]) / 1000 # milliseconds\n", + "num_trials = 200\n", + "\n", + "# Initial covariance (NavState tangent order: w, p, v)\n", + "init_sigmas = np.array([0.02, 0.02, 0.02, 0.2, 0.2, 0.2, 0.5, 0.5, 0.5])\n", + "P0 = np.diag(init_sigmas**2)\n", + "\n", + "use_exact_delta_error = (\n", + " True # True: group error in delta space, False: delta subtraction\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "d91783e0", + "metadata": {}, + "source": [ + "### Run Monte Carlo across multiple preintegration intervals\n" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "a05fd96f", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[{'interval_s': np.float64(0.05),\n", + " 'nees_default_mean': 0.732541283415913,\n", + " 'nees_default_std': 0.45064446831420696,\n", + " 'nees_manifold_mean': 0.7325417437759313,\n", + " 'nees_manifold_std': 0.4506453732727786,\n", + " 'nees_ekf_mean': 8.578577369813082,\n", + " 'nees_ekf_std': 3.9847842910628835,\n", + " 'rel_cov_default_vs_manifold': 0.0001334517326373641,\n", + " 'rel_cov_default_vs_ekf': 0.23647551460143135,\n", + " 'rel_cov_manifold_vs_ekf': 0.23646635627849075,\n", + " 'rel_cov_default_vs_emp': 33.32910001244249,\n", + " 'rel_cov_ekf_vs_emp': 41.45994771378634},\n", + " {'interval_s': np.float64(0.1),\n", + " 'nees_default_mean': 2.787998817033879,\n", + " 'nees_default_std': 1.7413000172544384,\n", + " 'nees_manifold_mean': 2.7878594759967203,\n", + " 'nees_manifold_std': 1.7412603227374222,\n", + " 'nees_ekf_mean': 8.81305571909107,\n", + " 'nees_ekf_std': 3.855761694968445,\n", + " 'rel_cov_default_vs_manifold': 0.0024585853420875346,\n", + " 'rel_cov_default_vs_ekf': 0.5603329657944346,\n", + " 'rel_cov_manifold_vs_ekf': 0.5601135667787372,\n", + " 'rel_cov_default_vs_emp': 8.334023084040808,\n", + " 'rel_cov_ekf_vs_emp': 17.162980630477936},\n", + " {'interval_s': np.float64(0.15),\n", + " 'nees_default_mean': 6.682040247014302,\n", + " 'nees_default_std': 5.0592414606991065,\n", + " 'nees_manifold_mean': 6.6800602346612745,\n", + " 'nees_manifold_std': 5.059707972467027,\n", + " 'nees_ekf_mean': 8.927928820615273,\n", + " 'nees_ekf_std': 4.078586754442932,\n", + " 'rel_cov_default_vs_manifold': 0.01253932711487101,\n", + " 'rel_cov_default_vs_ekf': 0.7432679556418527,\n", + " 'rel_cov_manifold_vs_ekf': 0.7422565879812052,\n", + " 'rel_cov_default_vs_emp': 3.2092286129472383,\n", + " 'rel_cov_ekf_vs_emp': 11.234501032873606},\n", + " {'interval_s': np.float64(0.2),\n", + " 'nees_default_mean': 10.781014031215804,\n", + " 'nees_default_std': 6.9768829696487105,\n", + " 'nees_manifold_mean': 10.759646362466919,\n", + " 'nees_manifold_std': 6.966213923765401,\n", + " 'nees_ekf_mean': 8.775491964313076,\n", + " 'nees_ekf_std': 4.022079758983367,\n", + " 'rel_cov_default_vs_manifold': 0.03784550641194231,\n", + " 'rel_cov_default_vs_ekf': 0.8343955193736587,\n", + " 'rel_cov_manifold_vs_ekf': 0.8317971515524428,\n", + " 'rel_cov_default_vs_emp': 1.8216430889121278,\n", + " 'rel_cov_ekf_vs_emp': 10.24874899697862}]" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "results = []\n", + "for interval_s in intervals:\n", + " data = simulate_interval(interval_s, num_trials, dt, params, scenario, P0, seed=int(1000 * interval_s))\n", + " trials = data[\"trials\"]\n", + "\n", + " nees_default = np.array([nees(t.default.error, t.default.covariance) for t in trials])\n", + " nees_manifold = np.array([nees(t.manifold.error, t.manifold.covariance) for t in trials])\n", + " nees_ekf = np.array([nees(t.ekf.error, t.ekf.covariance) for t in trials])\n", + "\n", + " cov_mean_default = np.mean([t.default.covariance for t in trials], axis=0)\n", + " cov_mean_manifold = np.mean([t.manifold.covariance for t in trials], axis=0)\n", + " cov_mean_ekf = np.mean([t.ekf.covariance for t in trials], axis=0)\n", + "\n", + " emp_cov = np.cov(np.stack([t.default.error for t in trials], axis=0).T, bias=True)\n", + "\n", + " results.append({\n", + " \"interval_s\": interval_s,\n", + " \"nees_default_mean\": float(nees_default.mean()),\n", + " \"nees_default_std\": float(nees_default.std(ddof=0)),\n", + " \"nees_manifold_mean\": float(nees_manifold.mean()),\n", + " \"nees_manifold_std\": float(nees_manifold.std(ddof=0)),\n", + " \"nees_ekf_mean\": float(nees_ekf.mean()),\n", + " \"nees_ekf_std\": float(nees_ekf.std(ddof=0)),\n", + " \"rel_cov_default_vs_manifold\": rel_frobenius(cov_mean_default, cov_mean_manifold),\n", + " \"rel_cov_default_vs_ekf\": rel_frobenius(cov_mean_default, cov_mean_ekf),\n", + " \"rel_cov_manifold_vs_ekf\": rel_frobenius(cov_mean_manifold, cov_mean_ekf),\n", + " \"rel_cov_default_vs_emp\": rel_frobenius(cov_mean_default, emp_cov),\n", + " \"rel_cov_ekf_vs_emp\": rel_frobenius(cov_mean_ekf, emp_cov),\n", + " })\n", + "\n", + "results\n" + ] + }, + { + "cell_type": "markdown", + "id": "b8cb9ee8", + "metadata": {}, + "source": [ + "### Numerical summary\n" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "49441732", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "interval_s | nees_default_mean | nees_manifold_mean | nees_ekf_mean | rel_cov_default_vs_manifold | rel_cov_default_vs_ekf\n", + "-------------------------------------------------------------------------------------------------------------------------------\n", + "0.0500 | 0.7325 | 0.7325 | 8.5786 | 0.0001 | 0.2365 \n", + "0.1000 | 2.7880 | 2.7879 | 8.8131 | 0.0025 | 0.5603 \n", + "0.1500 | 6.6820 | 6.6801 | 8.9279 | 0.0125 | 0.7433 \n", + "0.2000 | 10.7810 | 10.7596 | 8.7755 | 0.0378 | 0.8344 \n" + ] + } + ], + "source": [ + "def print_table(rows):\n", + " headers = [\n", + " \"interval_s\",\n", + " \"nees_default_mean\",\n", + " \"nees_manifold_mean\",\n", + " \"nees_ekf_mean\",\n", + " \"rel_cov_default_vs_manifold\",\n", + " \"rel_cov_default_vs_ekf\",\n", + " ]\n", + " widths = {h: max(len(h), 14) for h in headers}\n", + " for row in rows:\n", + " for h in headers:\n", + " widths[h] = max(widths[h], len(f\"{row[h]:.4f}\") if isinstance(row[h], float) else len(str(row[h])))\n", + "\n", + " header_line = \" | \".join(f\"{h:<{widths[h]}}\" for h in headers)\n", + " print(header_line)\n", + " print(\"-\" * len(header_line))\n", + "\n", + " for row in rows:\n", + " line_parts = []\n", + " for h in headers:\n", + " val = row[h]\n", + " if isinstance(val, float):\n", + " line_parts.append(f\"{val:<{widths[h]}.4f}\")\n", + " else:\n", + " line_parts.append(f\"{val:<{widths[h]}}\")\n", + " print(\" | \".join(line_parts))\n", + "\n", + "print_table(results)\n" + ] + }, + { + "cell_type": "markdown", + "id": "2dfa05a9", + "metadata": {}, + "source": [ + "### NEES vs interval\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "731acc00", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "mode": "lines+markers", + "name": "PIM default", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 0.732541283415913, + 2.787998817033879, + 6.682040247014302, + 10.781014031215804 + ] + }, + { + "mode": "lines+markers", + "name": "PIM manifold", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 0.7325417437759313, + 2.7878594759967203, + 6.6800602346612745, + 10.759646362466919 + ] + }, + { + "mode": "lines+markers", + "name": "NavStateImuEKF", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 8.578577369813082, + 8.81305571909107, + 8.927928820615273, + 8.775491964313076 + ] + }, + { + "line": { + "color": "black", + "dash": "dash" + }, + "mode": "lines", + "name": "Expected NEES (dim)", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 9, + 9, + 9, + 9 + ] + } + ], + "layout": { + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Mean NEES vs preintegration interval" + }, + "xaxis": { + "title": { + "text": "Interval (ms)" + } + }, + "yaxis": { + "title": { + "text": "Mean NEES" + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "dim = 9\n", + "interval_ms = [int(1000 * r[\"interval_s\"]) for r in results]\n", + "\n", + "fig = go.Figure()\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"nees_default_mean\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"PIM default\",\n", + ")\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"nees_manifold_mean\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"PIM manifold\",\n", + ")\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"nees_ekf_mean\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"NavStateImuEKF\",\n", + ")\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[dim] * len(results),\n", + " mode=\"lines\",\n", + " name=\"Expected NEES (dim)\",\n", + " line=dict(color=\"black\", dash=\"dash\"),\n", + ")\n", + "fig.update_layout(\n", + " title=\"Mean NEES vs preintegration interval\",\n", + " xaxis_title=\"Interval (ms)\",\n", + " yaxis_title=\"Mean NEES\",\n", + ")\n", + "fig.show()\n" + ] + }, + { + "cell_type": "markdown", + "id": "f256debd", + "metadata": {}, + "source": [ + "### Covariance consistency metrics\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "c3089888", + "metadata": {}, + "outputs": [ + { + "data": { + "application/vnd.plotly.v1+json": { + "config": { + "plotlyServerURL": "https://plot.ly" + }, + "data": [ + { + "mode": "lines+markers", + "name": "Default vs Manifold", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 0.0001334517326373641, + 0.0024585853420875346, + 0.01253932711487101, + 0.03784550641194231 + ] + }, + { + "mode": "lines+markers", + "name": "Default vs EKF", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 0.23647551460143135, + 0.5603329657944346, + 0.7432679556418527, + 0.8343955193736587 + ] + }, + { + "mode": "lines+markers", + "name": "Manifold vs EKF", + "type": "scatter", + "x": [ + 50, + 100, + 150, + 200 + ], + "y": [ + 0.23646635627849075, + 0.5601135667787372, + 0.7422565879812052, + 0.8317971515524428 + ] + } + ], + "layout": { + "template": { + "data": { + "bar": [ + { + "error_x": { + "color": "#2a3f5f" + }, + "error_y": { + "color": "#2a3f5f" + }, + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "bar" + } + ], + "barpolar": [ + { + "marker": { + "line": { + "color": "#E5ECF6", + "width": 0.5 + }, + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "barpolar" + } + ], + "carpet": [ + { + "aaxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "baxis": { + "endlinecolor": "#2a3f5f", + "gridcolor": "white", + "linecolor": "white", + "minorgridcolor": "white", + "startlinecolor": "#2a3f5f" + }, + "type": "carpet" + } + ], + "choropleth": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "choropleth" + } + ], + "contour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "contour" + } + ], + "contourcarpet": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "contourcarpet" + } + ], + "heatmap": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "heatmap" + } + ], + "histogram": [ + { + "marker": { + "pattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + } + }, + "type": "histogram" + } + ], + "histogram2d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2d" + } + ], + "histogram2dcontour": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "histogram2dcontour" + } + ], + "mesh3d": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "type": "mesh3d" + } + ], + "parcoords": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "parcoords" + } + ], + "pie": [ + { + "automargin": true, + "type": "pie" + } + ], + "scatter": [ + { + "fillpattern": { + "fillmode": "overlay", + "size": 10, + "solidity": 0.2 + }, + "type": "scatter" + } + ], + "scatter3d": [ + { + "line": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatter3d" + } + ], + "scattercarpet": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattercarpet" + } + ], + "scattergeo": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergeo" + } + ], + "scattergl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattergl" + } + ], + "scattermap": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermap" + } + ], + "scattermapbox": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scattermapbox" + } + ], + "scatterpolar": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolar" + } + ], + "scatterpolargl": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterpolargl" + } + ], + "scatterternary": [ + { + "marker": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "type": "scatterternary" + } + ], + "surface": [ + { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + }, + "colorscale": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "type": "surface" + } + ], + "table": [ + { + "cells": { + "fill": { + "color": "#EBF0F8" + }, + "line": { + "color": "white" + } + }, + "header": { + "fill": { + "color": "#C8D4E3" + }, + "line": { + "color": "white" + } + }, + "type": "table" + } + ] + }, + "layout": { + "annotationdefaults": { + "arrowcolor": "#2a3f5f", + "arrowhead": 0, + "arrowwidth": 1 + }, + "autotypenumbers": "strict", + "coloraxis": { + "colorbar": { + "outlinewidth": 0, + "ticks": "" + } + }, + "colorscale": { + "diverging": [ + [ + 0, + "#8e0152" + ], + [ + 0.1, + "#c51b7d" + ], + [ + 0.2, + "#de77ae" + ], + [ + 0.3, + "#f1b6da" + ], + [ + 0.4, + "#fde0ef" + ], + [ + 0.5, + "#f7f7f7" + ], + [ + 0.6, + "#e6f5d0" + ], + [ + 0.7, + "#b8e186" + ], + [ + 0.8, + "#7fbc41" + ], + [ + 0.9, + "#4d9221" + ], + [ + 1, + "#276419" + ] + ], + "sequential": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ], + "sequentialminus": [ + [ + 0, + "#0d0887" + ], + [ + 0.1111111111111111, + "#46039f" + ], + [ + 0.2222222222222222, + "#7201a8" + ], + [ + 0.3333333333333333, + "#9c179e" + ], + [ + 0.4444444444444444, + "#bd3786" + ], + [ + 0.5555555555555556, + "#d8576b" + ], + [ + 0.6666666666666666, + "#ed7953" + ], + [ + 0.7777777777777778, + "#fb9f3a" + ], + [ + 0.8888888888888888, + "#fdca26" + ], + [ + 1, + "#f0f921" + ] + ] + }, + "colorway": [ + "#636efa", + "#EF553B", + "#00cc96", + "#ab63fa", + "#FFA15A", + "#19d3f3", + "#FF6692", + "#B6E880", + "#FF97FF", + "#FECB52" + ], + "font": { + "color": "#2a3f5f" + }, + "geo": { + "bgcolor": "white", + "lakecolor": "white", + "landcolor": "#E5ECF6", + "showlakes": true, + "showland": true, + "subunitcolor": "white" + }, + "hoverlabel": { + "align": "left" + }, + "hovermode": "closest", + "mapbox": { + "style": "light" + }, + "paper_bgcolor": "white", + "plot_bgcolor": "#E5ECF6", + "polar": { + "angularaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "radialaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "scene": { + "xaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "yaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + }, + "zaxis": { + "backgroundcolor": "#E5ECF6", + "gridcolor": "white", + "gridwidth": 2, + "linecolor": "white", + "showbackground": true, + "ticks": "", + "zerolinecolor": "white" + } + }, + "shapedefaults": { + "line": { + "color": "#2a3f5f" + } + }, + "ternary": { + "aaxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "baxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + }, + "bgcolor": "#E5ECF6", + "caxis": { + "gridcolor": "white", + "linecolor": "white", + "ticks": "" + } + }, + "title": { + "x": 0.05 + }, + "xaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + }, + "yaxis": { + "automargin": true, + "gridcolor": "white", + "linecolor": "white", + "ticks": "", + "title": { + "standoff": 15 + }, + "zerolinecolor": "white", + "zerolinewidth": 2 + } + } + }, + "title": { + "text": "Relative covariance differences" + }, + "xaxis": { + "title": { + "text": "Interval (ms)" + } + }, + "yaxis": { + "title": { + "text": "Relative Frobenius norm" + } + } + } + } + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "fig = go.Figure()\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"rel_cov_default_vs_manifold\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"Default vs Manifold\",\n", + ")\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"rel_cov_default_vs_ekf\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"Default vs EKF\",\n", + ")\n", + "fig.add_scatter(\n", + " x=interval_ms,\n", + " y=[r[\"rel_cov_manifold_vs_ekf\"] for r in results],\n", + " mode=\"lines+markers\",\n", + " name=\"Manifold vs EKF\",\n", + ")\n", + "fig.update_layout(\n", + " title=\"Relative covariance differences\",\n", + " xaxis_title=\"Interval (ms)\",\n", + " yaxis_title=\"Relative Frobenius norm\",\n", + ")\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Interpretation\n", + "\n", + "- The NEES curves should sit near 9 if the covariance is consistent.\n", + "- The two PIM implementations should produce nearly identical covariance and NEES.\n", + "- The EKF predict covariance should match the PIM covariance for the same IMU data.\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py312", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.12.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +}