Add heading pseudo aiding to VRU to ensure convergence of all gyro biases

docs/user_guide/quickstart.rst

@@ -196,9 +196,7 @@ aiding is sufficient:
 
 * Position: 0 m with 1000 m standard deviation
 * Velocity: 0 m/s with 10 m/s standard deviation
-
-Note that the heading is not corrected in VRU mode, and the yaw degree of freedom
-will thus drift arbitrarily.
+* Heading: 0 rad with 1 rad standard deviation
 
 If you have access to accelerometer and gyroscope data from an IMU sensor, you can
 estimate the roll and pitch degrees of freedom of a moving body using the
@@ -218,9 +216,9 @@ estimate the roll and pitch degrees of freedom of a moving body using the
     roll_pitch_est = []
     for f_i, w_i in zip(acc_imu, gyro_imu):
         vru.update(f_i, w_i, degrees=False)
-        roll_pitch_est.append(vru.euler(degrees=False)[:2])
+        euler_est.append(vru.euler(degrees=False))
 
-    roll_pitch_est = np.array(roll_pitch_est)
+    euler_est = np.array(euler_est)
 
 
 Smoothing
@@ -261,4 +259,4 @@ additional aiding parameters depending on the type of AINS instance used.
         smoother.update(f_i, w_i, degrees=False)
 
     # Get smoothed roll and pitch estimates
-    roll_pitch_est = smoother.euler(degrees=False)[:, :2]
+    euler_est = smoother.euler(degrees=False)

src/smsfusion/_ins.py

@@ -1297,11 +1297,12 @@ def update(
         degrees: bool = False,
         pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]),
         vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]),
+        head_var: float = 1.0,
     ) -> Self:
         """
         Update/correct the VRU's state estimate with pseudo aiding measurements
-        (i.e., zero velocity and zero position with corresponding variances), and
-        project ahead using IMU measurements.
+        (i.e., zero velocity, zero position and zero heading with corresponding
+        variances), and project ahead using IMU measurements.
 
         Parameters
         ----------
@@ -1316,11 +1317,17 @@ def update(
         degrees : bool, default False
             Specifies whether the unit of ``w_imu`` are in degrees or radians.
         pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
-            Variance of position measurement noise in m^2. Defaults to
+            Variance of position measurement noise in m^2. Defaults to a
             standard deviation of 1000 m, while assuming zero position.
         vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
-            Variance of velocity measurement noise in (m/s)^2. Defaults to
+            Variance of velocity measurement noise in (m/s)^2. Defaults to a
             standard deviation of 10 m/s, while assuming zero velocity.
+        head_var : float, default 1.0
+            Variance of heading measurement noise in (rad)^2. Defaults to a
+            standard deviation of 1.0 rad, while assuming zero heading. Lower
+            values can speed up convergence of the gyroscope biases; however, a
+            too small value may be detrimental to the roll and pitch estimates
+            if the sensor experiences significant yaw motion.
 
         Returns
         -------
@@ -1335,8 +1342,9 @@ def update(
             pos_var=pos_var,
             vel=np.array([0.0, 0.0, 0.0]),
             vel_var=vel_var,
-            head=None,
-            head_var=None,
+            head=0.0,
+            head_var=head_var,
+            head_degrees=False,
         )
 
 
@@ -1477,10 +1485,10 @@ def update(
             Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2,
             or radians and radians^2. Default is in radians and radians^2.
         pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6]
-            Variance of position measurement noise in m^2. Defaults to
+            Variance of position measurement noise in m^2. Defaults to a
             standard deviation of 1000 m, while assuming zero position.
         vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2]
-            Variance of velocity measurement noise in (m/s)^2. Defaults to
+            Variance of velocity measurement noise in (m/s)^2. Defaults to a
             standard deviation of 10 m/s, while assuming zero velocity.
 
         Returns