delete volocity project ahead function

Author: vegard-solum-4ss

src/smsfusion/_ins_v2.py

@@ -8,7 +8,6 @@
 from ._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion
 from ._vectorops import _normalize, _skew_symmetric
 
-
 P0 = (
     (1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
     (0.0, 1.0e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
@@ -22,7 +21,6 @@
 )
 
 
-
 @njit  # type: ignore[misc]
 def _update_quaternion_with_gibbs2(
     q: NDArray[np.float64], da: NDArray[np.float64]
@@ -247,26 +245,6 @@ def _project_covariance_ahead(
     P[:, :] = phi @ P @ phi.T + Q
 
 
-@njit  # type: ignore[misc]
-def _project_velocity_ahead(dvel, v_n, R_nb, dvel_g_corr):
-    """
-    Project velocity estimate ahead (in place).
-
-    Parameters
-    ----------
-    dvel : ndarray, shape (3,)
-        Velocity increment measurement (sculling integral).
-    v_n : ndarray, shape (3,)
-        Current velocity estimate expressed in the navigation frame (projected ahead
-        in place).
-    R_nb : ndarray, shape (3, 3)
-        Current rotation matrix from body to navigation frame.
-    dvel_g_corr : ndarray, shape (3,)
-        Gravity correction.
-    """
-    v_n[:] += R_nb @ dvel + dvel_g_corr
-
-
 def _state_transition_matrix(
     dt: float,
     dvel: NDArray[np.float64],
@@ -677,7 +655,7 @@ def update(
         _update_state_transition_matrix(self._phi, dvel, dtheta, R_nb)
 
         # Project (a priori) state estimates ahead
-        _project_velocity_ahead(dvel, self._v_n, R_nb, self._dvel_g_corr)
+        self._v_n[:] += R_nb @ dvel + self._dvel_g_corr  # TODO: speed-up with njit?
         _update_quaternion_with_rotvec(self._q_nb, dtheta)
 
         # Project (a priori) error covariance matrix estimate ahead