ains

Author: ali-cetin-4ss

src/smsfusion/_ins/_ahrs.py

@@ -229,7 +229,7 @@ class AHRS:
     ----------
     fs : float
         Sampling rate in Hz.
-    v : array_like, shape (3,), optional
+    vel : array_like, shape (3,), optional
         Initial velocity estimate in m/s. Defaults to zero velocity (stationary).
     q : Attitude or array_like, shape (4,), optional
         Initial attitude estimate as a unit quaternion (qw, qx, qy, qz). Defaults
@@ -263,7 +263,7 @@ class AHRS:
     def __init__(
         self,
         fs: float,
-        v: ArrayLike = (0.0, 0.0, 0.0),
+        vel: ArrayLike = (0.0, 0.0, 0.0),
         q: ArrayLike = (1.0, 0.0, 0.0, 0.0),
         bg: ArrayLike = (0.0, 0.0, 0.0),
         P: ArrayLike = P0,
@@ -288,7 +288,7 @@ def __init__(
         self._gbc = gyro_bias_corr_time  # gyro bias correlation time
 
         # State and covariance estimates
-        self._v_n = np.asarray_chkfinite(v).reshape(3).copy()
+        self._v_n = np.asarray_chkfinite(vel).reshape(3).copy()
         self._q_nb = np.asarray_chkfinite(q).reshape(4).copy()
         self._bg_b = np.asarray_chkfinite(bg).reshape(3).copy()
         self._P = np.asarray_chkfinite(P).reshape(9, 9).copy()
@@ -307,18 +307,18 @@ def __init__(
         )
         self._H = _measurement_matrix_init(self._q_nb)
 
-    def quaternion(self) -> NDArray[np.float64]:
-        """
-        Attitude expressed as a unit quaternion.
-        """
-        return self._q_nb.copy()
-
     def velocity(self) -> NDArray[np.float64]:
         """
         Velocity expressed in the navigation frame.
         """
         return self._v_n.copy()
 
+    def quaternion(self) -> NDArray[np.float64]:
+        """
+        Attitude expressed as a unit quaternion.
+        """
+        return self._q_nb.copy()
+
     def euler(self, degrees: bool = False) -> NDArray[np.float64]:
         """
         Attitude expressed as Euler angles (roll, pitch, yaw).

src/smsfusion/_ins/_aiding.py

@@ -13,6 +13,33 @@
 )
 
 
+@njit  # type: ignore[misc]
+def _aiding_update_pos(
+    dx: NDArray[np.float64],
+    P: NDArray[np.float64],
+    H: NDArray[np.float64],
+    pos_n: NDArray[np.float64],
+    pos_meas: NDArray[np.float64],
+    pos_var: NDArray[np.float64],
+    R_nb: NDArray[np.float64],
+    lever_arm: NDArray[np.float64],
+) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+    """
+    Update with position aiding measurement.
+    """
+
+    if pos_var is None:
+        raise ValueError("'pos_var' not provided.")
+
+    if not lever_arm.any():
+        dz = pos_meas - (pos_n + R_nb @ lever_arm)
+    else:
+        dz = pos_meas - pos_n
+
+    dx, P = _kalman_update_sequential(dx, P, dz, pos_var, H)
+    return dx, P
+
+
 @njit  # type: ignore[misc]
 def _aiding_update_vel(
     dx: NDArray[np.float64],