From 3259352e4e056572b127f15ddc9b363e377efc24 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 09:54:20 +0200 Subject: [PATCH 01/34] add FixedIntervalSmoother --- src/smsfusion/__init__.py | 2 + src/smsfusion/_ins/__init__.py | 2 + src/smsfusion/_ins/_smoothing.py | 177 +++++++++++++++++++++++++++++++ 3 files changed, 181 insertions(+) create mode 100644 src/smsfusion/_ins/_smoothing.py 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/_smoothing.py b/src/smsfusion/_ins/_smoothing.py new file mode 100644 index 00000000..13af1c97 --- /dev/null +++ b/src/smsfusion/_ins/_smoothing.py @@ -0,0 +1,177 @@ +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: + def __init__(self, mekf: PVAMEKF): + self._mekf = mekf + self._mekf._keep_smoothing_params = True + + # Buffers with estimates from the forward pass + self._p_buf = [] + self._v_buf = [] + self._q_buf = [] + self._bg_buf = [] + self._dx_buf = [] + self._P_buf = [] + self._dvel_buf = [] + self._dtheta_buf = [] + + # 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, **kwargs): + """ + Update with IMU and aiding measurements. + """ + 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_copy) + self._dvel_buf.append(self._mekf._dvel_copy) + self._dtheta_buf.append(self._mekf._dtheta_copy) + return self + + def _smooth(self): + 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), + self._dvel_buf, + self._dtheta_buf, + self._mekf._phi, + self._mekf._Q, + ) + + def quaternion(self) -> NDArray[np.float64]: + """ + Smoothed 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): + """ + Smoothed Euler angles estimates. + + 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) -> NDArray[np.float64]: + """ + Smoothed gyroscope bias estimates. + + 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() + return self._bg_b.copy() + + @property + def P(self) -> NDArray[np.float64]: + """ + Smoothed error covariance estimates. + + Returns + ------- + np.ndarray, shape (N, 12, 12) + Error covariance 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, v_n, q_nb, bg_b, P, dx, dvel, dtheta, phi_k, Q): + """ + 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 + P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T + + # Update 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 From d5ca79b46b7d48eb0fe6184bbe9330476b3a6cb8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 11:18:18 +0200 Subject: [PATCH 02/34] add smoothing test --- tests/test_ins/test_smoothing.py | 196 +++++++++++++++++++++++++++++++ 1 file changed, 196 insertions(+) create mode 100644 tests/test_ins/test_smoothing.py diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py new file mode 100644 index 00000000..6cffc6e4 --- /dev/null +++ b/tests/test_ins/test_smoothing.py @@ -0,0 +1,196 @@ +import numpy as np +import pytest +from scipy.signal import resample_poly + +import smsfusion as sf +from smsfusion import PVAMEKF +from smsfusion._ins._smoothing import FixedIntervalSmoother +from smsfusion.benchmark import ( + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, +) + + +class Test_FixedIntervalSmoother: + + @pytest.mark.xfail( + reason=( + "Known bug: when position, velocity and heading aiding are all active " + "at the same time, the RTS backward sweep makes the smoothed position " + "(and roll/pitch) estimates worse than the forward filter, instead of " + "better. Velocity and gyro bias smoothing are unaffected. See combined " + "pos+vel+head aiding case; gref aiding is not involved." + ), + strict=False, + ) + @pytest.mark.parametrize( + "benchmark_gen", + [ + benchmark_full_pva_beat_202311A, + benchmark_full_pva_chirp_202311A, + ], + ) + def test_benchmark_full_aiding(self, benchmark_gen): + 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) + bg = np.array([0.01, -0.02, 0.03]) # rad/s + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape) + vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape) + head_meas = euler_ref[:, 2] + np.random.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) + ) + + pos_fwd, vel_fwd, euler_fwd, bg_fwd = [], [], [], [] + for f_i, w_i, h_i, p_i, v_i in zip( + acc_meas, gyro_meas, head_meas, pos_meas, vel_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, + "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)) + bg_fwd.append(mekf.bias_gyro()) + + pos_fwd = np.array(pos_fwd) + vel_fwd = np.array(vel_fwd) + euler_fwd = np.array(euler_fwd) + bg_fwd = np.array(bg_fwd) + + pos_smth = smoother.position() + vel_smth = smoother.velocity() + euler_smth = smoother.euler(degrees=False) + bg_smth = smoother.bias_gyro() + + # 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] + bg_fwd = resample_poly(bg_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] + bg_smth = resample_poly(bg_smth, 2, 1)[1:-1:2] + + pos_ref = pos_ref[1:, :] + vel_ref = vel_ref[1:, :] + euler_ref = euler_ref[1:, :] + bg_ref = np.tile(bg, (len(bg_fwd), 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:]) + bg_rmse_fwd = rmse(bg_ref[warmup:], bg_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:]) + bg_rmse_smth = rmse(bg_ref[warmup:], bg_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) + assert np.all(bg_rmse_smth < bg_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-only aiding, i.e. no position or velocity aiding. Unlike the + full-aiding case (position + velocity + heading), this aiding + configuration does not trigger the smoothing bug, and the smoother + should consistently improve on the forward filter's Euler angle + estimates. + """ + 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) + bg = np.array([0.01, -0.02, 0.0]) # rad/s + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + head_meas = euler_ref[:, 2] + np.random.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) From f511115382307779ab2c4f9efe39f4769c032918 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 11:25:24 +0200 Subject: [PATCH 03/34] add smoothing params to pvamekf --- src/smsfusion/_ins/_pvamekf.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/smsfusion/_ins/_pvamekf.py b/src/smsfusion/_ins/_pvamekf.py index 5f3ed7bd..062b8697 100644 --- a/src/smsfusion/_ins/_pvamekf.py +++ b/src/smsfusion/_ins/_pvamekf.py @@ -313,6 +313,7 @@ def __init__( self._g_n = _gravity_nav(self._g, self._nav_frame) self._dvel_g_corr = self._dt * self._g_n self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy() + self._keep_smoothing_params = False # IMU noise parameters self._vrw = acc_noise_density # velocity random walk @@ -566,6 +567,11 @@ def update( head_degrees, ) + if self._keep_smoothing_params: + self._dx_copy = self._dx.copy() + self._dvel_copy = dvel.copy() + self._dtheta_copy = dtheta.copy() + # Reset state -> update p_n, v_n, q_nb, bg_b and dx (in place) _reset(self._dx, self._p_n, self._v_n, self._q_nb, self._bg_b) From f5bf99c4b3983d637ca6a3b821d521f2be53e9ae Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 11:35:23 +0200 Subject: [PATCH 04/34] cov smoothing optional --- src/smsfusion/_ins/_smoothing.py | 45 +++++++++++++++++++++++++++++--- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 13af1c97..30cf4eb5 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -8,9 +8,34 @@ class FixedIntervalSmoother: - def __init__(self, mekf: PVAMEKF): + """ + 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, default True + 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): self._mekf = mekf self._mekf._keep_smoothing_params = True + self._cov_smoothing = cov_smoothing # Buffers with estimates from the forward pass self._p_buf = [] @@ -58,6 +83,7 @@ def _smooth(self): self._dtheta_buf, self._mekf._phi, self._mekf._Q, + self._cov_smoothing, ) def quaternion(self) -> NDArray[np.float64]: @@ -146,7 +172,19 @@ def P(self) -> NDArray[np.float64]: @njit # type: ignore[misc] -def _rts_backward_sweep(p_n, v_n, q_nb, bg_b, P, dx, dvel, dtheta, phi_k, Q): +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 = True, +): """ Perform a backward sweep with the Rauch-Tung-Striebel (RTS) algorithm. """ @@ -166,7 +204,8 @@ def _rts_backward_sweep(p_n, v_n, q_nb, bg_b, P, dx, dvel, dtheta, phi_k, Q): A = P[k] @ phi_k.T @ np.linalg.inv(P_prior_kp1) ddx_k = A @ dx[k + 1] dx[k] += ddx_k - P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T + if cov_smoothing: + P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T # Update smoothed state estimates p_n[k] += ddx_k[0:3] From 98e15416a8d8e1947cc92f74dc05b41339548217 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 11:36:09 +0200 Subject: [PATCH 05/34] docstring default cov smoothing --- src/smsfusion/_ins/_smoothing.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 30cf4eb5..64da522a 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -21,10 +21,11 @@ class FixedIntervalSmoother: ---------- mekf : PVAMEKF The underlying PVAMEKF instance used for forward filtering. - cov_smoothing : bool, default True + 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. + Defaults to ``True``. References ---------- From 7d740172863405f18d9ee8fce885b5dd70c8892e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 11:37:53 +0200 Subject: [PATCH 06/34] typing fix --- src/smsfusion/_ins/_smoothing.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 64da522a..65e82311 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -1,3 +1,5 @@ +from typing import Self + import numpy as np from numba import njit from numpy.typing import NDArray @@ -33,7 +35,7 @@ class FixedIntervalSmoother: filtering with MATLAB exercises", 4th ed. Wiley, pp. 208-212, 2012. """ - def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True): + def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: self._mekf = mekf self._mekf._keep_smoothing_params = True self._cov_smoothing = cov_smoothing @@ -55,7 +57,7 @@ def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True): self._bg_b = np.empty((0, 3), dtype="float64") self._P = np.empty((0, *self._mekf._P.shape), dtype="float64") - def update(self, *args, **kwargs): + def update(self, *args, **kwargs) -> Self: """ Update with IMU and aiding measurements. """ From ff1737863e71f89a9c4f2d1ef180bd06d28f93b2 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 13:39:00 +0200 Subject: [PATCH 07/34] small fix --- tests/test_ins/test_smoothing.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 6cffc6e4..9c8cd434 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -138,13 +138,6 @@ def rmse(ref, est): ], ) def test_benchmark_head_aiding(self, benchmark_gen): - """ - Heading-only aiding, i.e. no position or velocity aiding. Unlike the - full-aiding case (position + velocity + heading), this aiding - configuration does not trigger the smoothing bug, and the smoother - should consistently improve on the forward filter's Euler angle - estimates. - """ fs_imu = 10.0 warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning From bf208a484e42a67a30f66fa918969d16d631d529 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 14:00:23 +0200 Subject: [PATCH 08/34] refactor keep smoothing params --- src/smsfusion/_ins/_pvamekf.py | 29 +++++++++++++++-------------- src/smsfusion/_ins/_smoothing.py | 7 +++---- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/smsfusion/_ins/_pvamekf.py b/src/smsfusion/_ins/_pvamekf.py index 062b8697..f4ee249a 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. """ @@ -328,6 +328,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( @@ -474,17 +477,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) + self._dtheta[:] = np.asarray(dtheta) 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) @@ -492,8 +497,8 @@ def update( self._v_n, self._q_nb, R_nb, - dvel, - dtheta, + self._dvel, + self._dtheta, self._dt, self._dvel_g_corr, ) @@ -545,7 +550,7 @@ def update( self._P, self._H[6:9], vg_b, - dvel, + self._dvel, np.asarray(gref_var), ) @@ -567,12 +572,8 @@ def update( head_degrees, ) - if self._keep_smoothing_params: - self._dx_copy = self._dx.copy() - self._dvel_copy = dvel.copy() - self._dtheta_copy = dtheta.copy() - # 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 index 65e82311..e0ca21bf 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -37,7 +37,6 @@ class FixedIntervalSmoother: def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: self._mekf = mekf - self._mekf._keep_smoothing_params = True self._cov_smoothing = cov_smoothing # Buffers with estimates from the forward pass @@ -67,9 +66,9 @@ def update(self, *args, **kwargs) -> Self: 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_copy) - self._dvel_buf.append(self._mekf._dvel_copy) - self._dtheta_buf.append(self._mekf._dtheta_copy) + 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): From f2059e784f9cb0e061cc8fdbc709cff5a185c07d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 14:47:07 +0200 Subject: [PATCH 09/34] fix tests using coning/sculling --- tests/test_ins/test_smoothing.py | 31 ++++++++++++++----------------- 1 file changed, 14 insertions(+), 17 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 9c8cd434..642a8e2b 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -3,7 +3,7 @@ from scipy.signal import resample_poly import smsfusion as sf -from smsfusion import PVAMEKF +from smsfusion import PVAMEKF, ConingScullingAlg from smsfusion._ins._smoothing import FixedIntervalSmoother from smsfusion.benchmark import ( benchmark_full_pva_beat_202311A, @@ -13,16 +13,6 @@ class Test_FixedIntervalSmoother: - @pytest.mark.xfail( - reason=( - "Known bug: when position, velocity and heading aiding are all active " - "at the same time, the RTS backward sweep makes the smoothed position " - "(and roll/pitch) estimates worse than the forward filter, instead of " - "better. Velocity and gyro bias smoothing are unaffected. See combined " - "pos+vel+head aiding case; gref aiding is not involved." - ), - strict=False, - ) @pytest.mark.parametrize( "benchmark_gen", [ @@ -48,9 +38,10 @@ def test_benchmark_full_aiding(self, benchmark_gen): imu_noise = noise_model(fs_imu, len(t)) acc_meas = acc_ref + imu_noise[:, :3] gyro_meas = gyro_ref + imu_noise[:, 3:] + bg - pos_meas = pos_ref + np.random.normal(0.0, pos_std, pos_ref.shape) - vel_meas = vel_ref + np.random.normal(0.0, vel_std, vel_ref.shape) - head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref)) + 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) @@ -59,13 +50,18 @@ def test_benchmark_full_aiding(self, benchmark_gen): 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, bg_fwd = [], [], [], [] for f_i, w_i, h_i, p_i, v_i in zip( acc_meas, gyro_meas, head_meas, pos_meas, vel_meas ): - dvel_i = f_i / fs_imu - dtheta_i = w_i / fs_imu + coning_sculling.update(f_i, w_i) + dtheta_i, dvel_i = coning_sculling.flush() aid_kwargs = { "head": h_i, @@ -153,7 +149,8 @@ def test_benchmark_head_aiding(self, benchmark_gen): imu_noise = noise_model(fs_imu, len(t)) acc_meas = acc_ref + imu_noise[:, :3] gyro_meas = gyro_ref + imu_noise[:, 3:] + bg - head_meas = euler_ref[:, 2] + np.random.normal(0.0, head_std, len(euler_ref)) + 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) From 6a0ffdf2856488e8477d596104e0029ecfded4f8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 17 Sep 2026 16:07:20 +0200 Subject: [PATCH 10/34] delete unused keep_smoothing_params parameter from pvamekf --- src/smsfusion/_ins/_pvamekf.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/smsfusion/_ins/_pvamekf.py b/src/smsfusion/_ins/_pvamekf.py index f4ee249a..d5e1ee55 100644 --- a/src/smsfusion/_ins/_pvamekf.py +++ b/src/smsfusion/_ins/_pvamekf.py @@ -313,7 +313,6 @@ def __init__( self._g_n = _gravity_nav(self._g, self._nav_frame) self._dvel_g_corr = self._dt * self._g_n self._lever_arm = np.asarray_chkfinite(lever_arm).reshape(3).copy() - self._keep_smoothing_params = False # IMU noise parameters self._vrw = acc_noise_density # velocity random walk From e72476c0f75a4256be78c5420345171a9393c502 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 10:11:00 +0200 Subject: [PATCH 11/34] test aiding denied --- tests/test_ins/test_smoothing.py | 89 ++++++++++++++++++++++++++++++-- 1 file changed, 85 insertions(+), 4 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 642a8e2b..5b34606b 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -8,6 +8,8 @@ from smsfusion.benchmark import ( benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A, + benchmark_pure_attitude_beat_202311A, + benchmark_pure_attitude_chirp_202311A, ) @@ -45,9 +47,9 @@ def test_benchmark_full_aiding(self, benchmark_gen): # 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) + mekf = PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0, bg0=bg) smoother = FixedIntervalSmoother( - PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0) + PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0, bg0=bg) ) # Coning and sculling corrected IMU increments. The crude approximation, @@ -154,8 +156,8 @@ def test_benchmark_head_aiding(self, benchmark_gen): # MEKF q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) - mekf = PVAMEKF(fs_imu, q0=q0) - smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0)) + mekf = PVAMEKF(fs_imu, q0=q0, bg0=bg) + smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0, bg0=bg)) euler_fwd = [] for f_i, w_i, h_i in zip(acc_meas, gyro_meas, head_meas): @@ -184,3 +186,82 @@ def rmse(ref, est): 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. Yaw and the z-axis gyroscope bias are not, and are therefore not + asserted on. + """ + 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) + bg = np.array([0.01, -0.02, 0.03]) # rad/s + imu_noise = noise_model(fs_imu, len(t)) + acc_meas = acc_ref + imu_noise[:, :3] + gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + + # MEKF + q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) + mekf = PVAMEKF(fs_imu, q0=q0, bg0=bg) + smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0, bg0=bg)) + + coning_sculling = ConingScullingAlg(fs_imu) + + euler_fwd, bg_fwd = [], [] + for f_i, w_i in zip(acc_meas, gyro_meas): + + coning_sculling.update(f_i, w_i) + dtheta_i, dvel_i = coning_sculling.flush() + + mekf.update(dvel_i, dtheta_i, degrees=False) + smoother.update(dvel_i, dtheta_i, degrees=False) + + euler_fwd.append(mekf.euler(degrees=False)) + bg_fwd.append(mekf.bias_gyro()) + + euler_fwd = np.array(euler_fwd) + bg_fwd = np.array(bg_fwd) + + euler_smth = smoother.euler(degrees=False) + bg_smth = smoother.bias_gyro() + + # Half-sample shift (compensates for the time shift introduced by Euler integration) + euler_fwd = resample_poly(euler_fwd, 2, 1)[1:-1:2] + bg_fwd = resample_poly(bg_fwd, 2, 1)[1:-1:2] + euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] + bg_smth = resample_poly(bg_smth, 2, 1)[1:-1:2] + + euler_ref = euler_ref[1:, :] + bg_ref = np.tile(bg, (len(bg_fwd), 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:]) + bg_rmse_fwd = rmse(bg_ref[warmup:], bg_fwd[warmup:]) + bg_rmse_smth = rmse(bg_ref[warmup:], bg_smth[warmup:]) + + # Only roll and pitch are observable with this aiding configuration + assert np.all(euler_rmse_smth[:2] < euler_rmse_fwd[:2]) + assert np.all(bg_rmse_fwd[:2] < 1.0e-4) # rad/s + assert np.all(bg_rmse_smth[:2] < 1.0e-4) # rad/s From 6584496b0828a4e911d8274c81a0d112d33d8bee Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 10:25:53 +0200 Subject: [PATCH 12/34] small test fix --- tests/test_ins/test_smoothing.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 5b34606b..24f691ab 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -224,13 +224,11 @@ def test_benchmark_no_aiding(self, benchmark_gen): mekf = PVAMEKF(fs_imu, q0=q0, bg0=bg) smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0, bg0=bg)) - coning_sculling = ConingScullingAlg(fs_imu) - euler_fwd, bg_fwd = [], [] for f_i, w_i in zip(acc_meas, gyro_meas): - coning_sculling.update(f_i, w_i) - dtheta_i, dvel_i = coning_sculling.flush() + 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) From e3734e85154349fc919a633ae941c53c9bf97454 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 10:39:00 +0200 Subject: [PATCH 13/34] set gyro bias to zero --- tests/test_ins/test_smoothing.py | 48 ++++++++------------------------ 1 file changed, 12 insertions(+), 36 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 24f691ab..298c51c3 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -36,10 +36,9 @@ def test_benchmark_full_aiding(self, benchmark_gen): 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) - bg = np.array([0.01, -0.02, 0.03]) # rad/s imu_noise = noise_model(fs_imu, len(t)) acc_meas = acc_ref + imu_noise[:, :3] - gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + 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) @@ -47,9 +46,9 @@ def test_benchmark_full_aiding(self, benchmark_gen): # 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, bg0=bg) + 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, bg0=bg) + PVAMEKF(fs_imu, p0=pos_ref[0], v0=vel_ref[0], q0=q0) ) # Coning and sculling corrected IMU increments. The crude approximation, @@ -57,7 +56,7 @@ def test_benchmark_full_aiding(self, benchmark_gen): # compensation error which the RTS backward sweep integrates coherently. coning_sculling = ConingScullingAlg(fs_imu) - pos_fwd, vel_fwd, euler_fwd, bg_fwd = [], [], [], [] + 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 ): @@ -82,32 +81,26 @@ def test_benchmark_full_aiding(self, benchmark_gen): pos_fwd.append(mekf.position()) vel_fwd.append(mekf.velocity()) euler_fwd.append(mekf.euler(degrees=False)) - bg_fwd.append(mekf.bias_gyro()) pos_fwd = np.array(pos_fwd) vel_fwd = np.array(vel_fwd) euler_fwd = np.array(euler_fwd) - bg_fwd = np.array(bg_fwd) pos_smth = smoother.position() vel_smth = smoother.velocity() euler_smth = smoother.euler(degrees=False) - bg_smth = smoother.bias_gyro() # 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] - bg_fwd = resample_poly(bg_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] - bg_smth = resample_poly(bg_smth, 2, 1)[1:-1:2] pos_ref = pos_ref[1:, :] vel_ref = vel_ref[1:, :] euler_ref = euler_ref[1:, :] - bg_ref = np.tile(bg, (len(bg_fwd), 1)) def rmse(ref, est): return np.sqrt(np.mean((ref - est) ** 2, axis=0)) @@ -115,18 +108,15 @@ def rmse(ref, est): 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:]) - bg_rmse_fwd = rmse(bg_ref[warmup:], bg_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:]) - bg_rmse_smth = rmse(bg_ref[warmup:], bg_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) - assert np.all(bg_rmse_smth < bg_rmse_fwd) @pytest.mark.parametrize( "benchmark_gen", @@ -147,17 +137,16 @@ def test_benchmark_head_aiding(self, benchmark_gen): 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) - bg = np.array([0.01, -0.02, 0.0]) # rad/s imu_noise = noise_model(fs_imu, len(t)) acc_meas = acc_ref + imu_noise[:, :3] - gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + 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, bg0=bg) - smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0, bg0=bg)) + 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): @@ -201,8 +190,7 @@ def test_benchmark_no_aiding(self, benchmark_gen): benchmark, so these pseudo measurements are valid. Only roll, pitch, and the x- and y-axis gyroscope biases are observable in this - configuration. Yaw and the z-axis gyroscope bias are not, and are therefore not - asserted on. + configuration. """ fs_imu = 10.0 warmup = int(fs_imu * 600.0) # truncate 600 seconds from the beginning @@ -214,17 +202,16 @@ def test_benchmark_no_aiding(self, benchmark_gen): 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) - bg = np.array([0.01, -0.02, 0.03]) # rad/s imu_noise = noise_model(fs_imu, len(t)) acc_meas = acc_ref + imu_noise[:, :3] - gyro_meas = gyro_ref + imu_noise[:, 3:] + bg + gyro_meas = gyro_ref + imu_noise[:, 3:] # MEKF q0 = sf.quaternion_from_euler(euler_ref[0], degrees=False) - mekf = PVAMEKF(fs_imu, q0=q0, bg0=bg) - smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0, bg0=bg)) + mekf = PVAMEKF(fs_imu, q0=q0) + smoother = FixedIntervalSmoother(PVAMEKF(fs_imu, q0=q0)) - euler_fwd, bg_fwd = [], [] + euler_fwd = [] for f_i, w_i in zip(acc_meas, gyro_meas): dvel_i = f_i / fs_imu @@ -234,32 +221,21 @@ def test_benchmark_no_aiding(self, benchmark_gen): smoother.update(dvel_i, dtheta_i, degrees=False) euler_fwd.append(mekf.euler(degrees=False)) - bg_fwd.append(mekf.bias_gyro()) euler_fwd = np.array(euler_fwd) - bg_fwd = np.array(bg_fwd) - euler_smth = smoother.euler(degrees=False) - bg_smth = smoother.bias_gyro() # Half-sample shift (compensates for the time shift introduced by Euler integration) euler_fwd = resample_poly(euler_fwd, 2, 1)[1:-1:2] - bg_fwd = resample_poly(bg_fwd, 2, 1)[1:-1:2] euler_smth = resample_poly(euler_smth, 2, 1)[1:-1:2] - bg_smth = resample_poly(bg_smth, 2, 1)[1:-1:2] euler_ref = euler_ref[1:, :] - bg_ref = np.tile(bg, (len(bg_fwd), 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:]) - bg_rmse_fwd = rmse(bg_ref[warmup:], bg_fwd[warmup:]) - bg_rmse_smth = rmse(bg_ref[warmup:], bg_smth[warmup:]) # Only roll and pitch are observable with this aiding configuration assert np.all(euler_rmse_smth[:2] < euler_rmse_fwd[:2]) - assert np.all(bg_rmse_fwd[:2] < 1.0e-4) # rad/s - assert np.all(bg_rmse_smth[:2] < 1.0e-4) # rad/s From 06ffad634219af06ca17eed06562aad2abbdf39e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 10:41:57 +0200 Subject: [PATCH 14/34] test docstrings --- tests/test_ins/test_smoothing.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 298c51c3..ed900790 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -23,6 +23,11 @@ class Test_FixedIntervalSmoother: ], ) 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 @@ -126,6 +131,13 @@ def rmse(ref, est): ], ) 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 From 0a570553863f77c80dc2ebb69f5d0c1c677d84de Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:16:25 +0200 Subject: [PATCH 15/34] mekf reshape in update --- src/smsfusion/_ins/_pvamekf.py | 4 ++-- tests/test_ins/test_pvamekf.py | 14 ++++++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins/_pvamekf.py b/src/smsfusion/_ins/_pvamekf.py index d5e1ee55..ece3231c 100644 --- a/src/smsfusion/_ins/_pvamekf.py +++ b/src/smsfusion/_ins/_pvamekf.py @@ -476,8 +476,8 @@ def update( A reference to the instance itself after the update. """ - self._dvel[:] = np.asarray(dvel) - self._dtheta[:] = np.asarray(dtheta) + self._dvel[:] = np.asarray(dvel).reshape(3) + self._dtheta[:] = np.asarray(dtheta).reshape(3) if degrees: self._dtheta[:] *= np.pi / 180.0 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", [ From b9229fce686159921e08d03828a2a4bda34d4146 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:19:54 +0200 Subject: [PATCH 16/34] make same reshape fix to VAMEKF and AMEKF --- src/smsfusion/_ins/_amekf.py | 4 ++-- src/smsfusion/_ins/_vamekf.py | 4 ++-- tests/test_ins/test_amekf.py | 14 ++++++++++++++ tests/test_ins/test_vamekf.py | 14 ++++++++++++++ 4 files changed, 32 insertions(+), 4 deletions(-) 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/_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_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", [ From 28038c470be3213cd60f8c0be7aa489f654c06ec Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:24:00 +0200 Subject: [PATCH 17/34] send buffers as arrays --- src/smsfusion/_ins/_smoothing.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index e0ca21bf..7bd9607d 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -81,8 +81,8 @@ def _smooth(self): np.array(self._bg_buf), np.array(self._P_buf), np.array(self._dx_buf), - self._dvel_buf, - self._dtheta_buf, + np.array(self._dvel_buf), + np.array(self._dtheta_buf), self._mekf._phi, self._mekf._Q, self._cov_smoothing, From 25dddc63643147a2276ae46899b6e330bf3fb9a6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:28:48 +0200 Subject: [PATCH 18/34] add degrees flag to bias gyro --- src/smsfusion/_ins/_smoothing.py | 10 ++++++++-- tests/test_ins/test_smoothing.py | 11 +++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 7bd9607d..5525e40e 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -145,10 +145,15 @@ def velocity(self) -> NDArray[np.float64]: self._smooth() return self._v_n.copy() - def bias_gyro(self) -> NDArray[np.float64]: + 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) @@ -156,7 +161,8 @@ def bias_gyro(self) -> NDArray[np.float64]: been updated with measurements. """ self._smooth() - return self._bg_b.copy() + bg_b = self._bg_b.copy() + return np.degrees(bg_b) if degrees else bg_b @property def P(self) -> NDArray[np.float64]: diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index ed900790..3ae74c82 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -15,6 +15,17 @@ class Test_FixedIntervalSmoother: + def test_bias_gyro(self): + bg0 = np.array([0.01, -0.02, 0.03]) + smoother = FixedIntervalSmoother(PVAMEKF(10.0, bg0=bg0)) + for _ in range(10): + smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) + + bg_rad = smoother.bias_gyro() + np.testing.assert_allclose(smoother.bias_gyro(degrees=False), bg_rad) + np.testing.assert_allclose(smoother.bias_gyro(degrees=True), np.degrees(bg_rad)) + assert smoother.bias_gyro() is not smoother._bg_b # copy + @pytest.mark.parametrize( "benchmark_gen", [ From 286b97c6b8a57c05d1b54d02afe4244dcbfd9fe7 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:34:16 +0200 Subject: [PATCH 19/34] Docstring fix of P property --- src/smsfusion/_ins/_smoothing.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 5525e40e..9db4829b 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -27,7 +27,6 @@ class FixedIntervalSmoother: 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. - Defaults to ``True``. References ---------- @@ -169,11 +168,14 @@ def P(self) -> NDArray[np.float64]: """ Smoothed error covariance estimates. + NB! If the smoother was created with ``cov_smoothing=False``, the forward + filter's (i.e., unsmoothed) error covariance estimates are returned instead. + Returns ------- np.ndarray, shape (N, 12, 12) - Error covariance estimates for each of the N time steps where the smoother has - been updated with measurements. + Error covariance estimates for each of the N time steps where the smoother + has been updated with measurements. """ self._smooth() return self._P.copy() From 130230d9f9b8d06afe3961026649d9fb07f622e1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:41:08 +0200 Subject: [PATCH 20/34] typing fixes --- src/smsfusion/_ins/_smoothing.py | 32 +++++++++++++++++++------------- 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 9db4829b..2690b4ce 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -1,4 +1,4 @@ -from typing import Self +from typing import Any, Self import numpy as np from numba import njit @@ -39,14 +39,14 @@ def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: self._cov_smoothing = cov_smoothing # Buffers with estimates from the forward pass - self._p_buf = [] - self._v_buf = [] - self._q_buf = [] - self._bg_buf = [] - self._dx_buf = [] - self._P_buf = [] - self._dvel_buf = [] - self._dtheta_buf = [] + 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") @@ -55,7 +55,7 @@ def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: self._bg_b = np.empty((0, 3), dtype="float64") self._P = np.empty((0, *self._mekf._P.shape), dtype="float64") - def update(self, *args, **kwargs) -> Self: + def update(self, *args: Any, **kwargs: Any) -> Self: """ Update with IMU and aiding measurements. """ @@ -70,7 +70,7 @@ def update(self, *args, **kwargs) -> Self: self._dtheta_buf.append(self._mekf._dtheta.copy()) return self - def _smooth(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( @@ -100,7 +100,7 @@ def quaternion(self) -> NDArray[np.float64]: self._smooth() return self._q_nb.copy() - def euler(self, degrees: bool = False): + def euler(self, degrees: bool = False) -> NDArray[np.float64]: """ Smoothed Euler angles estimates. @@ -194,7 +194,13 @@ def _rts_backward_sweep( phi_k: NDArray[np.float64], Q: NDArray[np.float64], cov_smoothing: bool = True, -): +) -> 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. """ From bd6181a5d09f0645df05a5e82272791172d563d3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 11:56:27 +0200 Subject: [PATCH 21/34] more unit tests of smoother --- tests/test_ins/test_smoothing.py | 181 ++++++++++++++++++++++++++++++- 1 file changed, 178 insertions(+), 3 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 3ae74c82..ae7cf4b3 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -15,17 +15,192 @@ class Test_FixedIntervalSmoother: + FS = 10.0 + + @classmethod + def _run(cls, n_samples=50, seed=0, bg0=(0.0, 0.0, 0.0), **smoother_kwargs): + """ + Run a forward filter and a smoother over identical measurements. The + measurements describe a nominally stationary and level body. + """ + rng = np.random.default_rng(seed) + dvel = np.array([0.0, 0.0, -sf.gravity() / cls.FS]) + rng.normal( + 0.0, 1.0e-3, (n_samples, 3) + ) + dtheta = rng.normal(0.0, 1.0e-3, (n_samples, 3)) + + mekf = PVAMEKF(cls.FS, bg0=bg0) + smoother = FixedIntervalSmoother(PVAMEKF(cls.FS, bg0=bg0), **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(self.FS)) + assert smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) is smoother + + @pytest.mark.parametrize( + "method, shape", + [ + ("position", (50, 3)), + ("velocity", (50, 3)), + ("quaternion", (50, 4)), + ("euler", (50, 3)), + ("bias_gyro", (50, 3)), + ], + ) + def test_state_shapes(self, method, shape): + _, smoother = self._run(n_samples=shape[0]) + assert getattr(smoother, method)().shape == shape + + def test_P_shape(self): + _, smoother = self._run(n_samples=50) + assert smoother.P.shape == (50, 12, 12) + + @pytest.mark.parametrize( + "method, shape", + [ + ("position", (0, 3)), + ("velocity", (0, 3)), + ("quaternion", (0, 4)), + ("euler", (0, 3)), + ("bias_gyro", (0, 3)), + ], + ) + def test_state_shapes_without_updates(self, method, shape): + smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + assert getattr(smoother, method)().shape == shape + + def test_P_shape_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + assert smoother.P.shape == (0, 12, 12) + + @pytest.mark.parametrize( + "method, attribute", + [ + ("position", "_p_n"), + ("velocity", "_v_n"), + ("quaternion", "_q_nb"), + ("bias_gyro", "_bg_b"), + ("P", "_P"), + ], + ) + def test_state_methods_return_copies(self, method, attribute): + _, smoother = self._run(n_samples=10) + out = getattr(smoother, method) + out = out if method == "P" else out() # 'P' is a property + assert out is not getattr(smoother, attribute) + + @pytest.mark.parametrize( + "method", + ["position", "velocity", "quaternion", "euler", "bias_gyro"], + ) + def test_last_sample_equals_forward_filter(self, method): + """ + The RTS backward sweep leaves the last time step uncorrected, so it must + equal the forward filter estimate. + """ + mekf, smoother = self._run(n_samples=50, bg0=(0.01, -0.02, 0.03)) + np.testing.assert_allclose( + getattr(smoother, method)()[-1], getattr(mekf, method)() + ) + + def test_P_last_sample_equals_forward_filter(self): + mekf, smoother = self._run(n_samples=50) + np.testing.assert_allclose(smoother.P[-1], mekf.P) + def test_bias_gyro(self): bg0 = np.array([0.01, -0.02, 0.03]) - smoother = FixedIntervalSmoother(PVAMEKF(10.0, bg0=bg0)) - for _ in range(10): - smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) + _, smoother = self._run(n_samples=10, bg0=bg0) bg_rad = smoother.bias_gyro() np.testing.assert_allclose(smoother.bias_gyro(degrees=False), bg_rad) np.testing.assert_allclose(smoother.bias_gyro(degrees=True), np.degrees(bg_rad)) assert smoother.bias_gyro() is not smoother._bg_b # copy + def test_euler_degrees(self): + _, smoother = self._run(n_samples=10) + + euler_rad = smoother.euler() + np.testing.assert_allclose(smoother.euler(degrees=False), euler_rad) + np.testing.assert_allclose(smoother.euler(degrees=True), np.degrees(euler_rad)) + + def test_euler_matches_quaternion(self): + _, smoother = self._run(n_samples=10) + + quaternion = smoother.quaternion() + np.testing.assert_allclose(np.linalg.norm(quaternion, axis=1), 1.0) + + quaternion_expect = np.array( + [sf.quaternion_from_euler(theta_i) for theta_i in smoother.euler()] + ) + # The quaternion and its negative describe the same rotation + quaternion_expect *= np.sign(quaternion_expect[:, 0:1] * quaternion[:, 0:1]) + np.testing.assert_allclose(quaternion, quaternion_expect, atol=1e-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_accessor_before_further_updates(self): + """ + Reading an estimate must not prevent the smoother from accounting for + measurements that are applied afterwards. + """ + _, smoother = self._run(n_samples=20) + smoother.position() # trigger a backward sweep + + for _ in range(5): + smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) + + _, smoother_expect = self._run(n_samples=20) + for _ in range(5): + smoother_expect.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) + + assert smoother.position().shape == (25, 3) + np.testing.assert_allclose(smoother.position(), smoother_expect.position()) + + def test_cov_smoothing_false(self): + """ + Disabling covariance smoothing returns the forward filter covariances, and + leaves the smoothed state estimates unchanged. + """ + n_samples = 30 + rng = np.random.default_rng(0) + dvel = np.array([0.0, 0.0, -sf.gravity() / self.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(self.FS) + smoother = FixedIntervalSmoother(PVAMEKF(self.FS), cov_smoothing=False) + smoother_cov = FixedIntervalSmoother(PVAMEKF(self.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()) + @pytest.mark.parametrize( "benchmark_gen", [ From 438d77ba92683c1c1e9f35933fa4d8d0654ab2be Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 12:03:20 +0200 Subject: [PATCH 22/34] delete a few tests --- tests/test_ins/test_smoothing.py | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index ae7cf4b3..41030e24 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -146,24 +146,6 @@ def test_smoothing_is_idempotent(self): second = smoother.position() np.testing.assert_array_equal(first, second) - def test_accessor_before_further_updates(self): - """ - Reading an estimate must not prevent the smoother from accounting for - measurements that are applied afterwards. - """ - _, smoother = self._run(n_samples=20) - smoother.position() # trigger a backward sweep - - for _ in range(5): - smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) - - _, smoother_expect = self._run(n_samples=20) - for _ in range(5): - smoother_expect.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) - - assert smoother.position().shape == (25, 3) - np.testing.assert_allclose(smoother.position(), smoother_expect.position()) - def test_cov_smoothing_false(self): """ Disabling covariance smoothing returns the forward filter covariances, and From 7b7637e497d39ac4945d1956007417c46edf6781 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 12:12:30 +0200 Subject: [PATCH 23/34] add a clear method to FixedIntervalSmoother --- src/smsfusion/_ins/_smoothing.py | 9 +++++ tests/test_ins/test_smoothing.py | 68 ++++++++++++++++++++++++++++++++ 2 files changed, 77 insertions(+) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 2690b4ce..b69f0d2e 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -37,7 +37,16 @@ class FixedIntervalSmoother: 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 error covariance estimates. This + resets the smoother, and prepares it for a new interval of measurements. + + NB! The underlying PVAMEKF instance is not affected. The forward filtering + continues from its current state. + """ # Buffers with estimates from the forward pass self._p_buf: list[NDArray[np.float64]] = [] self._v_buf: list[NDArray[np.float64]] = [] diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 41030e24..0b86ca80 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -183,6 +183,74 @@ def test_cov_smoothing_false(self): 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(self.FS)) + 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) + + def test_clear_allows_reuse(self): + """ + After clearing, the smoother covers the subsequent interval only. The forward + filtering carries on, so the result must equal that of a smoother attached to + a filter in the same state. + """ + n_samples = 15 + rng = np.random.default_rng(0) + dvel = np.array([0.0, 0.0, -sf.gravity() / self.FS]) + rng.normal( + 0.0, 1.0e-3, (2 * n_samples, 3) + ) + dtheta = rng.normal(0.0, 1.0e-3, (2 * n_samples, 3)) + + # Buffer the first interval, clear it, then buffer the second interval + smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): + smoother.update(dvel_i, dtheta_i) + smoother.position() # populate the smoothed estimates + smoother.clear() + for dvel_i, dtheta_i in zip(dvel[n_samples:], dtheta[n_samples:]): + smoother.update(dvel_i, dtheta_i) + + # Advance an identical filter over the first interval without buffering it + mekf_expect = PVAMEKF(self.FS) + for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): + mekf_expect.update(dvel_i, dtheta_i) + smoother_expect = FixedIntervalSmoother(mekf_expect) + for dvel_i, dtheta_i in zip(dvel[n_samples:], dtheta[n_samples:]): + smoother_expect.update(dvel_i, dtheta_i) + + assert smoother.position().shape == (n_samples, 3) + np.testing.assert_allclose(smoother.position(), smoother_expect.position()) + np.testing.assert_allclose(smoother.euler(), smoother_expect.euler()) + np.testing.assert_allclose(smoother.bias_gyro(), smoother_expect.bias_gyro()) + np.testing.assert_allclose(smoother.P, smoother_expect.P) + @pytest.mark.parametrize( "benchmark_gen", [ From 276cf72d73582c1832b6102334adbfc344b1f192 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 12:14:12 +0200 Subject: [PATCH 24/34] copy phi --- src/smsfusion/_ins/_smoothing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index b69f0d2e..1623f0e2 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -91,7 +91,7 @@ def _smooth(self) -> None: np.array(self._dx_buf), np.array(self._dvel_buf), np.array(self._dtheta_buf), - self._mekf._phi, + self._mekf._phi.copy(), self._mekf._Q, self._cov_smoothing, ) From d16ec9928b3d220930694bd112feb661f1abbd37 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 12:19:02 +0200 Subject: [PATCH 25/34] docstring euler degrees --- src/smsfusion/_ins/_smoothing.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 1623f0e2..4e50c6bd 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -113,6 +113,11 @@ 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) From 0bf2b454b317baa7d8acdf216d8dcf47be5f310d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 12:23:17 +0200 Subject: [PATCH 26/34] docstring update params --- src/smsfusion/_ins/_smoothing.py | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 4e50c6bd..486024db 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -66,7 +66,27 @@ def clear(self) -> None: def update(self, *args: Any, **kwargs: Any) -> Self: """ - Update with IMU and aiding measurements. + 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()) From b951345403e89209bb0943bf4031d0edbdc24424 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 20:56:25 +0200 Subject: [PATCH 27/34] remove bg from test run --- tests/test_ins/test_smoothing.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 0b86ca80..17634e01 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -18,7 +18,7 @@ class Test_FixedIntervalSmoother: FS = 10.0 @classmethod - def _run(cls, n_samples=50, seed=0, bg0=(0.0, 0.0, 0.0), **smoother_kwargs): + def _run(cls, 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. @@ -29,8 +29,8 @@ def _run(cls, n_samples=50, seed=0, bg0=(0.0, 0.0, 0.0), **smoother_kwargs): ) dtheta = rng.normal(0.0, 1.0e-3, (n_samples, 3)) - mekf = PVAMEKF(cls.FS, bg0=bg0) - smoother = FixedIntervalSmoother(PVAMEKF(cls.FS, bg0=bg0), **smoother_kwargs) + mekf = PVAMEKF(cls.FS) + smoother = FixedIntervalSmoother(PVAMEKF(cls.FS), **smoother_kwargs) for dvel_i, dtheta_i in zip(dvel, dtheta): mekf.update(dvel_i, dtheta_i) smoother.update(dvel_i, dtheta_i) @@ -101,7 +101,7 @@ def test_last_sample_equals_forward_filter(self, method): The RTS backward sweep leaves the last time step uncorrected, so it must equal the forward filter estimate. """ - mekf, smoother = self._run(n_samples=50, bg0=(0.01, -0.02, 0.03)) + mekf, smoother = self._run(n_samples=50) np.testing.assert_allclose( getattr(smoother, method)()[-1], getattr(mekf, method)() ) @@ -111,8 +111,7 @@ def test_P_last_sample_equals_forward_filter(self): np.testing.assert_allclose(smoother.P[-1], mekf.P) def test_bias_gyro(self): - bg0 = np.array([0.01, -0.02, 0.03]) - _, smoother = self._run(n_samples=10, bg0=bg0) + _, smoother = self._run(n_samples=10) bg_rad = smoother.bias_gyro() np.testing.assert_allclose(smoother.bias_gyro(degrees=False), bg_rad) From 49e01ed6ff49acbd8d895e85adec73978b4ccc70 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 21:02:02 +0200 Subject: [PATCH 28/34] comment --- tests/test_ins/test_smoothing.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index 17634e01..e1352a42 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -15,7 +15,7 @@ class Test_FixedIntervalSmoother: - FS = 10.0 + FS = 10.0 # sampling frequency in Hz @classmethod def _run(cls, n_samples=50, seed=0, **smoother_kwargs): From a435f1f98f6b2fb8cd01adefedc9ee7225ce26ff Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 21:35:47 +0200 Subject: [PATCH 29/34] test each methid instead of parametrizing --- tests/test_ins/test_smoothing.py | 152 +++++++++++++++---------------- 1 file changed, 72 insertions(+), 80 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index e1352a42..f7f58b88 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -40,103 +40,95 @@ def test_update_returns_self(self): smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) assert smoother.update(np.array([0.0, 0.0, -0.98]), np.zeros(3)) is smoother - @pytest.mark.parametrize( - "method, shape", - [ - ("position", (50, 3)), - ("velocity", (50, 3)), - ("quaternion", (50, 4)), - ("euler", (50, 3)), - ("bias_gyro", (50, 3)), - ], - ) - def test_state_shapes(self, method, shape): - _, smoother = self._run(n_samples=shape[0]) - assert getattr(smoother, method)().shape == shape + def test_position(self): + mekf, smoother = self._run(n_samples=50) + position = smoother.position() - def test_P_shape(self): - _, smoother = self._run(n_samples=50) - assert smoother.P.shape == (50, 12, 12) + assert position.shape == (50, 3) + assert position is not smoother._p_n # copy - @pytest.mark.parametrize( - "method, shape", - [ - ("position", (0, 3)), - ("velocity", (0, 3)), - ("quaternion", (0, 4)), - ("euler", (0, 3)), - ("bias_gyro", (0, 3)), - ], - ) - def test_state_shapes_without_updates(self, method, shape): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) - assert getattr(smoother, method)().shape == shape + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(position[-1], mekf.position()) - def test_P_shape_without_updates(self): + def test_position_without_updates(self): smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) - assert smoother.P.shape == (0, 12, 12) + assert smoother.position().shape == (0, 3) - @pytest.mark.parametrize( - "method, attribute", - [ - ("position", "_p_n"), - ("velocity", "_v_n"), - ("quaternion", "_q_nb"), - ("bias_gyro", "_bg_b"), - ("P", "_P"), - ], - ) - def test_state_methods_return_copies(self, method, attribute): - _, smoother = self._run(n_samples=10) - out = getattr(smoother, method) - out = out if method == "P" else out() # 'P' is a property - assert out is not getattr(smoother, attribute) + def test_velocity(self): + mekf, smoother = self._run(n_samples=50) + velocity = smoother.velocity() - @pytest.mark.parametrize( - "method", - ["position", "velocity", "quaternion", "euler", "bias_gyro"], - ) - def test_last_sample_equals_forward_filter(self, method): - """ - The RTS backward sweep leaves the last time step uncorrected, so it must - equal the forward filter estimate. - """ + 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(self.FS)) + assert smoother.velocity().shape == (0, 3) + + def test_quaternion(self): mekf, smoother = self._run(n_samples=50) - np.testing.assert_allclose( - getattr(smoother, method)()[-1], getattr(mekf, method)() - ) + quaternion = smoother.quaternion() - def test_P_last_sample_equals_forward_filter(self): + 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(self.FS)) + assert smoother.quaternion().shape == (0, 4) + + def test_euler(self): mekf, smoother = self._run(n_samples=50) - np.testing.assert_allclose(smoother.P[-1], mekf.P) + 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(self.FS)) + assert smoother.euler().shape == (0, 3) def test_bias_gyro(self): - _, smoother = self._run(n_samples=10) + mekf, smoother = self._run(n_samples=50) + bias_gyro = smoother.bias_gyro() - bg_rad = smoother.bias_gyro() - np.testing.assert_allclose(smoother.bias_gyro(degrees=False), bg_rad) - np.testing.assert_allclose(smoother.bias_gyro(degrees=True), np.degrees(bg_rad)) - assert smoother.bias_gyro() is not smoother._bg_b # copy + 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) + ) - def test_euler_degrees(self): - _, smoother = self._run(n_samples=10) + # The RTS backward sweep leaves the last time step uncorrected + np.testing.assert_allclose(bias_gyro[-1], mekf.bias_gyro()) - euler_rad = smoother.euler() - np.testing.assert_allclose(smoother.euler(degrees=False), euler_rad) - np.testing.assert_allclose(smoother.euler(degrees=True), np.degrees(euler_rad)) + def test_bias_gyro_without_updates(self): + smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + assert smoother.bias_gyro().shape == (0, 3) - def test_euler_matches_quaternion(self): - _, smoother = self._run(n_samples=10) + def test_P(self): + mekf, smoother = self._run(n_samples=50) + P = smoother.P - quaternion = smoother.quaternion() - np.testing.assert_allclose(np.linalg.norm(quaternion, axis=1), 1.0) + assert P.shape == (50, 12, 12) + assert P is not smoother._P # copy - quaternion_expect = np.array( - [sf.quaternion_from_euler(theta_i) for theta_i in smoother.euler()] - ) - # The quaternion and its negative describe the same rotation - quaternion_expect *= np.sign(quaternion_expect[:, 0:1] * quaternion[:, 0:1]) - np.testing.assert_allclose(quaternion, quaternion_expect, atol=1e-12) + # 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(self.FS)) + assert smoother.P.shape == (0, 12, 12) def test_smoothing_is_idempotent(self): _, smoother = self._run(n_samples=20) From 14f1cb64b5c77a21a8b5625fd954f14c43be0cfa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 22:02:05 +0200 Subject: [PATCH 30/34] remove FS class variable --- tests/test_ins/test_smoothing.py | 41 ++++++++++++++++---------------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index f7f58b88..cf014720 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -15,29 +15,28 @@ class Test_FixedIntervalSmoother: - FS = 10.0 # sampling frequency in Hz - @classmethod def _run(cls, 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() / cls.FS]) + rng.normal( + 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(cls.FS) - smoother = FixedIntervalSmoother(PVAMEKF(cls.FS), **smoother_kwargs) + 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(self.FS)) + 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): @@ -51,7 +50,7 @@ def test_position(self): np.testing.assert_allclose(position[-1], mekf.position()) def test_position_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.position().shape == (0, 3) def test_velocity(self): @@ -65,7 +64,7 @@ def test_velocity(self): np.testing.assert_allclose(velocity[-1], mekf.velocity()) def test_velocity_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.velocity().shape == (0, 3) def test_quaternion(self): @@ -80,7 +79,7 @@ def test_quaternion(self): np.testing.assert_allclose(quaternion[-1], mekf.quaternion()) def test_quaternion_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.quaternion().shape == (0, 4) def test_euler(self): @@ -95,7 +94,7 @@ def test_euler(self): np.testing.assert_allclose(euler[-1], mekf.euler()) def test_euler_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.euler().shape == (0, 3) def test_bias_gyro(self): @@ -113,7 +112,7 @@ def test_bias_gyro(self): np.testing.assert_allclose(bias_gyro[-1], mekf.bias_gyro()) def test_bias_gyro_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.bias_gyro().shape == (0, 3) def test_P(self): @@ -127,7 +126,7 @@ def test_P(self): np.testing.assert_allclose(P[-1], mekf.P) def test_P_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) assert smoother.P.shape == (0, 12, 12) def test_smoothing_is_idempotent(self): @@ -142,17 +141,18 @@ 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() / self.FS]) + rng.normal( + 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(self.FS) - smoother = FixedIntervalSmoother(PVAMEKF(self.FS), cov_smoothing=False) - smoother_cov = FixedIntervalSmoother(PVAMEKF(self.FS), cov_smoothing=True) + 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): @@ -188,7 +188,7 @@ def test_clear(self): assert smoother.P.shape == (0, 12, 12) def test_clear_without_updates(self): - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(10.0)) smoother.clear() assert smoother.position().shape == (0, 3) @@ -212,15 +212,16 @@ def test_clear_allows_reuse(self): filtering carries on, so the result must equal that of a smoother attached to a filter in the same state. """ + fs = 10.0 n_samples = 15 rng = np.random.default_rng(0) - dvel = np.array([0.0, 0.0, -sf.gravity() / self.FS]) + rng.normal( + dvel = np.array([0.0, 0.0, -sf.gravity() / fs]) + rng.normal( 0.0, 1.0e-3, (2 * n_samples, 3) ) dtheta = rng.normal(0.0, 1.0e-3, (2 * n_samples, 3)) # Buffer the first interval, clear it, then buffer the second interval - smoother = FixedIntervalSmoother(PVAMEKF(self.FS)) + smoother = FixedIntervalSmoother(PVAMEKF(fs)) for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): smoother.update(dvel_i, dtheta_i) smoother.position() # populate the smoothed estimates @@ -229,7 +230,7 @@ def test_clear_allows_reuse(self): smoother.update(dvel_i, dtheta_i) # Advance an identical filter over the first interval without buffering it - mekf_expect = PVAMEKF(self.FS) + mekf_expect = PVAMEKF(fs) for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): mekf_expect.update(dvel_i, dtheta_i) smoother_expect = FixedIntervalSmoother(mekf_expect) From 114a3226ae9432ca1c472a33e66cba564f8cd0e3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 22:03:39 +0200 Subject: [PATCH 31/34] _run as normal method --- tests/test_ins/test_smoothing.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index cf014720..ae3e8c0f 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -15,8 +15,7 @@ class Test_FixedIntervalSmoother: - @classmethod - def _run(cls, n_samples=50, seed=0, **smoother_kwargs): + 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. From c8f307ace02022133396f8dfd70bd99f2b5679fa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 18 Sep 2026 22:06:53 +0200 Subject: [PATCH 32/34] delete complicated clear test --- tests/test_ins/test_smoothing.py | 37 -------------------------------- 1 file changed, 37 deletions(-) diff --git a/tests/test_ins/test_smoothing.py b/tests/test_ins/test_smoothing.py index ae3e8c0f..d0fd37e0 100644 --- a/tests/test_ins/test_smoothing.py +++ b/tests/test_ins/test_smoothing.py @@ -205,43 +205,6 @@ def test_clear_does_not_affect_filter(self): np.testing.assert_array_equal(smoother._mekf.euler(), euler) np.testing.assert_array_equal(smoother._mekf.P, P) - def test_clear_allows_reuse(self): - """ - After clearing, the smoother covers the subsequent interval only. The forward - filtering carries on, so the result must equal that of a smoother attached to - a filter in the same state. - """ - fs = 10.0 - n_samples = 15 - rng = np.random.default_rng(0) - dvel = np.array([0.0, 0.0, -sf.gravity() / fs]) + rng.normal( - 0.0, 1.0e-3, (2 * n_samples, 3) - ) - dtheta = rng.normal(0.0, 1.0e-3, (2 * n_samples, 3)) - - # Buffer the first interval, clear it, then buffer the second interval - smoother = FixedIntervalSmoother(PVAMEKF(fs)) - for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): - smoother.update(dvel_i, dtheta_i) - smoother.position() # populate the smoothed estimates - smoother.clear() - for dvel_i, dtheta_i in zip(dvel[n_samples:], dtheta[n_samples:]): - smoother.update(dvel_i, dtheta_i) - - # Advance an identical filter over the first interval without buffering it - mekf_expect = PVAMEKF(fs) - for dvel_i, dtheta_i in zip(dvel[:n_samples], dtheta[:n_samples]): - mekf_expect.update(dvel_i, dtheta_i) - smoother_expect = FixedIntervalSmoother(mekf_expect) - for dvel_i, dtheta_i in zip(dvel[n_samples:], dtheta[n_samples:]): - smoother_expect.update(dvel_i, dtheta_i) - - assert smoother.position().shape == (n_samples, 3) - np.testing.assert_allclose(smoother.position(), smoother_expect.position()) - np.testing.assert_allclose(smoother.euler(), smoother_expect.euler()) - np.testing.assert_allclose(smoother.bias_gyro(), smoother_expect.bias_gyro()) - np.testing.assert_allclose(smoother.P, smoother_expect.P) - @pytest.mark.parametrize( "benchmark_gen", [ From 40aadd376c6258b954354dee12473f4f5855647c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 21 Sep 2026 08:19:09 +0200 Subject: [PATCH 33/34] docstring fixes --- src/smsfusion/_ins/_smoothing.py | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 486024db..227ef899 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -41,11 +41,8 @@ def __init__(self, mekf: PVAMEKF, cov_smoothing: bool = True) -> None: def clear(self) -> None: """ - Clear the internal buffers of state and error covariance estimates. This - resets the smoother, and prepares it for a new interval of measurements. - - NB! The underlying PVAMEKF instance is not affected. The forward filtering - continues from its current state. + 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]] = [] @@ -69,8 +66,8 @@ 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. + The arguments are passed on to the underlying PVAMEKF instance unaltered. + See :meth:`smsfusion.PVAMEKF.update` for a full description of them. Parameters ---------- @@ -118,7 +115,7 @@ def _smooth(self) -> None: def quaternion(self) -> NDArray[np.float64]: """ - Smoothed quaternion estimates. + Smoothed unit quaternion estimates. Returns ------- @@ -200,16 +197,16 @@ def bias_gyro(self, degrees: bool = False) -> NDArray[np.float64]: @property def P(self) -> NDArray[np.float64]: """ - Smoothed error covariance estimates. + Error covariance matrix estimates. - NB! If the smoother was created with ``cov_smoothing=False``, the forward - filter's (i.e., unsmoothed) error covariance estimates are returned instead. + 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 estimates for each of the N time steps where the smoother - has been updated with measurements. + 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() From 68fbbcb63d1bc8fa452df4cd45c49c6c9bc8b6bd Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 21 Sep 2026 08:25:55 +0200 Subject: [PATCH 34/34] small fix --- src/smsfusion/_ins/_smoothing.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins/_smoothing.py b/src/smsfusion/_ins/_smoothing.py index 227ef899..9643b0c1 100644 --- a/src/smsfusion/_ins/_smoothing.py +++ b/src/smsfusion/_ins/_smoothing.py @@ -224,7 +224,7 @@ def _rts_backward_sweep( dtheta: NDArray[np.float64], phi_k: NDArray[np.float64], Q: NDArray[np.float64], - cov_smoothing: bool = True, + cov_smoothing: bool, ) -> tuple[ NDArray[np.float64], NDArray[np.float64], @@ -254,7 +254,7 @@ def _rts_backward_sweep( if cov_smoothing: P[k] += A @ (P[k + 1] - P_prior_kp1) @ A.T - # Update smoothed state estimates + # 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])