feat: cold start (initial roll and pitch calibration) (#94)

Author: vegard-solum-4ss

docs/user_guide/quickstart.rst

@@ -14,42 +14,15 @@ long-term stable aiding measurements to ensure convergence and stability of the
 The aiding measurements are typically provided by a `global navigation satellite system`
 (GNSS) and a compass, providing absolute position, velocity, and heading information.
 
-In scenarios where only compass aiding (but no GNSS) is available, the INS cannot provide
-reliable position and velocity information but still deliver stable attitude estimates.
-When the AINS is operated in this mode, we call it an `Attitude and Heading Reference System`
-(AHRS).
-
-In aiding-denied scenarios, where no aiding measurements are available, the INS
-must rely solely on the IMU's measurements to estimate the body's motion. In such
-scenarios, only the roll and pitch degrees of freedom are observable, as they can
-still be corrected using the IMU's accelerometer data and the known direction of
-the gravitational field. When operated in this mode, the AINS is referred to as
-a `Vertical Reference Unit` (VRU).
-
-``smsfusion`` provides Python implementations of a few INS algorithms, including:
-
-* :class:`~smsfusion.AidedINS`: Aided INS (AINS) algorithm. Used to estimate position,
-  velocity and attitude (PVA) using IMU data, GNSS data and compass data.
-* :class:`~smsfusion.AHRS`: AHRS wrapper around :class:`~smsfusion.AidedINS` with sane defaults.
-  Used to estimate attitude only using IMU data and compass data.
-* :class:`~smsfusion.VRU`: VRU wrapper around :class:`~smsfusion.AidedINS` with sane defaults.
-  Used to estimate roll and pitch only using IMU data.
-* :class:`~smsfusion.StrapdownINS`: Simple strapdown INS algorithm, where the
-  IMU measurements are integrated without incorporating any additional aiding measurements.
-  The state estimates will therefore drift over time and quickly diverge from their true values.
-  This class is primarily used for PVA propagation in other aided INS algorithms.
-
-All AINS algorithms in ``smsfusion`` are based on a fusion filtering technique known
-as the `multiplicative extended Kalman filter` (MEKF).
-
-In this quickstart guide, we will demonstrate how to use the AINS algorithms
-available in ``smsfusion`` to estimate PVA of a moving body using IMU measurements
-and aiding measurements.
+``smsfusion`` provides Python implementations of a few AINS algorithms, including
+:class:`~smsfusion.AidedINS`, :class:`~smsfusion.AHRS` and :class:`~smsfusion.VRU`.
+In this quickstart guide we will demonstrate how to use these AINS algorithms to
+estimate PVA of a moving body using IMU measurements and aiding measurements.
 
 Measurement data
 ----------------
 This quickstart guide assumes that you have access to accelerometer and gyroscope
-data from an IMU sensor, and maybe position and heading data from other aiding
+data from an IMU sensor, and ideally position and heading data from other aiding
 sensors. If you do not have access to such data, you can generate synthetic
 measurements using the code provided here.
 
@@ -108,8 +81,26 @@ For simpler cases where only compass or no aiding is available, consider using
 :func:`~smsfusion.benchmark.benchmark_pure_attitude_beat_202311A` instead to
 generate synthetic data.
 
-Aided INS: Estimate position, velocity and attitude (PVA)
----------------------------------------------------------
+INS algorithms
+--------------
+The following INS algorithms are provided by ``smsfusion``:
+
+* :class:`~smsfusion.AidedINS`: Aided INS (AINS) algorithm. Used to estimate position,
+  velocity and attitude (PVA) using IMU data, GNSS data and compass data.
+* :class:`~smsfusion.AHRS`: AHRS wrapper around :class:`~smsfusion.AidedINS` with sane defaults.
+  Used to estimate attitude only using IMU data and compass data.
+* :class:`~smsfusion.VRU`: VRU wrapper around :class:`~smsfusion.AidedINS` with sane defaults.
+  Used to estimate roll and pitch only using IMU data.
+* :class:`~smsfusion.StrapdownINS`: Simple strapdown INS algorithm, where the
+  IMU measurements are integrated without incorporating any additional aiding measurements.
+  The state estimates will therefore drift over time and quickly diverge from their true values.
+  This class is primarily used for PVA propagation in other aided INS algorithms.
+
+All AINS algorithms provided by ``smsfusion`` are based on a fusion filtering technique
+known as the `multiplicative extended Kalman filter` (MEKF).
+
+AidedINS - IMU + heading and position aiding
+............................................
 If you have access to accelerometer and gyroscope data from an IMU sensor, as well
 as position and heading data from other aiding sensors, you can estimate the position,
 velocity and attitude (PVA) of a moving body using the :func:`~smsfusion.AidedINS` class:
@@ -120,16 +111,9 @@ velocity and attitude (PVA) of a moving body using the :func:`~smsfusion.AidedIN
     import smsfusion as sf
 
 
-    # Initial (a priori) state
-    p0 = pos[0]  # position [m]
-    v0 = vel[0]  # velocity [m/s]
-    q0 = sf.quaternion_from_euler(euler[0], degrees=False)  # attitude as unit quaternion
-    ba0 = np.zeros(3)  # accelerometer bias [m/s^2]
-    bg0 = np.zeros(3)  # gyroscope bias [rad/s]
-    x0 = np.concatenate((p0, v0, q0, ba0, bg0))
-
     # Initialize AINS
-    ains = sf.AidedINS(fs, x0)
+    fs = 10.24  # sampling rate in Hz
+    ains = sf.AidedINS(fs)
 
     # Estimate PVA states sequentially using AINS
     pos_est, vel_est, euler_est = [], [], []
@@ -152,8 +136,13 @@ velocity and attitude (PVA) of a moving body using the :func:`~smsfusion.AidedIN
     vel_est = np.array(vel_est)
     euler_est = np.array(euler_est)
 
-AHRS: Estimate attitude with compass-aiding
--------------------------------------------
+AHRS - IMU + heading aiding
+...........................
+In scenarios where only compass aiding is available (i.e., no GNSS), the INS is
+unable to provide reliable position and velocity information, but it can still
+deliver stable attitude estimates. When the AINS is operated in this mode, we call
+it an `Attitude and Heading Reference System` (AHRS).
+
 To limit integration drift in AHRS mode, we must assume that the sensor on average
 is stationary. The static assumtion is incorporated as so-called pseudo aiding measurements
 of zero with corresponding error variances. For most applications, the following pseudo
@@ -163,25 +152,18 @@ aiding is sufficient:
 * Velocity: 0 m/s with 10 m/s standard deviation
 
 If you have access to accelerometer and gyroscope data from an IMU sensor and
-compass measurements, you can estimate the attitude of a moving body using
-the :func:`~smsfusion.AHRS` class:
+heading measurements from a compass, you can estimate the attitude of a moving body
+using the :func:`~smsfusion.AHRS` class:
 
 .. code-block:: python
 
     import numpy as np
     import smsfusion as sf
 
 
-    # Initial (a priori) state
-    p0 = np.zeros(3)  # position [m]
-    v0 = np.zeros(3)  # velocity [m/s]
-    q0 = sf.quaternion_from_euler(euler[0], degrees=False)  # attitude as unit quaternion
-    ba0 = np.zeros(3)  # accelerometer bias [m/s^2]
-    bg0 = np.zeros(3)  # gyroscope bias [rad/s]
-    x0 = np.concatenate((p0, v0, q0, ba0, bg0))
-
     # Initialize AHRS
-    ahrs = sf.AHRS(fs, x0)
+    fs = 10.24  # sampling rate in Hz
+    ahrs = sf.AHRS(fs)
 
     # Estimate attitude sequentially using AHRS
     euler_est = []
@@ -198,8 +180,15 @@ the :func:`~smsfusion.AHRS` class:
 
     euler_est = np.array(euler_est)
 
-VRU: Estimate partial attitude in aiding-denied scenarios
----------------------------------------------------------
+VRU - IMU only (aiding-denied)
+..............................
+In aiding-denied scenarios, where no aiding measurements are available, the INS
+must rely solely on the IMU's measurements to estimate the body's motion. In such
+scenarios only the roll and pitch degrees of freedom are observable, as they can
+still be corrected using the IMU's accelerometer data and the known direction of
+the gravitational field. When operated in this mode, the AINS is referred to as
+a `Vertical Reference Unit` (VRU).
+
 To limit integration drift in VRU mode, we must assume that the sensor on average
 is stationary. The static assumption is incorporated as so-called pseudo aiding measurements
 of zero with corresponding error variances. For most applications, the following pseudo
@@ -221,25 +210,14 @@ estimate the roll and pitch degrees of freedom of a moving body using the
     import smsfusion as sf
 
 
-    # Initial (a priori) state
-    p0 = np.zeros(3)  # position [m]
-    v0 = np.zeros(3)  # velocity [m/s]
-    q0 = sf.quaternion_from_euler(euler[0], degrees=False)  # attitude as unit quaternion
-    ba0 = np.zeros(3)  # accelerometer bias [m/s^2]
-    bg0 = np.zeros(3)  # gyroscope bias [rad/s]
-    x0 = np.concatenate((p0, v0, q0, ba0, bg0))
-
     # Initialize VRU
-    vru = sf.VRU(fs, x0)
+    fs = 10.24  # sampling rate in Hz
+    vru = sf.VRU(fs)
 
     # Estimate roll and pitch sequentially using VRU
     roll_pitch_est = []
     for f_i, w_i in zip(acc_imu, gyro_imu):
-        vru.update(
-            f_i,
-            w_i,
-            degrees=False
-        )
+        vru.update(f_i, w_i, degrees=False)
         roll_pitch_est.append(vru.euler(degrees=False)[:2])
 
     roll_pitch_est = np.array(roll_pitch_est)
@@ -249,17 +227,17 @@ Smoothing
 ---------
 Smoothing refers to post-processing techniques that enhance the accuracy of a Kalman
 filter's state and covariance estimates by incorporating both past and future measurements.
-In contrast, standard forward filtering (as implemented in :class:`~smsfusion.AidedINS`)
-relies only on past and current measurements, leading to suboptimal estimates when
-future data is available.
+In contrast, standard forward filtering (as provided by :class:`~smsfusion.AidedINS`,
+:class:`~smsfusion.AHRS` and :class:`~smsfusion.VRU`) relies only on past and current
+measurements, leading to suboptimal estimates when future data is available.
 
 Fixed-interval smoothing
 ........................
 The :class:`~smsfusion.FixedIntervalSmoother` class implements fixed-interval smoothing
 for an :class:`~smsfusion.AidedINS` instance or one of its subclasses (:class:`~smsfusion.AHRS`
-or :class:`~smsfusion.VRU`). After a complete forward pass using the AINS algorithm,
-a backward sweep with a smoothing algorithm is performed to refine the state
-and covariance estimates. Fixed-interval smoothing is particularly useful
+or :class:`~smsfusion.VRU`). After a complete forward pass using the given AINS
+algorithm, a backward sweep with a smoothing algorithm is performed to refine the
+state and covariance estimates. Fixed-interval smoothing is particularly useful
 when the entire measurement sequence is available, as it allows for optimal state
 estimation by considering all measurements in the sequence.
 
@@ -274,13 +252,13 @@ additional aiding parameters depending on the type of AINS instance used.
     import smsfusion as sf
 
 
-    vru_smoother = sf.FixedIntervalSmoother(vru)
+    # Initialize VRU-based fixed-interval smoother
+    fs = 10.24  # sampling rate in Hz
+    smoother = sf.FixedIntervalSmoother(sf.VRU(fs))
 
+    # Update with accelerometer and gyroscope measurements
     for f_i, w_i in zip(acc_imu, gyro_imu):
-        vru_smoother.update(
-            f_i,
-            w_i,
-            degrees=False
-        )
+        smoother.update(f_i, w_i, degrees=False)
 
-    roll_pitch_est = vru_smoother.euler(degrees=False)[:, :2]
+    # Get smoothed roll and pitch estimates
+    roll_pitch_est = smoother.euler(degrees=False)[:, :2]