Add docs for AHRS-related classes

gtsam/navigation/AHRSFactor.h

@@ -29,7 +29,7 @@
 namespace gtsam {
 
 /**
- * PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope
+ * PreintegratedAHRSMeasurements accumulates (integrates) the gyroscope
  * measurements (rotation rates) and the corresponding covariance matrix.
  * Can be built incrementally so as to avoid costly integration at time of factor construction.
  */
@@ -49,7 +49,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
   /**
    *  Default constructor, initialize with no measurements
-   *  @param bias Current estimate of acceleration and rotation rate biases
+   *  @param bias Current estimate of rotation rate biases
    */
   PreintegratedAhrsMeasurements(const std::shared_ptr<Params>& p,
       const Vector3& biasHat) :
@@ -60,7 +60,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
   /**
    *  Non-Default constructor, initialize with measurements
    *  @param p: Parameters for AHRS pre-integration
-   *  @param bias_hat: Current estimate of acceleration and rotation rate biases
+   *  @param bias_hat: Current estimate of rotation rate biases
    *  @param deltaTij: Delta time in pre-integration
    *  @param deltaRij: Delta rotation in pre-integration
    *  @param delRdelBiasOmega: Jacobian of rotation wrt. to gyro bias
@@ -87,11 +87,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
   /// equals
   bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
 
-  /// Reset inetgrated quantities to zero
+  /// Reset integrated quantities to zero
   void resetIntegration();
 
   /**
-   * Add a single Gyroscope measurement to the preintegration.
+   * Add a single gyroscope measurement to the preintegration.
    * Measurements are taken to be in the sensor
    * frame and conversion to the body frame is handled by `body_P_sensor` in
    * `PreintegratedRotationParams` (if provided).

gtsam/navigation/doc/AHRSFactor.ipynb

@@ -0,0 +1,92 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# AHRSFactor\n",
+    "\n",
+    "## Overview\n",
+    "\n",
+    "The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n",
+    "\n",
+    "The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Mathematical Formulation\n",
+    "\n",
+    "The `AHRSFactor` relies on the use of `PreintegratedRotation`, which applies successive exponential maps to each individual gyroscope measurement $\\omega$ over the interval $[t_i, t_j]$. In this approach, every measurement contributes a small rotation given by $\\exp(\\omega_k \\Delta t)$, and the overall preintegrated rotation is obtained by composing these incremental rotations:\n",
+    "$$\n",
+    "\\Delta R_{ij}^{meas} = \\prod_{k} \\exp(\\omega_k \\Delta t)\n",
+    "$$\n",
+    "\n",
+    "Given two estimated rotations $R_i$ at time $t_i$ and $R_j$ at time $t_j$, the factor enforces:\n",
+    "$$\n",
+    "R_j \\approx R_i \\cdot \\Delta R_{ij}^{meas}\n",
+    "$$\n",
+    "\n",
+    "The error term optimized by the factor graph is the rotational discrepancy captured by the logarithmic map:\n",
+    "$$\n",
+    "e = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
+    "$$\n",
+    "\n",
+    "Note: the reality is a bit more complicated, because the code also takes into account the effects of bias, and if desired, coriolis forces."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Key Functionality\n",
+    "\n",
+    "### Constructor\n",
+    "\n",
+    "The constructor initializes the factor using a preintegrated AHRS measurement object, relating orientation states at discrete time instances $t_i$ and $t_j$.\n",
+    "\n",
+    "### Core Methods\n",
+    "\n",
+    "- `evaluateError`: calculates the error between two estimated orientations at times $t_i$ and $t_j$, relative to the preintegrated gyroscopic measurements. The error is computed as:\n",
+    "\n",
+    "  $$\n",
+    "  \\text{error} = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
+    "  $$\n",
+    "\n",
+    "  Here:\n",
+    "\n",
+    "  - $R_i, R_j$ are the estimated rotation matrices at times $t_i$ and $t_j$.\n",
+    "  - $\\Delta R_{ij}^{meas}$ is the rotation matrix obtained by integrating gyroscope measurements from $t_i$ to $t_j$.\n",
+    "  - $\\text{Log}(\\cdot)$ is the logarithmic map from $SO(3)$ to $\\mathbb{R}^3$.\n",
+    "\n",
+    "  The resulting 3-dimensional error vector represents the rotational discrepancy.\n",
+    "\n",
+    "- `print`: outputs a readable representation of the internal state of the factor, including the associated time steps and preintegrated measurements, aiding debugging and verification.\n",
+    "\n",
+    "- `equals` determines if another `AHRSFactor` instance is identical, useful in testing scenarios or when verifying the correctness of factor graph constructions."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Typical Use Case\n",
+    "\n",
+    "`AHRSFactor` is typically applied within factor-graph-based orientation estimators where orientation states are discretized in time. Its primary role is to incorporate gyroscope information in a computationally efficient and numerically stable way, especially useful in IMU-centric navigation scenarios like:\n",
+    "\n",
+    "- Robotics and autonomous vehicles\n",
+    "- UAV navigation systems\n",
+    "- Motion capture and tracking systems"
+   ]
+  }
+ ],
+ "metadata": {
+  "language_info": {
+   "name": "python"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}