diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index 76091773..a80d3a84 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -4,6 +4,7 @@ AMEKF, PVAMEKF, VAMEKF, + FixedIntervalSmoother, FixedNED, gravity, ) @@ -15,6 +16,7 @@ "VAMEKF", "ConingScullingAlg", "ConingScullingAlgCalibrated", + "FixedIntervalSmoother", "FixedNED", "benchmark", "calibrate", diff --git a/src/smsfusion/_ins/__init__.py b/src/smsfusion/_ins/__init__.py index 7338061a..ac2688b4 100644 --- a/src/smsfusion/_ins/__init__.py +++ b/src/smsfusion/_ins/__init__.py @@ -1,5 +1,6 @@ from ._amekf import AMEKF from ._pvamekf import PVAMEKF +from ._smoothing import FixedIntervalSmoother from ._utils import FixedNED, euler_from_acc, gravity from ._vamekf import VAMEKF @@ -7,6 +8,7 @@ "AMEKF", "PVAMEKF", "VAMEKF", + "FixedIntervalSmoother", "FixedNED", "euler_from_acc", "gravity", diff --git a/src/smsfusion/_ins/_amekf.py b/src/smsfusion/_ins/_amekf.py index bce2d019..cdb0e006 100644 --- a/src/smsfusion/_ins/_amekf.py +++ b/src/smsfusion/_ins/_amekf.py @@ -328,8 +328,8 @@ def update( A reference to the instance itself after the update. """ - dvel = np.asarray(dvel) - dtheta = np.asarray(dtheta) + dvel = np.asarray(dvel).reshape(3) + dtheta = np.asarray(dtheta).reshape(3) if degrees: dtheta = (np.pi / 180.0) * dtheta diff --git a/src/smsfusion/_ins/_pvamekf.py b/src/smsfusion/_ins/_pvamekf.py index 5f3ed7bd..ece3231c 100644 --- a/src/smsfusion/_ins/_pvamekf.py +++ b/src/smsfusion/_ins/_pvamekf.py @@ -209,7 +209,7 @@ def _reset( Attitude state estimate parameterized as a unit quaternion to be reset in place. bg_b : ndarray, shape (3,) Gyroscope bias state estimate to be reset in place. - dx : ndarray, shape (9,) + dx : ndarray, shape (12,) Error state vector containing the corrections to be applied to the state estimates. Will be reset to zero after applying the corrections. """ @@ -327,6 +327,9 @@ def __init__( self._bg_b = np.asarray_chkfinite(bg0).reshape(3).copy() self._P = np.asarray_chkfinite(P0).reshape(12, 12).copy() self._dx = np.zeros(12) + self._dx_before_reset = np.zeros(12) + self._dvel = np.zeros(3) + self._dtheta = np.zeros(3) # Discrete state-space model self._phi = _state_transition_matrix_init( @@ -473,17 +476,19 @@ def update( A reference to the instance itself after the update. """ - dvel = np.asarray(dvel) - dtheta = np.asarray(dtheta) + self._dvel[:] = np.asarray(dvel).reshape(3) + self._dtheta[:] = np.asarray(dtheta).reshape(3) if degrees: - dtheta = (np.pi / 180.0) * dtheta + self._dtheta[:] *= np.pi / 180.0 - dtheta = dtheta - self._dt * self._bg_b + self._dtheta[:] = self._dtheta - self._dt * self._bg_b # Update state-space model R_nb = _rot_matrix_from_quaternion(self._q_nb) - _state_transition_matrix_update(self._phi, dvel, dtheta, R_nb) # -> update phi + _state_transition_matrix_update( + self._phi, self._dvel, self._dtheta, R_nb + ) # -> update phi # Project (a priori) state estimates ahead _project_state_ahead( # -> update p_n, v_n, q_nb (in place) @@ -491,8 +496,8 @@ def update( self._v_n, self._q_nb, R_nb, - dvel, - dtheta, + self._dvel, + self._dtheta, self._dt, self._dvel_g_corr, ) @@ -544,7 +549,7 @@ def update( self._P, self._H[6:9], vg_b, - dvel, + self._dvel, np.asarray(gref_var), ) @@ -567,6 +572,7 @@ def update( ) # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place) + self._dx_before_reset[:] = self._dx # keep copy of dx estimate for smoothing _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b) return self diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py new file mode 100644 index 00000000..9643b0c1 --- /dev/null +++ b/src/smsfusion/_ins/_smoothing.py @@ -0,0 +1,263 @@ +from typing import Any, Self + +import numpy as np +from numba import njit +from numpy.typing import NDArray + +from .._transforms import _euler_from_quaternion, _rot_matrix_from_quaternion +from ._common import _update_quaternion_with_gibbs2 +from ._pvamekf import PVAMEKF, _state_transition_matrix_update + + +class FixedIntervalSmoother: + """ + Fixed-interval smoothing for PVAMEKF. + + This class wraps an instance of PVAMEKF, and maintains a time-ordered buffer + of state and error covariance estimates as measurements are processed via + the ``update()`` method. A backward sweep over the buffered data using the + Rauch-Tung-Striebel (RTS) algorithm [1] is performed to refine the filter + estimates. + + Parameters + ---------- + mekf : PVAMEKF + The underlying PVAMEKF instance used for forward filtering. + cov_smoothing : bool, optional + Whether to include the error covariance matrix, `P`, in the smoothing process. + Disabling the covariance smoothing has no effect on the smoothed state estimates, + and can reduce computation time if smoothed covariances are not required. + + References + ---------- + [1] R. G. Brown and P. Y. C. Hwang, "Random signals and applied Kalman + filtering with MATLAB exercises", 4th ed. Wiley, pp. 208-212, 2012. + """ + + def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: + self._mekf = mekf + self._cov_smoothing = cov_smoothing + self.clear() + + def clear(self) -> None: + """ + Clear the internal buffers of state and covariance estimates. This resets + the smoother, and prepares it for a new interval of measurements. + """ + # Buffers with estimates from the forward pass + self._p_buf: list[NDArray[np.float64]] = [] + self._v_buf: list[NDArray[np.float64]] = [] + self._q_buf: list[NDArray[np.float64]] = [] + self._bg_buf: list[NDArray[np.float64]] = [] + self._dx_buf: list[NDArray[np.float64]] = [] + self._P_buf: list[NDArray[np.float64]] = [] + self._dvel_buf: list[NDArray[np.float64]] = [] + self._dtheta_buf: list[NDArray[np.float64]] = [] + + # Smoothed state and covariance estimates + self._p_n = np.empty((0, 3), dtype="float64") + self._v_n = np.empty((0, 3), dtype="float64") + self._q_nb = np.empty((0, 4), dtype="float64") + self._bg_b = np.empty((0, 3), dtype="float64") + self._P = np.empty((0, *self._mekf._P.shape), dtype="float64") + + def update(self, *args: Any, **kwargs: Any) -> Self: + """ + Update with IMU and aiding measurements, and buffer the resulting estimates + for smoothing. + + The arguments are passed on to the underlying PVAMEKF instance unaltered. + See :meth:`smsfusion.PVAMEKF.update` for a full description of them. + + Parameters + ---------- + *args : tuple + Positional arguments passed on to ``PVAMEKF.update``. + **kwargs : dict + Keyword arguments passed on to ``PVAMEKF.update``. + + Returns + ------- + FixedIntervalSmoother + A reference to the instance itself after the update. + + See Also + -------- + smsfusion.PVAMEKF.update + """ + self._mekf.update(*args, **kwargs) + self._p_buf.append(self._mekf.position()) + self._v_buf.append(self._mekf.velocity()) + self._q_buf.append(self._mekf.quaternion()) + self._bg_buf.append(self._mekf.bias_gyro(degrees=False)) + self._P_buf.append(self._mekf.P) + self._dx_buf.append(self._mekf._dx_before_reset.copy()) + self._dvel_buf.append(self._mekf._dvel.copy()) + self._dtheta_buf.append(self._mekf._dtheta.copy()) + return self + + def _smooth(self) -> None: + n_samples = len(self._q_buf) + if n_samples != len(self._p_n): + self._p_n, self._v_n, self._q_nb, self._bg_b, self._P = _rts_backward_sweep( + np.array(self._p_buf), + np.array(self._v_buf), + np.array(self._q_buf), + np.array(self._bg_buf), + np.array(self._P_buf), + np.array(self._dx_buf), + np.array(self._dvel_buf), + np.array(self._dtheta_buf), + self._mekf._phi.copy(), + self._mekf._Q, + self._cov_smoothing, + ) + + def quaternion(self) -> NDArray[np.float64]: + """ + Smoothed unit quaternion estimates. + + Returns + ------- + np.ndarray, shape (N, 4) + Quaternion estimates for each of the N time steps where the smoother has + been updated with measurements. + """ + self._smooth() + return self._q_nb.copy() + + def euler(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Smoothed Euler angles estimates. + + Parameters + ---------- + degrees : bool, optional + Whether to return the Euler angles in degrees or radians. Defaults to radians. + + Returns + ------- + np.ndarray, shape (N, 3) + Euler angles estimates for each of the N time steps where the smoother has + been updated with measurements. + """ + self._smooth() + if self._q_nb.size == 0: + return np.empty((0, 3), dtype="float64") + + theta = np.array([_euler_from_quaternion(q_i) for q_i in self._q_nb]) + + return np.degrees(theta) if degrees else theta + + def position(self) -> NDArray[np.float64]: + """ + Smoothed position estimates. + + Returns + ------- + np.ndarray, shape (N, 3) + Position estimates for each of the N time steps where the smoother has + been updated with measurements. + """ + self._smooth() + return self._p_n.copy() + + def velocity(self) -> NDArray[np.float64]: + """ + Smoothed velocity estimates. + + Returns + ------- + np.ndarray, shape (N, 3) + Velocity estimates for each of the N time steps where the smoother has + been updated with measurements. + """ + self._smooth() + return self._v_n.copy() + + def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]: + """ + Smoothed gyroscope bias estimates. + + Parameters + ---------- + degrees : bool, optional + Whether to return the bias in deg/s or rad/s. Defaults to rad/s. + + Returns + ------- + np.ndarray, shape (N, 3) + Gyroscope bias estimates for each of the N time steps where the smoother has + been updated with measurements. + """ + self._smooth() + bg_b = self._bg_b.copy() + return np.degrees(bg_b) if degrees else bg_b + + @property + def P(self) -> NDArray[np.float64]: + """ + Error covariance matrix estimates. + + If ``cov_smoothing=True``, smoothed error covariance estimates are returned. + Otherwise, the forward filter covariance estimates are returned. + + Returns + ------- + np.ndarray, shape (N, 12, 12) + Error covariance matrix estimates for each of the N time steps where + the smoother has been updated with measurements. + """ + self._smooth() + return self._P.copy() + + +@njit # type: ignore[misc] +def _rts_backward_sweep( + p_n: NDArray[np.float64], + v_n: NDArray[np.float64], + q_nb: NDArray[np.float64], + bg_b: NDArray[np.float64], + P: NDArray[np.float64], + dx: NDArray[np.float64], + dvel: NDArray[np.float64], + dtheta: NDArray[np.float64], + phi_k: NDArray[np.float64], + Q: NDArray[np.float64], + cov_smoothing: bool, +) -> tuple[ + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], +]: + """ + Perform a backward sweep with the Rauch-Tung-Striebel (RTS) algorithm. + """ + + # Backward sweep + n = len(q_nb) + for k in range(n - 2, -1, -1): + + # Update state space model for step k + R_nb_k = _rot_matrix_from_quaternion(q_nb[k]) + _state_transition_matrix_update(phi_k, dvel[k + 1], dtheta[k + 1], R_nb_k) + + # Calculate a priori error covariance matrix for step k + 1 + P_prior_kp1 = phi_k @ P[k] @ phi_k.T + Q + + # Smoothed error-state estimate and corresponding covariance + A = P[k] @ phi_k.T @ np.linalg.inv(P_prior_kp1) + ddx_k = A @ dx[k + 1] + dx[k] += ddx_k + if cov_smoothing: + P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T + + # Smoothed state estimates + p_n[k] += ddx_k[0:3] + v_n[k] += ddx_k[3:6] + _update_quaternion_with_gibbs2(q_nb[k], ddx_k[6:9]) + bg_b[k] += ddx_k[9:12] + + return p_n, v_n, q_nb, bg_b, P diff --git a/src/smsfusion/_ins/_vamekf.py b/src/smsfusion/_ins/_vamekf.py index dd7b03e7..9c530035 100644 --- a/src/smsfusion/_ins/_vamekf.py +++ b/src/smsfusion/_ins/_vamekf.py @@ -426,8 +426,8 @@ def update( A reference to the instance itself after the update. """ - dvel = np.asarray(dvel) - dtheta = np.asarray(dtheta) + dvel = np.asarray(dvel).reshape(3) + dtheta = np.asarray(dtheta).reshape(3) if degrees: dtheta = (np.pi / 180.0) * dtheta diff --git a/tests/test_ins/test_amekf.py b/tests/test_ins/test_amekf.py index 11e67313..fc586978 100644 --- a/tests/test_ins/test_amekf.py +++ b/tests/test_ins/test_amekf.py @@ -192,6 +192,20 @@ def test_P(self): np.testing.assert_allclose(mekf.P, P0) assert mekf.P is not mekf._P # copy + @pytest.mark.parametrize( + "dvel, dtheta", + [ + (0.1, (0.0, 0.0, 0.0)), # scalar dvel + ((0.0, 0.0, -0.98), 0.1), # scalar dtheta + ((0.0, -0.98), (0.0, 0.0, 0.0)), # too few elements + ((0.0, 0.0, -0.98), (0.0, 0.0, 0.0, 0.0)), # too many elements + ], + ) + def test_update_rejects_bad_increment_shape(self, dvel, dtheta): + mekf = AMEKF(10.0) + with pytest.raises(ValueError): + mekf.update(dvel, dtheta) + @pytest.mark.parametrize( "benchmark_gen, gyro_degrees", [ diff --git a/tests/test_ins/test_pvamekf.py b/tests/test_ins/test_pvamekf.py index 611d590d..aa4dcfb8 100644 --- a/tests/test_ins/test_pvamekf.py +++ b/tests/test_ins/test_pvamekf.py @@ -301,6 +301,20 @@ def test_P(self): np.testing.assert_allclose(mekf.P, P0) assert mekf.P is not mekf._P # copy + @pytest.mark.parametrize( + "dvel, dtheta", + [ + (0.1, (0.0, 0.0, 0.0)), # scalar dvel + ((0.0, 0.0, -0.98), 0.1), # scalar dtheta + ((0.0, -0.98), (0.0, 0.0, 0.0)), # too few elements + ((0.0, 0.0, -0.98), (0.0, 0.0, 0.0, 0.0)), # too many elements + ], + ) + def test_update_rejects_bad_increment_shape(self, dvel, dtheta): + mekf = PVAMEKF(10.0) + with pytest.raises(ValueError): + mekf.update(dvel, dtheta) + @pytest.mark.parametrize( "benchmark_gen, gyro_degrees", [ diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py new file mode 100644 index 00000000..d0fd37e0 --- /dev/null +++ b/tests/test_ins/test_smoothing.py @@ -0,0 +1,443 @@ +import numpy as np +import pytest +from scipy.signal import resample_poly + +import smsfusion as sf +from smsfusion import PVAMEKF, ConingScullingAlg +from smsfusion._ins._smoothing import FixedIntervalSmoother +from smsfusion.benchmark import ( + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, + benchmark_pure_attitude_beat_202311A, + benchmark_pure_attitude_chirp_202311A, +) + + +class Test_FixedIntervalSmoother: + + def _run(self, n_samples=50, seed=0, **smoother_kwargs): + """ + Run a forward filter and a smoother over identical measurements. The + measurements describe a nominally stationary and level body. + """ + fs = 10.0 + rng = np.random.default_rng(seed) + dvel = np.array([0.0, 0.0, -sf.gravity() / fs]) + rng.normal( + 0.0, 1.0e-3, (n_samples, 3) + ) + dtheta = rng.normal(0.0, 1.0e-3, (n_samples, 3)) + + mekf = PVAMEKF(fs) + smoother = FixedIntervalSmoother(PVAMEKF(fs), **smoother_kwargs) + for dvel_i, dtheta_i in zip(dvel, dtheta): + mekf.update(dvel_i, dtheta_i) + smoother.update(dvel_i, dtheta_i) + return mekf, smoother + + def test_update_returns_self(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) is smoother + + def test_position(self): + mekf, smoother = self._run(n_samples=50) + position = smoother.position() + + assert position.shape == (50, 3) + assert position is not smoother._p_n # copy + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(position[-1], mekf.position()) + + def test_position_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.position().shape == (0, 3) + + def test_velocity(self): + mekf, smoother = self._run(n_samples=50) + velocity = smoother.velocity() + + assert velocity.shape == (50, 3) + assert velocity is not smoother._v_n # copy + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(velocity[-1], mekf.velocity()) + + def test_velocity_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.velocity().shape == (0, 3) + + def test_quaternion(self): + mekf, smoother = self._run(n_samples=50) + quaternion = smoother.quaternion() + + assert quaternion.shape == (50, 4) + assert quaternion is not smoother._q_nb # copy + np.testing.assert_allclose(np.linalg.norm(quaternion, axis=1), 1.0) + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(quaternion[-1], mekf.quaternion()) + + def test_quaternion_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.quaternion().shape == (0, 4) + + def test_euler(self): + mekf, smoother = self._run(n_samples=50) + euler = smoother.euler() + + assert euler.shape == (50, 3) + np.testing.assert_allclose(smoother.euler(degrees=False), euler) + np.testing.assert_allclose(smoother.euler(degrees=True), np.degrees(euler)) + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(euler[-1], mekf.euler()) + + def test_euler_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.euler().shape == (0, 3) + + def test_bias_gyro(self): + mekf, smoother = self._run(n_samples=50) + bias_gyro = smoother.bias_gyro() + + assert bias_gyro.shape == (50, 3) + assert bias_gyro is not smoother._bg_b # copy + np.testing.assert_allclose(smoother.bias_gyro(degrees=False), bias_gyro) + np.testing.assert_allclose( + smoother.bias_gyro(degrees=True), np.degrees(bias_gyro) + ) + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(bias_gyro[-1], mekf.bias_gyro()) + + def test_bias_gyro_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.bias_gyro().shape == (0, 3) + + def test_P(self): + mekf, smoother = self._run(n_samples=50) + P = smoother.P + + assert P.shape == (50, 12, 12) + assert P is not smoother._P # copy + + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(P[-1], mekf.P) + + def test_P_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + assert smoother.P.shape == (0, 12, 12) + + def test_smoothing_is_idempotent(self): + _, smoother = self._run(n_samples=20) + + first = smoother.position() + second = smoother.position() + np.testing.assert_array_equal(first, second) + + def test_cov_smoothing_false(self): + """ + Disabling covariance smoothing returns the forward filter covariances, and + leaves the smoothed state estimates unchanged. + """ + fs = 10.0 + n_samples = 30 + rng = np.random.default_rng(0) + dvel = np.array([0.0, 0.0, -sf.gravity() / fs]) + rng.normal( + 0.0, 1.0e-3, (n_samples, 3) + ) + dtheta = rng.normal(0.0, 1.0e-3, (n_samples, 3)) + aid_kwargs = {"pos_var": (0.01, 0.01, 0.01), "vel_var": (0.01, 0.01, 0.01)} + + mekf = PVAMEKF(fs) + smoother = FixedIntervalSmoother(PVAMEKF(fs), cov_smoothing=False) + smoother_cov = FixedIntervalSmoother(PVAMEKF(fs), cov_smoothing=True) + + P_fwd = [] + for dvel_i, dtheta_i in zip(dvel, dtheta): + mekf.update(dvel_i, dtheta_i, **aid_kwargs) + smoother.update(dvel_i, dtheta_i, **aid_kwargs) + smoother_cov.update(dvel_i, dtheta_i, **aid_kwargs) + P_fwd.append(mekf.P) + P_fwd = np.array(P_fwd) + + # Covariances are passed through unsmoothed + np.testing.assert_array_equal(smoother.P, P_fwd) + + # ... whereas smoothing them reduces the uncertainty + assert np.all(np.diagonal(smoother_cov.P[0]) < np.diagonal(P_fwd[0])) + + # The state estimates are unaffected either way + np.testing.assert_allclose(smoother.position(), smoother_cov.position()) + np.testing.assert_allclose(smoother.velocity(), smoother_cov.velocity()) + np.testing.assert_allclose(smoother.euler(), smoother_cov.euler()) + np.testing.assert_allclose(smoother.bias_gyro(), smoother_cov.bias_gyro()) + + def test_clear(self): + _, smoother = self._run(n_samples=20) + smoother.position() # populate the smoothed estimates + + assert smoother.clear() is None + + assert smoother.position().shape == (0, 3) + assert smoother.velocity().shape == (0, 3) + assert smoother.quaternion().shape == (0, 4) + assert smoother.euler().shape == (0, 3) + assert smoother.bias_gyro().shape == (0, 3) + assert smoother.P.shape == (0, 12, 12) + + def test_clear_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) + smoother.clear() + + assert smoother.position().shape == (0, 3) + assert smoother.P.shape == (0, 12, 12) + + def test_clear_does_not_affect_filter(self): + _, smoother = self._run(n_samples=20) + position = smoother._mekf.position() + euler = smoother._mekf.euler() + P = smoother._mekf.P + + smoother.clear() + + np.testing.assert_array_equal(smoother._mekf.position(), position) + np.testing.assert_array_equal(smoother._mekf.euler(), euler) + np.testing.assert_array_equal(smoother._mekf.P, P) + + @pytest.mark.parametrize( + "benchmark_gen", + [ + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, + ], + ) + def test_benchmark_full_aiding(self, benchmark_gen): + """ + Full aiding (position, velocity, and heading). + + All degrees of freedom are observable with this aiding configuration. + """ + fs_imu = 10.0 + warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning + + # Reference signals (without noise) + t, pos_ref, vel_ref, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) + + # IMU and aiding measurements (with noise) + pos_std = 0.1 # m + vel_std = 0.01 # m/s + head_std = np.radians(0.1) # rad + err_acc = sf.constants.ERR_ACC_MOTION2 + err_gyro = sf.constants.ERR_GYRO_MOTION2 + noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0) + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + rng = np.random.default_rng(0) + pos_meas = pos_ref + rng.normal(0.0, pos_std, pos_ref.shape) + vel_meas = vel_ref + rng.normal(0.0, vel_std, vel_ref.shape) + head_meas = euler_ref[:, 2] + rng.normal(0.0, head_std, len(euler_ref)) + + # MEKF + q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) + mekf = PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0) + smoother = FixedIntervalSmoother( + PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0) + ) + + # Coning and sculling corrected IMU increments. The crude approximation, + # dvel = f * dt and dtheta = w * dt, leaves a deterministic rotation + # compensation error which the RTS backward sweep integrates coherently. + coning_sculling = ConingScullingAlg(fs_imu) + + pos_fwd, vel_fwd, euler_fwd = [], [], [] + for f_i, w_i, h_i, p_i, v_i in zip( + acc_meas, gyro_meas, head_meas, pos_meas, vel_meas + ): + + coning_sculling.update(f_i, w_i) + dtheta_i, dvel_i = coning_sculling.flush() + + aid_kwargs = { + "head": h_i, + "head_var": head_std**2, + "head_degrees": False, + "pos": p_i, + "pos_var": pos_std**2 * np.ones(3), + "vel": v_i, + "vel_var": vel_std**2 * np.ones(3), + "gref": True, + "gref_var": (0.1, 0.1, 0.1), + } + mekf.update(dvel_i, dtheta_i, degrees=False, **aid_kwargs) + smoother.update(dvel_i, dtheta_i, degrees=False, **aid_kwargs) + + pos_fwd.append(mekf.position()) + vel_fwd.append(mekf.velocity()) + euler_fwd.append(mekf.euler(degrees=False)) + + pos_fwd = np.array(pos_fwd) + vel_fwd = np.array(vel_fwd) + euler_fwd = np.array(euler_fwd) + + pos_smth = smoother.position() + vel_smth = smoother.velocity() + euler_smth = smoother.euler(degrees=False) + + # Half-sample shift (compensates for the time shift introduced by Euler integration) + pos_fwd = resample_poly(pos_fwd, 2, 1)[1:-1:2] + vel_fwd = resample_poly(vel_fwd, 2, 1)[1:-1:2] + euler_fwd = resample_poly(euler_fwd, 2, 1)[1:-1:2] + pos_smth = resample_poly(pos_smth, 2, 1)[1:-1:2] + vel_smth = resample_poly(vel_smth, 2, 1)[1:-1:2] + euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] + + pos_ref = pos_ref[1:, :] + vel_ref = vel_ref[1:, :] + euler_ref = euler_ref[1:, :] + + def rmse(ref, est): + return np.sqrt(np.mean((ref - est) ** 2, axis=0)) + + pos_rmse_fwd = rmse(pos_ref[warmup:], pos_fwd[warmup:]) + vel_rmse_fwd = rmse(vel_ref[warmup:], vel_fwd[warmup:]) + euler_rmse_fwd = rmse(euler_ref[warmup:], euler_fwd[warmup:]) + + pos_rmse_smth = rmse(pos_ref[warmup:], pos_smth[warmup:]) + vel_rmse_smth = rmse(vel_ref[warmup:], vel_smth[warmup:]) + euler_rmse_smth = rmse(euler_ref[warmup:], euler_smth[warmup:]) + + # The smoother should improve on every estimate compared to the forward filter + assert np.all(pos_rmse_smth < pos_rmse_fwd) + assert np.all(vel_rmse_smth < vel_rmse_fwd) + assert np.all(euler_rmse_smth < euler_rmse_fwd) + + @pytest.mark.parametrize( + "benchmark_gen", + [ + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, + ], + ) + def test_benchmark_head_aiding(self, benchmark_gen): + """ + Heading aiding and the default pseudo zero-position and zero-velocity + measurements are applied. + + Only the attitude (roll, pitch and yaw) is observable with this aiding + configuration. + """ + fs_imu = 10.0 + warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning + + # Reference signals (without noise) + t, _, _, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) + + # IMU and aiding measurements (with noise) + head_std = np.radians(0.1) # rad + err_acc = sf.constants.ERR_ACC_MOTION2 + err_gyro = sf.constants.ERR_GYRO_MOTION2 + noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0) + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + rng = np.random.default_rng(0) + head_meas = euler_ref[:, 2] + rng.normal(0.0, head_std, len(euler_ref)) + + # MEKF + q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) + mekf = PVAMEKF(fs_imu, q0=q0) + smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0)) + + euler_fwd = [] + for f_i, w_i, h_i in zip(acc_meas, gyro_meas, head_meas): + + dvel_i = f_i / fs_imu + dtheta_i = w_i / fs_imu + + aid_kwargs = {"head": h_i, "head_var": head_std**2, "head_degrees": False} + mekf.update(dvel_i, dtheta_i, degrees=False, **aid_kwargs) + smoother.update(dvel_i, dtheta_i, degrees=False, **aid_kwargs) + + euler_fwd.append(mekf.euler(degrees=False)) + + euler_fwd = np.array(euler_fwd) + euler_smth = smoother.euler(degrees=False) + + # Half-sample shift (compensates for the time shift introduced by Euler integration) + euler_fwd = resample_poly(euler_fwd, 2, 1)[1:-1:2] + euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] + euler_ref = euler_ref[1:, :] + + def rmse(ref, est): + return np.sqrt(np.mean((ref - est) ** 2, axis=0)) + + euler_rmse_fwd = rmse(euler_ref[warmup:], euler_fwd[warmup:]) + euler_rmse_smth = rmse(euler_ref[warmup:], euler_smth[warmup:]) + + assert np.all(euler_rmse_smth < euler_rmse_fwd) + + @pytest.mark.parametrize( + "benchmark_gen", + [ + benchmark_pure_attitude_beat_202311A, + benchmark_pure_attitude_chirp_202311A, + ], + ) + def test_benchmark_no_aiding(self, benchmark_gen): + """ + No external aiding, i.e., only the default pseudo zero-position and + zero-velocity measurements are applied. The body does not translate in this + benchmark, so these pseudo measurements are valid. + + Only roll, pitch, and the x- and y-axis gyroscope biases are observable in this + configuration. + """ + fs_imu = 10.0 + warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning + + # Reference signals (without noise) + t, euler_ref, acc_ref, gyro_ref = benchmark_gen(fs_imu) + + # IMU measurements (with noise) + err_acc = sf.constants.ERR_ACC_MOTION2 + err_gyro = sf.constants.ERR_GYRO_MOTION2 + noise_model = sf.noise.IMUNoise(err_acc=err_acc, err_gyro=err_gyro, seed=0) + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + + # MEKF + q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) + mekf = PVAMEKF(fs_imu, q0=q0) + smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0)) + + euler_fwd = [] + for f_i, w_i in zip(acc_meas, gyro_meas): + + dvel_i = f_i / fs_imu + dtheta_i = w_i / fs_imu + + mekf.update(dvel_i, dtheta_i, degrees=False) + smoother.update(dvel_i, dtheta_i, degrees=False) + + euler_fwd.append(mekf.euler(degrees=False)) + + euler_fwd = np.array(euler_fwd) + euler_smth = smoother.euler(degrees=False) + + # Half-sample shift (compensates for the time shift introduced by Euler integration) + euler_fwd = resample_poly(euler_fwd, 2, 1)[1:-1:2] + euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] + + euler_ref = euler_ref[1:, :] + + def rmse(ref, est): + return np.sqrt(np.mean((ref - est) ** 2, axis=0)) + + euler_rmse_fwd = rmse(euler_ref[warmup:], euler_fwd[warmup:]) + euler_rmse_smth = rmse(euler_ref[warmup:], euler_smth[warmup:]) + + # Only roll and pitch are observable with this aiding configuration + assert np.all(euler_rmse_smth[:2] < euler_rmse_fwd[:2]) diff --git a/tests/test_ins/test_vamekf.py b/tests/test_ins/test_vamekf.py index ba28a978..950936dc 100644 --- a/tests/test_ins/test_vamekf.py +++ b/tests/test_ins/test_vamekf.py @@ -231,6 +231,20 @@ def test_P(self): np.testing.assert_allclose(mekf.P, P0) assert mekf.P is not mekf._P # copy + @pytest.mark.parametrize( + "dvel, dtheta", + [ + (0.1, (0.0, 0.0, 0.0)), # scalar dvel + ((0.0, 0.0, -0.98), 0.1), # scalar dtheta + ((0.0, -0.98), (0.0, 0.0, 0.0)), # too few elements + ((0.0, 0.0, -0.98), (0.0, 0.0, 0.0, 0.0)), # too many elements + ], + ) + def test_update_rejects_bad_increment_shape(self, dvel, dtheta): + mekf = VAMEKF(10.0) + with pytest.raises(ValueError): + mekf.update(dvel, dtheta) + @pytest.mark.parametrize( "benchmark_gen, gyro_degrees", [