From d9c426910d838faf27f16242bcaece34d915a692 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 6 Nov 2025 17:25:49 +0100 Subject: [PATCH 001/129] docstring --- src/smsfusion/benchmark/_coning.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 src/smsfusion/benchmark/_coning.py diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py new file mode 100644 index 00000000..f2f06f19 --- /dev/null +++ b/src/smsfusion/benchmark/_coning.py @@ -0,0 +1,15 @@ +class ConingTrajectorySimulator: + """ + Coning trajectory generator and IMU simulator. + + A coning trajectory is defined as a circular motion of a vector, r, of constant + amplitude, making a constant angle, theta, with respect to a fixed axis. Here, + the fixed axis is the z-axis. + """ + + + def __init__(self): + pass + + def __call__(self, n: int): + pass \ No newline at end of file From f0bf3172892fac5f277c3f3ebb6accb1cfca923a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 6 Nov 2025 17:30:26 +0100 Subject: [PATCH 002/129] docstring --- src/smsfusion/benchmark/_coning.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index f2f06f19..7fce1029 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -5,6 +5,17 @@ class ConingTrajectorySimulator: A coning trajectory is defined as a circular motion of a vector, r, of constant amplitude, making a constant angle, theta, with respect to a fixed axis. Here, the fixed axis is the z-axis. + + Let, + - R be the amplitude of the vector, r. + - theta be the coning (half) angle. I.e., the angle between r and the z-axis. + - phi be the heading angle. I.e., the angle between the projection of r onto the + x-y plane and the x-axis. + + Then, the vector, r, can be expressed as: + + r(t) = R * [sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)]^T + """ From f1963f716e7c37cdcddafcbbd7518260be695867 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 09:42:07 +0100 Subject: [PATCH 003/129] some changes --- src/smsfusion/benchmark/_coning.py | 178 ++++++++++++++++++++++++++--- 1 file changed, 162 insertions(+), 16 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 7fce1029..b7be6377 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -1,26 +1,172 @@ +import numpy as np + + +# class ConingTrajectorySimulator: +# """ +# Coning trajectory generator and IMU simulator. + +# A coning trajectory is defined as a circular motion of a vector, r, of constant +# amplitude, making a constant angle, theta, with respect to a fixed axis. Here, +# the fixed axis is the z-axis. + +# Let, +# - R be the amplitude of the vector, r. +# - theta be the coning (half) angle. I.e., the angle between r and the z-axis. +# - phi be the heading angle. I.e., the angle between the projection of r onto the +# x-y plane and the x-axis. + +# Then, the vector, r, can be expressed as: + +# r(t) = R * [sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)]^T + +# """ + + +# def __init__(self): +# pass + +# def __call__(self, n: int): +# pass + + +# class ConingTrajectorySimulator: +# """ +# Coning trajectory generator and IMU simulator. +# """ + + +# def __init__(self): +# pass + +# def __call__(self, n: int): +# pass + + class ConingTrajectorySimulator: """ - Coning trajectory generator and IMU simulator. + Coning trajectory generator and IMU (gyro) simulator. - A coning trajectory is defined as a circular motion of a vector, r, of constant - amplitude, making a constant angle, theta, with respect to a fixed axis. Here, - the fixed axis is the z-axis. + Internally uses 313 Euler angles with constant nutation (cone half-angle) β, + constant precession rate Ω about inertial Z, and constant spin rate σ about + the body Z. Orientation is then converted to ZYX (yaw, pitch, roll). - Let, - - R be the amplitude of the vector, r. - - theta be the coning (half) angle. I.e., the angle between r and the z-axis. - - phi be the heading angle. I.e., the angle between the projection of r onto the - x-y plane and the x-axis. - - Then, the vector, r, can be expressed as: + What you get from calling the simulator: + - t: (n,) time array [s] + - euler_zyx: (n, 3) array of [yaw, pitch, roll] in radians (ZYX convention) + - omega_b: (n, 3) array of body angular rates [p, q, r] in rad/s + (these are exactly what a 3-axis gyro in the body frame measures) + + Parameters + ---------- + beta : float, rad + Cone half-angle (nutation). β = 0 is pure spin with no coning. + omega_prec : float, rad/s + Precession rate Ω about the inertial Z axis. + sigma_spin : float, rad/s + Spin rate σ about the body Z axis. + dt : float, s + Sampling interval. + psi0 : float, rad + Initial precession angle ψ(0). + phi0 : float, rad + Initial spin angle φ(0). - r(t) = R * [sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)]^T + Notes + ----- + 313 angles are: + ψ(t) = psi0 + Ω t (precession) + θ(t) = β (nutation, constant) + φ(t) = phi0 + σ t (spin) + Rotation matrix (body->inertial): R = Rz(ψ) Rx(β) Rz(φ) + ZYX (yaw–pitch–roll) extraction from R: + yaw = atan2(R21, R11) + pitch = -arcsin(R31) + roll = atan2(R32, R33) + + Body angular velocity (what gyros measure), for 313 with θ=β const: + p = Ω sinβ sinφ + q = Ω sinβ cosφ + r = σ + Ω cosβ """ + def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0): + self._beta = beta + self._w_prec = omega_prec + self._w_spin = omega_spin + self._psi0 = 0.0 + self._phi0 = 0.0 + + def __call__(self, fs: float, n: int): + """ + Generate a length-n coning trajectory sample. + + Parameters + ---------- + n : int + Number of samples to generate. + + Returns + ------- + t : ndarray, shape (n,) + Time vector in seconds. + euler_zyx : ndarray, shape (n, 3) + ZYX Euler angles [yaw, pitch, roll] in radians. + omega_b : ndarray, shape (n, 3) + Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). + """ + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + # ZYZ Euler angles + psi = self._psi0 + self._w_prec * t # precession + theta = self._beta * np.ones_like(t) # constant nutation + phi = self._phi0 + self._w_spin * t # spin + + cpsi, spsi = np.cos(psi), np.sin(psi) + cth, sth = np.cos(theta), np.sin(theta) + cphi, sphi = np.cos(phi), np.sin(phi) + + # Rotation matrices (vectorized): R = Rz(psi) @ Rx(theta) @ Rz(phi) + # Build component matrices for each t: shape (n, 3, 3) + Rz_psi = np.stack([ + np.stack([ cpsi, -spsi, np.zeros_like(t)], axis=-1), + np.stack([ spsi, cpsi, np.zeros_like(t)], axis=-1), + np.stack([ np.zeros_like(t), np.zeros_like(t), np.ones_like(t)], axis=-1) + ], axis=-2) # (n, 3, 3) + + Rx_th = np.array([[1.0, 0.0, 0.0], + [0.0, cth, -sth], + [0.0, sth, cth]], dtype=float) # (3,3) constant + + Rz_phi = np.stack([ + np.stack([ cphi, -sphi, np.zeros_like(t)], axis=-1), + np.stack([ sphi, cphi, np.zeros_like(t)], axis=-1), + np.stack([ np.zeros_like(t), np.zeros_like(t), np.ones_like(t)], axis=-1) + ], axis=-2) # (n, 3, 3) + + # Multiply R = Rz(psi) @ Rx(th) @ Rz(phi) for all t using einsum + # First A = Rz(psi) @ Rx(th) -> (n,3,3) + A = np.einsum('nij,jk->nik', Rz_psi, Rx_th) + # Then R = A @ Rz(phi) -> (n,3,3) + R = np.einsum('nij,njk->nik', A, Rz_phi) + + # Extract ZYX (yaw, pitch, roll) from R + R11, R21, R31 = R[:, 0, 0], R[:, 1, 0], R[:, 2, 0] + R32, R33 = R[:, 2, 1], R[:, 2, 2] + + yaw = np.arctan2(R21, R11) + pitch = -np.arcsin(np.clip(R31, -1.0, 1.0)) + roll = np.arctan2(R32, R33) + + euler_zyx = np.stack([yaw, pitch, roll], axis=-1) - def __init__(self): - pass + # IMU gyros (body angular velocity) for uniform coning + p = self.omega_prec * sth * sphi + q = self.omega_prec * sth * cphi + r = self.sigma_spin + self.omega_prec * cth + omega_b = np.stack([p, q, np.full_like(p, r)], axis=-1) - def __call__(self, n: int): - pass \ No newline at end of file + return t, euler_zyx, omega_b From 196d6d0991a4532a65e738fb2a2520bbcb5bd295 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 12:01:42 +0100 Subject: [PATCH 004/129] some changes --- src/smsfusion/benchmark/__init__.py | 2 + src/smsfusion/benchmark/_coning.py | 140 +++++++++++----------------- 2 files changed, 56 insertions(+), 86 deletions(-) diff --git a/src/smsfusion/benchmark/__init__.py b/src/smsfusion/benchmark/__init__.py index 40e4ab88..d4bf578e 100644 --- a/src/smsfusion/benchmark/__init__.py +++ b/src/smsfusion/benchmark/__init__.py @@ -7,6 +7,7 @@ benchmark_pure_attitude_beat_202311A, benchmark_pure_attitude_chirp_202311A, ) +from ._coning import ConingSimulator __all__ = [ "_benchmark_helper", @@ -16,4 +17,5 @@ "benchmark_pure_attitude_beat_202311A", "benchmark_pure_attitude_chirp_202311A", "ChirpSignal", + "ConingSimulator", ] diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index b7be6377..fe16ce5a 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -42,67 +42,58 @@ # pass -class ConingTrajectorySimulator: +class ConingSimulator: """ Coning trajectory generator and IMU (gyro) simulator. - Internally uses 313 Euler angles with constant nutation (cone half-angle) β, - constant precession rate Ω about inertial Z, and constant spin rate σ about - the body Z. Orientation is then converted to ZYX (yaw, pitch, roll). - - What you get from calling the simulator: - - t: (n,) time array [s] - - euler_zyx: (n, 3) array of [yaw, pitch, roll] in radians (ZYX convention) - - omega_b: (n, 3) array of body angular rates [p, q, r] in rad/s - (these are exactly what a 3-axis gyro in the body frame measures) - Parameters ---------- - beta : float, rad - Cone half-angle (nutation). β = 0 is pure spin with no coning. - omega_prec : float, rad/s - Precession rate Ω about the inertial Z axis. - sigma_spin : float, rad/s - Spin rate σ about the body Z axis. - dt : float, s - Sampling interval. - psi0 : float, rad - Initial precession angle ψ(0). - phi0 : float, rad - Initial spin angle φ(0). - - Notes - ----- - 313 angles are: - ψ(t) = psi0 + Ω t (precession) - θ(t) = β (nutation, constant) - φ(t) = phi0 + σ t (spin) - Rotation matrix (body->inertial): R = Rz(ψ) Rx(β) Rz(φ) - - ZYX (yaw–pitch–roll) extraction from R: - yaw = atan2(R21, R11) - pitch = -arcsin(R31) - roll = atan2(R32, R33) - - Body angular velocity (what gyros measure), for 313 with θ=β const: - p = Ω sinβ sinφ - q = Ω sinβ cosφ - r = σ + Ω cosβ + omega_prec : float + Precession angular velocity in rad/s. + omega_spin : float + Spin angular velocity in rad/s. + beta : float + Cone half-angle in radians. + degrees: bool + Whether to interpret beta in degrees (True) or radians (False), and angular + velocities in deg/s (True) or rad/s (False). """ - def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0): + def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0, degrees: bool = False): self._beta = beta self._w_prec = omega_prec self._w_spin = omega_spin self._psi0 = 0.0 self._phi0 = 0.0 + if degrees: + self._beta = np.deg2rad(self._beta) + self._w_prec = np.deg2rad(self._w_prec) + self._w_spin = np.deg2rad(self._w_spin) + + @staticmethod + def _rot_matrix_from_euler_zyz(psi: np.ndarray, theta: np.ndarray, phi: np.ndarray) -> np.ndarray: + cpsi, spsi = np.cos(psi), np.sin(psi) + ctheta, stheta = np.cos(theta), np.sin(theta) + cphi, sphi = np.cos(phi), np.sin(phi) + + R_zyz = np.stack([ + np.stack([cpsi * cphi - spsi * ctheta * sphi, -cpsi * sphi - spsi * ctheta * cphi, spsi * stheta], axis=-1), + np.stack([spsi * cphi + cpsi * ctheta * sphi, -spsi * sphi + cpsi * ctheta * cphi, -cpsi * stheta], axis=-1), + np.stack([stheta * sphi, stheta * cphi, ctheta], axis=-1) + ], axis=-2) + + return R_zyz + def __call__(self, fs: float, n: int): """ - Generate a length-n coning trajectory sample. + Generate a length-n coning trajectory and corresponding body-frame angular + velocities as measured by an IMU sensor. Parameters ---------- + fs : float + Sampling frequency in Hz. n : int Number of samples to generate. @@ -121,52 +112,29 @@ def __call__(self, fs: float, n: int): t = dt * np.arange(n) # ZYZ Euler angles - psi = self._psi0 + self._w_prec * t # precession - theta = self._beta * np.ones_like(t) # constant nutation - phi = self._phi0 + self._w_spin * t # spin - - cpsi, spsi = np.cos(psi), np.sin(psi) - cth, sth = np.cos(theta), np.sin(theta) - cphi, sphi = np.cos(phi), np.sin(phi) - - # Rotation matrices (vectorized): R = Rz(psi) @ Rx(theta) @ Rz(phi) - # Build component matrices for each t: shape (n, 3, 3) - Rz_psi = np.stack([ - np.stack([ cpsi, -spsi, np.zeros_like(t)], axis=-1), - np.stack([ spsi, cpsi, np.zeros_like(t)], axis=-1), - np.stack([ np.zeros_like(t), np.zeros_like(t), np.ones_like(t)], axis=-1) - ], axis=-2) # (n, 3, 3) - - Rx_th = np.array([[1.0, 0.0, 0.0], - [0.0, cth, -sth], - [0.0, sth, cth]], dtype=float) # (3,3) constant - - Rz_phi = np.stack([ - np.stack([ cphi, -sphi, np.zeros_like(t)], axis=-1), - np.stack([ sphi, cphi, np.zeros_like(t)], axis=-1), - np.stack([ np.zeros_like(t), np.zeros_like(t), np.ones_like(t)], axis=-1) - ], axis=-2) # (n, 3, 3) - - # Multiply R = Rz(psi) @ Rx(th) @ Rz(phi) for all t using einsum - # First A = Rz(psi) @ Rx(th) -> (n,3,3) - A = np.einsum('nij,jk->nik', Rz_psi, Rx_th) - # Then R = A @ Rz(phi) -> (n,3,3) - R = np.einsum('nij,njk->nik', A, Rz_phi) - - # Extract ZYX (yaw, pitch, roll) from R - R11, R21, R31 = R[:, 0, 0], R[:, 1, 0], R[:, 2, 0] - R32, R33 = R[:, 2, 1], R[:, 2, 2] - + psi = self._psi0 + self._w_prec * t # precession angle + theta = self._beta * np.ones_like(t) # constant cone half-angle + phi = self._phi0 + self._w_spin * t # spin angle + + R = self._rot_matrix_from_euler_zyz(psi, theta, phi) + + # Extract ZYX (yaw, pitch, roll) Euler angles + R11 = R[:, 0, 0] + R21 = R[:, 1, 0] + R31 = R[:, 2, 0] + R32 = R[:, 2, 1] + R33 = R[:, 2, 2] yaw = np.arctan2(R21, R11) pitch = -np.arcsin(np.clip(R31, -1.0, 1.0)) roll = np.arctan2(R32, R33) - euler_zyx = np.stack([yaw, pitch, roll], axis=-1) + euler_zyx = np.column_stack([roll, pitch, yaw]) - # IMU gyros (body angular velocity) for uniform coning - p = self.omega_prec * sth * sphi - q = self.omega_prec * sth * cphi - r = self.sigma_spin + self.omega_prec * cth - omega_b = np.stack([p, q, np.full_like(p, r)], axis=-1) + # Body frame angular velocities + p = self._w_prec * np.sin(theta) * np.sin(phi) + q = self._w_prec * np.sin(theta) * np.cos(phi) + r = self._w_spin + self._w_prec * np.cos(theta) + r = np.full_like(p, r) + w_b = np.column_stack([p, q, r]) - return t, euler_zyx, omega_b + return t, euler_zyx, w_b From 1a562f1b4917dd3971e28d57ea4c0420d280edab Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 12:25:13 +0100 Subject: [PATCH 005/129] small fix --- src/smsfusion/benchmark/_coning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index fe16ce5a..58790718 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -59,7 +59,7 @@ class ConingSimulator: velocities in deg/s (True) or rad/s (False). """ - def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0, degrees: bool = False): + def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0, degrees: bool = True): self._beta = beta self._w_prec = omega_prec self._w_spin = omega_spin From 4103cb6549e8a7f4cfa427a427fdaef95d54d7e6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 12:48:28 +0100 Subject: [PATCH 006/129] move euler from rot_matrix to method --- src/smsfusion/benchmark/_coning.py | 32 +++++++++++++++++++----------- 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 58790718..59150052 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -85,6 +85,22 @@ def _rot_matrix_from_euler_zyz(psi: np.ndarray, theta: np.ndarray, phi: np.ndarr return R_zyz + @staticmethod + def _euler_from_rot_matrix_zyx(R): + R11 = R[:, 0, 0] + R21 = R[:, 1, 0] + R31 = R[:, 2, 0] + R32 = R[:, 2, 1] + R33 = R[:, 2, 2] + yaw = np.arctan2(R21, R11) + # pitch = -np.arcsin(np.clip(R31, -1.0, 1.0)) + pitch = -np.arcsin(R31) + roll = np.arctan2(R32, R33) + + euler_zyx = np.column_stack([roll, pitch, yaw]) + + return euler_zyx + def __call__(self, fs: float, n: int): """ Generate a length-n coning trajectory and corresponding body-frame angular @@ -116,21 +132,13 @@ def __call__(self, fs: float, n: int): theta = self._beta * np.ones_like(t) # constant cone half-angle phi = self._phi0 + self._w_spin * t # spin angle + # Rotation matrix (body-to-inertial) R = self._rot_matrix_from_euler_zyz(psi, theta, phi) - # Extract ZYX (yaw, pitch, roll) Euler angles - R11 = R[:, 0, 0] - R21 = R[:, 1, 0] - R31 = R[:, 2, 0] - R32 = R[:, 2, 1] - R33 = R[:, 2, 2] - yaw = np.arctan2(R21, R11) - pitch = -np.arcsin(np.clip(R31, -1.0, 1.0)) - roll = np.arctan2(R32, R33) - - euler_zyx = np.column_stack([roll, pitch, yaw]) + # Extract ZYX Euler angles (yaw, pitch, roll) + euler_zyx = self._euler_from_rot_matrix_zyx(R) - # Body frame angular velocities + # Body frame angular velocities from ZYZ Euler angle rates p = self._w_prec * np.sin(theta) * np.sin(phi) q = self._w_prec * np.sin(theta) * np.cos(phi) r = self._w_spin + self._w_prec * np.cos(theta) From 801e0c205f8c8cdc81557f3a5a4e1b0b4a0c73aa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 13:14:11 +0100 Subject: [PATCH 007/129] zyz convention --- src/smsfusion/benchmark/_coning.py | 65 ++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 12 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 59150052..b7679561 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -71,19 +71,55 @@ def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float self._w_prec = np.deg2rad(self._w_prec) self._w_spin = np.deg2rad(self._w_spin) + # @staticmethod + # def _rot_matrix_from_euler_zxz(psi: np.ndarray, theta: np.ndarray, phi: np.ndarray) -> np.ndarray: + # cpsi, spsi = np.cos(psi), np.sin(psi) + # ctheta, stheta = np.cos(theta), np.sin(theta) + # cphi, sphi = np.cos(phi), np.sin(phi) + + # R_zyz = np.stack([ + # np.stack([cpsi * cphi - spsi * ctheta * sphi, -cpsi * sphi - spsi * ctheta * cphi, spsi * stheta], axis=-1), + # np.stack([spsi * cphi + cpsi * ctheta * sphi, -spsi * sphi + cpsi * ctheta * cphi, -cpsi * stheta], axis=-1), + # np.stack([stheta * sphi, stheta * cphi, ctheta], axis=-1) + # ], axis=-2) + + # return R_zyz + + # def _body_rates_from_euler_zxz(self, psi, theta, phi): + # p = self._w_prec * np.sin(theta) * np.sin(phi) + # q = self._w_prec * np.sin(theta) * np.cos(phi) + # r = self._w_spin + self._w_prec * np.cos(theta) + # r = np.full_like(p, r) + # w_b = np.column_stack([p, q, r]) + + # return w_b + @staticmethod - def _rot_matrix_from_euler_zyz(psi: np.ndarray, theta: np.ndarray, phi: np.ndarray) -> np.ndarray: + def _rot_matrix_from_euler_zyz(psi, theta, phi): + """ + Euler ZYZ rotation matrix: + R = Rz(psi) @ Ry(theta) @ Rz(phi) + + Parameters + ---------- + psi, theta, phi : array_like + Euler angles (ZYZ) in radians. Broadcasting is supported. + + Returns + ------- + R : ndarray + Rotation matrix/matrices of shape (..., 3, 3). + """ cpsi, spsi = np.cos(psi), np.sin(psi) ctheta, stheta = np.cos(theta), np.sin(theta) cphi, sphi = np.cos(phi), np.sin(phi) - R_zyz = np.stack([ - np.stack([cpsi * cphi - spsi * ctheta * sphi, -cpsi * sphi - spsi * ctheta * cphi, spsi * stheta], axis=-1), - np.stack([spsi * cphi + cpsi * ctheta * sphi, -spsi * sphi + cpsi * ctheta * cphi, -cpsi * stheta], axis=-1), - np.stack([stheta * sphi, stheta * cphi, ctheta], axis=-1) + R = np.stack([ + np.stack([ cpsi*ctheta*cphi - spsi*sphi, -cpsi*ctheta*sphi - spsi*cphi, cpsi*stheta ], axis=-1), + np.stack([ spsi*ctheta*cphi + cpsi*sphi, -spsi*ctheta*sphi + cpsi*cphi, spsi*stheta ], axis=-1), + np.stack([ -stheta*cphi, stheta*sphi, ctheta ], axis=-1) ], axis=-2) - - return R_zyz + return R @staticmethod def _euler_from_rot_matrix_zyx(R): @@ -101,6 +137,15 @@ def _euler_from_rot_matrix_zyx(R): return euler_zyx + def _body_rates_from_euler_zyz(self, psi, theta, phi): + p = -self._w_prec * np.sin(theta) * np.cos(phi) + q = self._w_prec * np.sin(theta) * np.sin(phi) + r = self._w_spin + self._w_prec * np.cos(theta) + r = np.full_like(p, r) + w_b = np.column_stack([p, q, r]) + + return w_b + def __call__(self, fs: float, n: int): """ Generate a length-n coning trajectory and corresponding body-frame angular @@ -139,10 +184,6 @@ def __call__(self, fs: float, n: int): euler_zyx = self._euler_from_rot_matrix_zyx(R) # Body frame angular velocities from ZYZ Euler angle rates - p = self._w_prec * np.sin(theta) * np.sin(phi) - q = self._w_prec * np.sin(theta) * np.cos(phi) - r = self._w_spin + self._w_prec * np.cos(theta) - r = np.full_like(p, r) - w_b = np.column_stack([p, q, r]) + w_b = self._body_rates_from_euler_zyz(psi, theta, phi) return t, euler_zyx, w_b From d519c54e72efa7d5b977ab0821882fd0a97b220e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 14:26:26 +0100 Subject: [PATCH 008/129] version 2 --- src/smsfusion/benchmark/_coning.py | 72 ++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index b7679561..dec5a743 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -187,3 +187,75 @@ def __call__(self, fs: float, n: int): w_b = self._body_rates_from_euler_zyz(psi, theta, phi) return t, euler_zyx, w_b + + + +class ConingSimulator2: + """ + Coning trajectory generator and IMU (gyro) simulator. + + Parameters + ---------- + omega_prec : float + Precession angular velocity in rad/s. + omega_spin : float + Spin angular velocity in rad/s. + beta : float + Cone half-angle in radians. + degrees: bool + Whether to interpret beta in degrees (True) or radians (False), and angular + velocities in deg/s (True) or rad/s (False). + """ + + def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, degrees: bool = True): + self._w_prec = omega_prec + self._w_spin = omega_spin + self._gamma0 = 0.0 + self._alpha0 = 0.0 + self._beta = 0.0 + + if degrees: + self._w_prec = np.deg2rad(self._w_prec) + self._w_spin = np.deg2rad(self._w_spin) + + def __call__(self, fs: float, n: int): + """ + Generate a length-n coning trajectory and corresponding body-frame angular + velocities as measured by an IMU sensor. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + + Returns + ------- + t : ndarray, shape (n,) + Time vector in seconds. + euler_zyx : ndarray, shape (n, 3) + ZYX Euler angles [yaw, pitch, roll] in radians. + omega_b : ndarray, shape (n, 3) + Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). + """ + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + # ZYX Euler angles + alpha_dot = self._w_spin + gamma_dot = self._w_prec + alpha = self._alpha0 + alpha_dot * t # spin angle + beta = self._beta * np.ones_like(t) # constant cone half-angle + gamma = self._gamma0 + gamma_dot * t # precession angle + euler = np.column_stack([alpha, beta, gamma]) + + # Body frame angular velocities from ZYZ Euler angle rates + w_x = alpha_dot * np.ones_like(t) + w_y = gamma_dot * np.sin(alpha) + w_z = gamma_dot * np.cos(alpha) + w_b = np.column_stack([w_x, w_y, w_z]) + + return t, euler, w_b From fd5c5dc066d602e5093a1485bbb9da6e916ba6d6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 14:31:01 +0100 Subject: [PATCH 009/129] some changes --- src/smsfusion/benchmark/_coning.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index dec5a743..c11ef1e6 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -210,9 +210,6 @@ class ConingSimulator2: def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, degrees: bool = True): self._w_prec = omega_prec self._w_spin = omega_spin - self._gamma0 = 0.0 - self._alpha0 = 0.0 - self._beta = 0.0 if degrees: self._w_prec = np.deg2rad(self._w_prec) @@ -245,11 +242,13 @@ def __call__(self, fs: float, n: int): t = dt * np.arange(n) # ZYX Euler angles + alpha0 = 0.0 + gamma0 = 0.0 alpha_dot = self._w_spin gamma_dot = self._w_prec - alpha = self._alpha0 + alpha_dot * t # spin angle - beta = self._beta * np.ones_like(t) # constant cone half-angle - gamma = self._gamma0 + gamma_dot * t # precession angle + alpha = alpha0 + alpha_dot * t # spin angle + gamma = gamma0 + gamma_dot * t # precession angle + beta = 0.0 * np.ones_like(t) # constant euler = np.column_stack([alpha, beta, gamma]) # Body frame angular velocities from ZYZ Euler angle rates From dc70269b703dd6b7e586feb8789222c3009c458f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:05:09 +0100 Subject: [PATCH 010/129] some changes --- src/smsfusion/benchmark/_coning.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index c11ef1e6..f77ad26c 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -244,11 +244,12 @@ def __call__(self, fs: float, n: int): # ZYX Euler angles alpha0 = 0.0 gamma0 = 0.0 + beta0 = 0.0 alpha_dot = self._w_spin gamma_dot = self._w_prec alpha = alpha0 + alpha_dot * t # spin angle gamma = gamma0 + gamma_dot * t # precession angle - beta = 0.0 * np.ones_like(t) # constant + beta = beta0 * np.ones_like(t) # constant euler = np.column_stack([alpha, beta, gamma]) # Body frame angular velocities from ZYZ Euler angle rates From 4849d0971ac01429efbe4b706e0f140256ecf7ab Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:19:12 +0100 Subject: [PATCH 011/129] simplify rot matrix from euler xyx --- src/smsfusion/benchmark/_coning.py | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index f77ad26c..eef05fbb 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -114,11 +114,29 @@ def _rot_matrix_from_euler_zyz(psi, theta, phi): ctheta, stheta = np.cos(theta), np.sin(theta) cphi, sphi = np.cos(phi), np.sin(phi) - R = np.stack([ - np.stack([ cpsi*ctheta*cphi - spsi*sphi, -cpsi*ctheta*sphi - spsi*cphi, cpsi*stheta ], axis=-1), - np.stack([ spsi*ctheta*cphi + cpsi*sphi, -spsi*ctheta*sphi + cpsi*cphi, spsi*stheta ], axis=-1), - np.stack([ -stheta*cphi, stheta*sphi, ctheta ], axis=-1) - ], axis=-2) + R11 = cpsi * ctheta * cphi - spsi * sphi + R12 = -cpsi * ctheta * sphi - spsi * cphi + R13 = cpsi*stheta + R21 = spsi*ctheta*cphi + cpsi*sphi + R22 = -spsi*ctheta*sphi + cpsi*cphi + R23 = spsi*stheta + R31 = -stheta*cphi + R32 = stheta*sphi + R33 = ctheta + + n = len(psi) + R = np.empty((n, 3, 3), dtype="float64") + + R[..., 0, 0] = R11 + R[..., 0, 1] = R12 + R[..., 0, 2] = R13 + R[..., 1, 0] = R21 + R[..., 1, 1] = R22 + R[..., 1, 2] = R23 + R[..., 2, 0] = R31 + R[..., 2, 1] = R32 + R[..., 2, 2] = R33 + return R @staticmethod @@ -129,7 +147,6 @@ def _euler_from_rot_matrix_zyx(R): R32 = R[:, 2, 1] R33 = R[:, 2, 2] yaw = np.arctan2(R21, R11) - # pitch = -np.arcsin(np.clip(R31, -1.0, 1.0)) pitch = -np.arcsin(R31) roll = np.arctan2(R32, R33) From e59baea76e19892096ccfecfbecf3150716ad26d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:22:59 +0100 Subject: [PATCH 012/129] some changes --- src/smsfusion/benchmark/_coning.py | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index eef05fbb..dd303bf7 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -71,29 +71,6 @@ def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float self._w_prec = np.deg2rad(self._w_prec) self._w_spin = np.deg2rad(self._w_spin) - # @staticmethod - # def _rot_matrix_from_euler_zxz(psi: np.ndarray, theta: np.ndarray, phi: np.ndarray) -> np.ndarray: - # cpsi, spsi = np.cos(psi), np.sin(psi) - # ctheta, stheta = np.cos(theta), np.sin(theta) - # cphi, sphi = np.cos(phi), np.sin(phi) - - # R_zyz = np.stack([ - # np.stack([cpsi * cphi - spsi * ctheta * sphi, -cpsi * sphi - spsi * ctheta * cphi, spsi * stheta], axis=-1), - # np.stack([spsi * cphi + cpsi * ctheta * sphi, -spsi * sphi + cpsi * ctheta * cphi, -cpsi * stheta], axis=-1), - # np.stack([stheta * sphi, stheta * cphi, ctheta], axis=-1) - # ], axis=-2) - - # return R_zyz - - # def _body_rates_from_euler_zxz(self, psi, theta, phi): - # p = self._w_prec * np.sin(theta) * np.sin(phi) - # q = self._w_prec * np.sin(theta) * np.cos(phi) - # r = self._w_spin + self._w_prec * np.cos(theta) - # r = np.full_like(p, r) - # w_b = np.column_stack([p, q, r]) - - # return w_b - @staticmethod def _rot_matrix_from_euler_zyz(psi, theta, phi): """ From d1fa86e3e0140d2434ee1c63e22751c876ccd727 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:23:33 +0100 Subject: [PATCH 013/129] style --- src/smsfusion/benchmark/_coning.py | 32 ++++++++++++++++++------------ 1 file changed, 19 insertions(+), 13 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index dd303bf7..bebdce6d 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -1,6 +1,5 @@ import numpy as np - # class ConingTrajectorySimulator: # """ # Coning trajectory generator and IMU simulator. @@ -14,7 +13,7 @@ # - theta be the coning (half) angle. I.e., the angle between r and the z-axis. # - phi be the heading angle. I.e., the angle between the projection of r onto the # x-y plane and the x-axis. - + # Then, the vector, r, can be expressed as: # r(t) = R * [sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)]^T @@ -59,7 +58,13 @@ class ConingSimulator: velocities in deg/s (True) or rad/s (False). """ - def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, beta: float = 10.0, degrees: bool = True): + def __init__( + self, + omega_prec: float = 1.0, + omega_spin: float = 2.0, + beta: float = 10.0, + degrees: bool = True, + ): self._beta = beta self._w_prec = omega_prec self._w_spin = omega_spin @@ -93,12 +98,12 @@ def _rot_matrix_from_euler_zyz(psi, theta, phi): R11 = cpsi * ctheta * cphi - spsi * sphi R12 = -cpsi * ctheta * sphi - spsi * cphi - R13 = cpsi*stheta - R21 = spsi*ctheta*cphi + cpsi*sphi - R22 = -spsi*ctheta*sphi + cpsi*cphi - R23 = spsi*stheta - R31 = -stheta*cphi - R32 = stheta*sphi + R13 = cpsi * stheta + R21 = spsi * ctheta * cphi + cpsi * sphi + R22 = -spsi * ctheta * sphi + cpsi * cphi + R23 = spsi * stheta + R31 = -stheta * cphi + R32 = stheta * sphi R33 = ctheta n = len(psi) @@ -123,9 +128,9 @@ def _euler_from_rot_matrix_zyx(R): R31 = R[:, 2, 0] R32 = R[:, 2, 1] R33 = R[:, 2, 2] - yaw = np.arctan2(R21, R11) + yaw = np.arctan2(R21, R11) pitch = -np.arcsin(R31) - roll = np.arctan2(R32, R33) + roll = np.arctan2(R32, R33) euler_zyx = np.column_stack([roll, pitch, yaw]) @@ -181,7 +186,6 @@ def __call__(self, fs: float, n: int): w_b = self._body_rates_from_euler_zyz(psi, theta, phi) return t, euler_zyx, w_b - class ConingSimulator2: @@ -201,7 +205,9 @@ class ConingSimulator2: velocities in deg/s (True) or rad/s (False). """ - def __init__(self, omega_prec: float = 1.0, omega_spin: float = 2.0, degrees: bool = True): + def __init__( + self, omega_prec: float = 1.0, omega_spin: float = 2.0, degrees: bool = True + ): self._w_prec = omega_prec self._w_spin = omega_spin From ee7438d5d82322f2105f1334d2c858c1e4725e9e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:42:46 +0100 Subject: [PATCH 014/129] comment --- src/smsfusion/benchmark/_coning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index bebdce6d..ed16feee 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -139,7 +139,7 @@ def _euler_from_rot_matrix_zyx(R): def _body_rates_from_euler_zyz(self, psi, theta, phi): p = -self._w_prec * np.sin(theta) * np.cos(phi) q = self._w_prec * np.sin(theta) * np.sin(phi) - r = self._w_spin + self._w_prec * np.cos(theta) + r = self._w_spin + self._w_prec * np.cos(theta) # constant r = np.full_like(p, r) w_b = np.column_stack([p, q, r]) From 6db8baf50ea08f62f276137d03fe771483580362 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:43:07 +0100 Subject: [PATCH 015/129] delete old code --- src/smsfusion/benchmark/_coning.py | 40 ------------------------------ 1 file changed, 40 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index ed16feee..ca809fa8 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -1,45 +1,5 @@ import numpy as np -# class ConingTrajectorySimulator: -# """ -# Coning trajectory generator and IMU simulator. - -# A coning trajectory is defined as a circular motion of a vector, r, of constant -# amplitude, making a constant angle, theta, with respect to a fixed axis. Here, -# the fixed axis is the z-axis. - -# Let, -# - R be the amplitude of the vector, r. -# - theta be the coning (half) angle. I.e., the angle between r and the z-axis. -# - phi be the heading angle. I.e., the angle between the projection of r onto the -# x-y plane and the x-axis. - -# Then, the vector, r, can be expressed as: - -# r(t) = R * [sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta)]^T - -# """ - - -# def __init__(self): -# pass - -# def __call__(self, n: int): -# pass - - -# class ConingTrajectorySimulator: -# """ -# Coning trajectory generator and IMU simulator. -# """ - - -# def __init__(self): -# pass - -# def __call__(self, n: int): -# pass - class ConingSimulator: """ From 4a9c02a0ffcbc178debfea78651ee001d85173c9 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 16:59:59 +0100 Subject: [PATCH 016/129] docstring --- src/smsfusion/benchmark/_coning.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index ca809fa8..8ed35121 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -5,14 +5,23 @@ class ConingSimulator: """ Coning trajectory generator and IMU (gyro) simulator. + Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with + respect to the inertial frame's z-axis. The sensor rotates about its z-axis + with a constant rate (the spin rate), while also spinning around the inertial + frame's z-axis with a constant rate (the precession rate). The IMU sensor's + z-axis will thus trace out a cone shape, with the half-angle defined by beta. + Parameters ---------- omega_prec : float - Precession angular velocity in rad/s. + Precession angular velocity in rad/s. I.e., the rate at which the IMU sensor's + z-axis rotates about the inertial frame's z-axis. omega_spin : float - Spin angular velocity in rad/s. + Spin angular velocity in rad/s. I.e., the rate at which the IMU sensor + rotates about its own z-axis. beta : float - Cone half-angle in radians. + Cone half-angle in radians. I.e., the angle between the IMU sensor's z-axis + and the inertial frame's z-axis. degrees: bool Whether to interpret beta in degrees (True) or radians (False), and angular velocities in deg/s (True) or rad/s (False). From 5dbafd626c5cd2407d2bf5102187c44706468bf7 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 7 Nov 2025 17:01:54 +0100 Subject: [PATCH 017/129] some changes --- src/smsfusion/benchmark/_coning.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 8ed35121..3d3438cc 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -29,9 +29,9 @@ class ConingSimulator: def __init__( self, - omega_prec: float = 1.0, - omega_spin: float = 2.0, - beta: float = 10.0, + omega_prec: float = 180.0, + omega_spin: float = 360.0, + beta: float = 45.0, degrees: bool = True, ): self._beta = beta From 0005280f1a425edd8ee6be03fd2b2934d1830ac4 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 10:49:16 +0100 Subject: [PATCH 018/129] version 3 --- src/smsfusion/benchmark/_coning.py | 163 +++++++++++++++++++++++++++++ 1 file changed, 163 insertions(+) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 3d3438cc..657ab353 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -228,3 +228,166 @@ def __call__(self, fs: float, n: int): w_b = np.column_stack([w_x, w_y, w_z]) return t, euler, w_b + + +class ConingSimulator3: + """ + Coning trajectory generator and IMU (gyro) simulator. + + Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with + respect to the inertial frame's z-axis. The sensor rotates about its z-axis + with a constant rate (the spin rate), while also spinning around the inertial + frame's z-axis with a constant rate (the precession rate). The IMU sensor's + z-axis will thus trace out a cone shape, with the half-angle defined by beta. + + Parameters + ---------- + psi_fun : callable + Precession angle in radians as a function of time. I.e., rotation of IMU + sensor's z-axis about the inertial frame's z-axis. + psi_dot_fun : callable + Precession angular velocity in rad/s as a function of time. + phi_fun : callable + Spin angle in radians as a function of time. + phi_dot_fun : callable + Spin angular velocity in rad/s as a function of time. I.e., the rate at + which the IMU sensor rotates about its own z-axis. + beta : float + Cone half-angle in radians (constant). I.e., the angle between the IMU sensor's + z-axis and the inertial frame's z-axis. + degrees: bool + Whether to interpret beta in degrees (True) or radians (False), and angular + velocities in deg/s (True) or rad/s (False). + """ + + def __init__( + self, + psi: callable, + psi_dot: callable, + phi: callable, + phi_dot: callable, + beta: float = np.pi/4.0, + degrees: bool = False, + ): + self._beta = (np.pi / 180.0) * beta if degrees else beta + self._psi = psi + self._psi_dot = psi_dot + self._phi = phi + self._phi_dot = phi_dot + self._psi0 = 0.0 + self._phi0 = 0.0 + + @staticmethod + def _rot_matrix_from_euler_zyz(psi, theta, phi): + """ + Euler ZYZ rotation matrix: + R = Rz(psi) @ Ry(theta) @ Rz(phi) + + Parameters + ---------- + psi, theta, phi : array_like + Euler angles (ZYZ) in radians. Broadcasting is supported. + + Returns + ------- + R : ndarray + Rotation matrix/matrices of shape (..., 3, 3). + """ + cpsi, spsi = np.cos(psi), np.sin(psi) + ctheta, stheta = np.cos(theta), np.sin(theta) + cphi, sphi = np.cos(phi), np.sin(phi) + + R11 = cpsi * ctheta * cphi - spsi * sphi + R12 = -cpsi * ctheta * sphi - spsi * cphi + R13 = cpsi * stheta + R21 = spsi * ctheta * cphi + cpsi * sphi + R22 = -spsi * ctheta * sphi + cpsi * cphi + R23 = spsi * stheta + R31 = -stheta * cphi + R32 = stheta * sphi + R33 = ctheta + + n = len(psi) + R = np.empty((n, 3, 3), dtype="float64") + + R[..., 0, 0] = R11 + R[..., 0, 1] = R12 + R[..., 0, 2] = R13 + R[..., 1, 0] = R21 + R[..., 1, 1] = R22 + R[..., 1, 2] = R23 + R[..., 2, 0] = R31 + R[..., 2, 1] = R32 + R[..., 2, 2] = R33 + + return R + + @staticmethod + def _euler_from_rot_matrix_zyx(R): + R11 = R[:, 0, 0] + R21 = R[:, 1, 0] + R31 = R[:, 2, 0] + R32 = R[:, 2, 1] + R33 = R[:, 2, 2] + yaw = np.arctan2(R21, R11) + pitch = -np.arcsin(R31) + roll = np.arctan2(R32, R33) + + euler_zyx = np.column_stack([roll, pitch, yaw]) + + return euler_zyx + + def _body_rates_from_euler_zyz(self, theta, phi, psi_dot, phi_dot): + p = -psi_dot * np.sin(theta) * np.cos(phi) + q = psi_dot * np.sin(theta) * np.sin(phi) + r = phi_dot + psi_dot * np.cos(theta) + r = np.full_like(p, r) + w_b = np.column_stack([p, q, r]) + + return w_b + + def __call__(self, fs: float, n: int): + """ + Generate a length-n coning trajectory and corresponding body-frame angular + velocities as measured by an IMU sensor. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + + Returns + ------- + t : ndarray, shape (n,) + Time vector in seconds. + euler_zyx : ndarray, shape (n, 3) + ZYX Euler angles [yaw, pitch, roll] in radians. + omega_b : ndarray, shape (n, 3) + Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). + """ + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + # ZYZ Euler angles + psi = self._psi(t) # precession angle + theta = self._beta * np.ones_like(t) # constant cone half-angle + phi = self._phit() # spin angle + + # Euler rates + psi_dot = self._psi_dot(t) + phi_dot = self._phi_dot(t) + + # Rotation matrix (body-to-inertial) + R = self._rot_matrix_from_euler_zyz(psi, theta, phi) + + # Extract ZYX Euler angles (yaw, pitch, roll) + euler_zyx = self._euler_from_rot_matrix_zyx(R) + + # Body frame angular velocities from ZYZ Euler angle rates + w_b = self._body_rates_from_euler_zyz(theta, phi, psi_dot, phi_dot) + + return t, euler_zyx, w_b From fd12f4418fd89725d8008dbc64e07bcb623f0daf Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 10:50:00 +0100 Subject: [PATCH 019/129] small fix --- src/smsfusion/benchmark/_coning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 657ab353..4290b12a 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -375,7 +375,7 @@ def __call__(self, fs: float, n: int): # ZYZ Euler angles psi = self._psi(t) # precession angle theta = self._beta * np.ones_like(t) # constant cone half-angle - phi = self._phit() # spin angle + phi = self._phi(t) # spin angle # Euler rates psi_dot = self._psi_dot(t) From 08af17c645001b15453d543c235ee55c2ed714c3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 11:14:08 +0100 Subject: [PATCH 020/129] some changes --- src/smsfusion/benchmark/_coning.py | 35 ++++++++++++++++++------------ 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 4290b12a..f7cc099f 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -262,18 +262,14 @@ class ConingSimulator3: def __init__( self, - psi: callable, - psi_dot: callable, - phi: callable, - phi_dot: callable, + psi_sim, + phi_sim, beta: float = np.pi/4.0, degrees: bool = False, ): self._beta = (np.pi / 180.0) * beta if degrees else beta - self._psi = psi - self._psi_dot = psi_dot - self._phi = phi - self._phi_dot = phi_dot + self._psi_sim = psi_sim + self._phi_sim = phi_sim self._psi0 = 0.0 self._phi0 = 0.0 @@ -373,13 +369,9 @@ def __call__(self, fs: float, n: int): t = dt * np.arange(n) # ZYZ Euler angles - psi = self._psi(t) # precession angle + psi, psi_dot = self._psi_sim(fs, n) # precession angle and rate + phi, phi_dot = self._phi_sim(fs, n) theta = self._beta * np.ones_like(t) # constant cone half-angle - phi = self._phi(t) # spin angle - - # Euler rates - psi_dot = self._psi_dot(t) - phi_dot = self._phi_dot(t) # Rotation matrix (body-to-inertial) R = self._rot_matrix_from_euler_zyz(psi, theta, phi) @@ -391,3 +383,18 @@ def __call__(self, fs: float, n: int): w_b = self._body_rates_from_euler_zyz(theta, phi, psi_dot, phi_dot) return t, euler_zyx, w_b + + +class SineSimulator1D: + def __init__(self, omega, phase, freq_hz=False, phase_degrees=False): + self._w = 2.0 * np.pi * omega if freq_hz else omega + self._phase = np.deg2rad(phase) if phase_degrees else phase + + def __call__(self, fs, n): + dt = 1.0 / fs + t = dt * np.arange(n) + + y = np.sin(self._w * t + self._phase) + dydt = self._w * np.cos(self._w * t + self._phase) + + return y, dydt From c1cb13ce1233c150eb7c48d3d7051f5dc6dff374 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 11:20:57 +0100 Subject: [PATCH 021/129] some changes --- src/smsfusion/benchmark/_coning.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index f7cc099f..9718e87e 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -385,7 +385,7 @@ def __call__(self, fs: float, n: int): return t, euler_zyx, w_b -class SineSimulator1D: +class Sine1DSimulator: def __init__(self, omega, phase, freq_hz=False, phase_degrees=False): self._w = 2.0 * np.pi * omega if freq_hz else omega self._phase = np.deg2rad(phase) if phase_degrees else phase From e423ce01bb73373c32481346697e13d06c3a7be9 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 11:57:34 +0100 Subject: [PATCH 022/129] move to simulate file --- src/smsfusion/benchmark/__init__.py | 2 + src/smsfusion/benchmark/_coning.py | 104 +++++++++++++++++++++++--- src/smsfusion/benchmark/_simulate.py | 108 +++++++++++++++++++++++++++ 3 files changed, 202 insertions(+), 12 deletions(-) create mode 100644 src/smsfusion/benchmark/_simulate.py diff --git a/src/smsfusion/benchmark/__init__.py b/src/smsfusion/benchmark/__init__.py index d4bf578e..5b9849b7 100644 --- a/src/smsfusion/benchmark/__init__.py +++ b/src/smsfusion/benchmark/__init__.py @@ -8,6 +8,7 @@ benchmark_pure_attitude_chirp_202311A, ) from ._coning import ConingSimulator +from ._simulate import GyroSimulator __all__ = [ "_benchmark_helper", @@ -18,4 +19,5 @@ "benchmark_pure_attitude_chirp_202311A", "ChirpSignal", "ConingSimulator", + "GyroSimulator", ] diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/benchmark/_coning.py index 9718e87e..a7370c3c 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/benchmark/_coning.py @@ -385,16 +385,96 @@ def __call__(self, fs: float, n: int): return t, euler_zyx, w_b -class Sine1DSimulator: - def __init__(self, omega, phase, freq_hz=False, phase_degrees=False): - self._w = 2.0 * np.pi * omega if freq_hz else omega - self._phase = np.deg2rad(phase) if phase_degrees else phase - - def __call__(self, fs, n): - dt = 1.0 / fs - t = dt * np.arange(n) - - y = np.sin(self._w * t + self._phase) - dydt = self._w * np.cos(self._w * t + self._phase) +# class IMUSimulator: +# """ +# Coning trajectory generator and IMU (gyro) simulator. + +# Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with +# respect to the inertial frame's z-axis. The sensor rotates about its z-axis +# with a constant rate (the spin rate), while also spinning around the inertial +# frame's z-axis with a constant rate (the precession rate). The IMU sensor's +# z-axis will thus trace out a cone shape, with the half-angle defined by beta. + +# Parameters +# ---------- +# alpha_sim : callable +# Roll angle and angular velocity simulator. +# beta_sim : callable +# Pitch angle and angular velocity simulator. +# gamma_sim : callable +# Yaw angle and angular velocity simulator. +# """ + +# def __init__( +# self, +# alpha_sim, +# beta_sim, +# gamma_sim, +# ): +# self._alpha_sim = alpha_sim +# self._beta_sim = beta_sim +# self._gamma_sim = gamma_sim + +# def _angular_velocity_body(self, euler, euler_dot): +# alpha, beta, _ = euler.T +# alpha_dot, beta_dot, gamma_dot = euler_dot.T + +# w_x = alpha_dot - np.sin(beta) * gamma_dot +# w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot +# w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + +# w_b = np.column_stack([w_x, w_y, w_z]) + +# return w_b + +# def __call__(self, fs: float, n: int): +# """ +# Generate a length-n coning trajectory and corresponding body-frame angular +# velocities as measured by an IMU sensor. + +# Parameters +# ---------- +# fs : float +# Sampling frequency in Hz. +# n : int +# Number of samples to generate. + +# Returns +# ------- +# t : ndarray, shape (n,) +# Time vector in seconds. +# euler_zyx : ndarray, shape (n, 3) +# ZYX Euler angles [yaw, pitch, roll] in radians. +# omega_b : ndarray, shape (n, 3) +# Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). +# """ + +# # Time +# dt = 1.0 / fs +# t = dt * np.arange(n) + +# # Euler angles and Euler rates +# alpha, alpha_dot = self._alpha_sim(fs, n) +# beta, beta_dot = self._beta_sim(fs, n) +# gamma, gamma_dot = self._gamma_sim(fs, n) +# euler = np.column_stack([alpha, beta, gamma]) +# euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) + +# w_b = self._angular_velocity_body(euler, euler_dot) + +# return t, euler, w_b + + +# class Sine1DSimulator: +# def __init__(self, omega, phase, freq_hz=False, phase_degrees=False): +# self._w = 2.0 * np.pi * omega if freq_hz else omega +# self._phase = np.deg2rad(phase) if phase_degrees else phase + +# def __call__(self, fs, n): +# dt = 1.0 / fs +# t = dt * np.arange(n) + +# y = np.sin(self._w * t + self._phase) +# dydt = self._w * np.cos(self._w * t + self._phase) - return y, dydt +# return y, dydt diff --git a/src/smsfusion/benchmark/_simulate.py b/src/smsfusion/benchmark/_simulate.py new file mode 100644 index 00000000..2e61bee7 --- /dev/null +++ b/src/smsfusion/benchmark/_simulate.py @@ -0,0 +1,108 @@ +import numpy as np + + +class GyroSimulator: + """ + Coning trajectory generator and IMU (gyro) simulator. + + Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with + respect to the inertial frame's z-axis. The sensor rotates about its z-axis + with a constant rate (the spin rate), while also spinning around the inertial + frame's z-axis with a constant rate (the precession rate). The IMU sensor's + z-axis will thus trace out a cone shape, with the half-angle defined by beta. + + Parameters + ---------- + alpha_sim : callable + Roll angle and angular velocity simulator. + beta_sim : callable + Pitch angle and angular velocity simulator. + gamma_sim : callable + Yaw angle and angular velocity simulator. + """ + + def __init__( + self, + alpha_sim, + beta_sim, + gamma_sim, + ): + self._alpha_sim = alpha_sim + self._beta_sim = beta_sim + self._gamma_sim = gamma_sim + + def _angular_velocity_body(self, euler, euler_dot): + alpha, beta, _ = euler.T + alpha_dot, beta_dot, gamma_dot = euler_dot.T + + w_x = alpha_dot - np.sin(beta) * gamma_dot + w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot + w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + + w_b = np.column_stack([w_x, w_y, w_z]) + + return w_b + + def __call__(self, fs: float, n: int): + """ + Generate a length-n coning trajectory and corresponding body-frame angular + velocities as measured by an IMU sensor. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + + Returns + ------- + t : ndarray, shape (n,) + Time vector in seconds. + euler_zyx : ndarray, shape (n, 3) + ZYX Euler angles [yaw, pitch, roll] in radians. + omega_b : ndarray, shape (n, 3) + Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). + """ + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + # Euler angles and Euler rates + alpha, alpha_dot = self._alpha_sim(fs, n) + beta, beta_dot = self._beta_sim(fs, n) + gamma, gamma_dot = self._gamma_sim(fs, n) + euler = np.column_stack([alpha, beta, gamma]) + euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) + + w_b = self._angular_velocity_body(euler, euler_dot) + + return t, euler, w_b + + +class Sine1DSimulator: + def __init__(self, omega, amp=0.0, phase=0.0, hz=False, phase_degrees=False): + self._w = 2.0 * np.pi * omega if hz else omega + self._amp = amp + self._phase = np.deg2rad(phase) if phase_degrees else phase + + def __call__(self, fs, n): + dt = 1.0 / fs + t = dt * np.arange(n) + + y = self._amp * np.sin(self._w * t + self._phase) + dydt = self._amp * self._w * np.cos(self._w * t + self._phase) + + return y, dydt + + +class Constant1DSimulator: + def __init__(self, const): + self._const = const + + def __call__(self, fs, n): + y = self._const * np.ones(int(n)) + dydt = np.zeros_like(y) + + return y, dydt \ No newline at end of file From 5975a35e46d42b0e531b82f96f1eefc59cf3aa5a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 12:05:29 +0100 Subject: [PATCH 023/129] docstrings --- src/smsfusion/benchmark/_simulate.py | 85 +++++++++++++++++++++++----- 1 file changed, 70 insertions(+), 15 deletions(-) diff --git a/src/smsfusion/benchmark/_simulate.py b/src/smsfusion/benchmark/_simulate.py index 2e61bee7..b6807338 100644 --- a/src/smsfusion/benchmark/_simulate.py +++ b/src/smsfusion/benchmark/_simulate.py @@ -3,13 +3,7 @@ class GyroSimulator: """ - Coning trajectory generator and IMU (gyro) simulator. - - Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with - respect to the inertial frame's z-axis. The sensor rotates about its z-axis - with a constant rate (the spin rate), while also spinning around the inertial - frame's z-axis with a constant rate (the precession rate). The IMU sensor's - z-axis will thus trace out a cone shape, with the half-angle defined by beta. + Gyroscope simulator. Parameters ---------- @@ -19,6 +13,8 @@ class GyroSimulator: Pitch angle and angular velocity simulator. gamma_sim : callable Yaw angle and angular velocity simulator. + degrees: bool + Whether to interpret the simulated angles in degrees (True) or radians (False). """ def __init__( @@ -26,10 +22,12 @@ def __init__( alpha_sim, beta_sim, gamma_sim, + degrees=False, ): self._alpha_sim = alpha_sim self._beta_sim = beta_sim self._gamma_sim = gamma_sim + self._degrees = degrees def _angular_velocity_body(self, euler, euler_dot): alpha, beta, _ = euler.T @@ -43,10 +41,9 @@ def _angular_velocity_body(self, euler, euler_dot): return w_b - def __call__(self, fs: float, n: int): + def __call__(self, fs: float, n: int, degrees=None): """ - Generate a length-n coning trajectory and corresponding body-frame angular - velocities as measured by an IMU sensor. + Generate a length-n gyroscope signal and corresponding Euler angles. Parameters ---------- @@ -54,16 +51,21 @@ def __call__(self, fs: float, n: int): Sampling frequency in Hz. n : int Number of samples to generate. + degrees : bool, optional + Whether to return angles and angular velocities in degrees and degrees + per second (True) or radians and radians per second (False). Returns ------- t : ndarray, shape (n,) Time vector in seconds. - euler_zyx : ndarray, shape (n, 3) - ZYX Euler angles [yaw, pitch, roll] in radians. - omega_b : ndarray, shape (n, 3) - Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). + euler : ndarray, shape (n, 3) + Simulated Euler angles (roll, pitch, yaw). + w_b : ndarray, shape (n, 3) + Simulated angular velocities in the body frame. """ + if degrees is None: + degrees = self._degrees # Time dt = 1.0 / fs @@ -78,16 +80,51 @@ def __call__(self, fs: float, n: int): w_b = self._angular_velocity_body(euler, euler_dot) + if degrees: + euler = np.rad2deg(euler) + w_b = np.rad2deg(w_b) + return t, euler, w_b class Sine1DSimulator: - def __init__(self, omega, amp=0.0, phase=0.0, hz=False, phase_degrees=False): + """ + Sine wave simulator for 1D signals. + + Parameters + ---------- + omega : float + Angular frequency of the sine wave. If `hz` is True, this is interpreted + as frequency in Hz; otherwise, it is interpreted as angular frequency in + radians per second. + amp : float, optional + Amplitude of the sine wave. Default is 1.0. + phase : float, optional + Phase offset of the sine wave. Default is 0.0. + hz : bool, optional + If True, interpret `omega` as frequency in Hz. If False, interpret as angular + frequency in radians per second. Default is False. + phase_degrees : bool, optional + If True, interpret `phase` in degrees. If False, interpret in radians. + Default is False. + """ + + def __init__(self, omega, amp=1.0, phase=0.0, hz=False, phase_degrees=False): self._w = 2.0 * np.pi * omega if hz else omega self._amp = amp self._phase = np.deg2rad(phase) if phase_degrees else phase def __call__(self, fs, n): + """ + Generate a sine wave and its derivative. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ dt = 1.0 / fs t = dt * np.arange(n) @@ -98,10 +135,28 @@ def __call__(self, fs, n): class Constant1DSimulator: + """ + Constant value simulator for 1D signals. + + Parameters + ---------- + const : float + Constant value to simulate. + """ def __init__(self, const): self._const = const def __call__(self, fs, n): + """ + Generate a constant signal and its derivative (always zero). + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ y = self._const * np.ones(int(n)) dydt = np.zeros_like(y) From ffafffffe814da0931d6cff87a00b9a91ab0b99b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 12:33:38 +0100 Subject: [PATCH 024/129] move to simulate modue --- src/smsfusion/__init__.py | 3 ++- src/smsfusion/benchmark/__init__.py | 4 ---- src/smsfusion/simulate/__init__.py | 3 +++ src/smsfusion/{benchmark => simulate}/_coning.py | 4 ++-- src/smsfusion/{benchmark => simulate}/_simulate.py | 11 ++++++----- 5 files changed, 13 insertions(+), 12 deletions(-) create mode 100644 src/smsfusion/simulate/__init__.py rename src/smsfusion/{benchmark => simulate}/_coning.py (99%) rename src/smsfusion/{benchmark => simulate}/_simulate.py (98%) diff --git a/src/smsfusion/__init__.py b/src/smsfusion/__init__.py index fb68cdbb..9e1cd436 100644 --- a/src/smsfusion/__init__.py +++ b/src/smsfusion/__init__.py @@ -1,4 +1,4 @@ -from . import benchmark, calibrate, constants, noise +from . import benchmark, calibrate, constants, noise, simulate from ._ins import AHRS, VRU, AidedINS, FixedNED, StrapdownINS, gravity from ._smoothing import FixedIntervalSmoother from ._transforms import quaternion_from_euler @@ -16,4 +16,5 @@ "StrapdownINS", "VRU", "quaternion_from_euler", + "simulate", ] diff --git a/src/smsfusion/benchmark/__init__.py b/src/smsfusion/benchmark/__init__.py index 5b9849b7..40e4ab88 100644 --- a/src/smsfusion/benchmark/__init__.py +++ b/src/smsfusion/benchmark/__init__.py @@ -7,8 +7,6 @@ benchmark_pure_attitude_beat_202311A, benchmark_pure_attitude_chirp_202311A, ) -from ._coning import ConingSimulator -from ._simulate import GyroSimulator __all__ = [ "_benchmark_helper", @@ -18,6 +16,4 @@ "benchmark_pure_attitude_beat_202311A", "benchmark_pure_attitude_chirp_202311A", "ChirpSignal", - "ConingSimulator", - "GyroSimulator", ] diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py new file mode 100644 index 00000000..210f586f --- /dev/null +++ b/src/smsfusion/simulate/__init__.py @@ -0,0 +1,3 @@ +from ._simulate import GyroSimulator, Sine1DSimulator + +__all__ = ["GyroSimulator", "Sine1DSimulator"] diff --git a/src/smsfusion/benchmark/_coning.py b/src/smsfusion/simulate/_coning.py similarity index 99% rename from src/smsfusion/benchmark/_coning.py rename to src/smsfusion/simulate/_coning.py index a7370c3c..4bcccecf 100644 --- a/src/smsfusion/benchmark/_coning.py +++ b/src/smsfusion/simulate/_coning.py @@ -264,7 +264,7 @@ def __init__( self, psi_sim, phi_sim, - beta: float = np.pi/4.0, + beta: float = np.pi / 4.0, degrees: bool = False, ): self._beta = (np.pi / 180.0) * beta if degrees else beta @@ -476,5 +476,5 @@ def __call__(self, fs: float, n: int): # y = np.sin(self._w * t + self._phase) # dydt = self._w * np.cos(self._w * t + self._phase) - + # return y, dydt diff --git a/src/smsfusion/benchmark/_simulate.py b/src/smsfusion/simulate/_simulate.py similarity index 98% rename from src/smsfusion/benchmark/_simulate.py rename to src/smsfusion/simulate/_simulate.py index b6807338..8b3f3c37 100644 --- a/src/smsfusion/benchmark/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -130,9 +130,9 @@ def __call__(self, fs, n): y = self._amp * np.sin(self._w * t + self._phase) dydt = self._amp * self._w * np.cos(self._w * t + self._phase) - + return y, dydt - + class Constant1DSimulator: """ @@ -143,13 +143,14 @@ class Constant1DSimulator: const : float Constant value to simulate. """ + def __init__(self, const): self._const = const def __call__(self, fs, n): """ Generate a constant signal and its derivative (always zero). - + Parameters ---------- fs : float @@ -159,5 +160,5 @@ def __call__(self, fs, n): """ y = self._const * np.ones(int(n)) dydt = np.zeros_like(y) - - return y, dydt \ No newline at end of file + + return y, dydt From 96acdc72c065bd6ccebd6b3ba1d1299d4c2c7e3c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 12:37:53 +0100 Subject: [PATCH 025/129] docstring --- src/smsfusion/simulate/_simulate.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 8b3f3c37..e3e51744 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -89,7 +89,12 @@ def __call__(self, fs: float, n: int, degrees=None): class Sine1DSimulator: """ - Sine wave simulator for 1D signals. + 1D sine wave simulator. + + Defined as: + + y(t) = amp * sin(omega * t + phase) + dy(t)/dt = amp * omega * cos(omega * t + phase) Parameters ---------- From f213f6a382fb3581c2be3c7f2f533b5c60a312f7 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:00:54 +0100 Subject: [PATCH 026/129] small fix --- src/smsfusion/simulate/_simulate.py | 76 ++++++++++++++++------------- 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index e3e51744..23be02f7 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -72,9 +72,9 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # Euler angles and Euler rates - alpha, alpha_dot = self._alpha_sim(fs, n) - beta, beta_dot = self._beta_sim(fs, n) - gamma, gamma_dot = self._gamma_sim(fs, n) + _, alpha, alpha_dot = self._alpha_sim(fs, n) + _, beta, beta_dot = self._beta_sim(fs, n) + _, gamma, gamma_dot = self._gamma_sim(fs, n) euler = np.column_stack([alpha, beta, gamma]) euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) @@ -93,8 +93,15 @@ class Sine1DSimulator: Defined as: - y(t) = amp * sin(omega * t + phase) - dy(t)/dt = amp * omega * cos(omega * t + phase) + y(t) = A * sin(w * t + phi) + B + dy(t)/dt = A * w * cos(w * t + phi) + + where, + + A: Amplitude of the sine wave. + w: Angular frequency of the sine wave. + phi: Phase offset of the sine wave. + B: Offset of the sine wave. Parameters ---------- @@ -102,10 +109,12 @@ class Sine1DSimulator: Angular frequency of the sine wave. If `hz` is True, this is interpreted as frequency in Hz; otherwise, it is interpreted as angular frequency in radians per second. - amp : float, optional + amp : float, default 1.0 Amplitude of the sine wave. Default is 1.0. - phase : float, optional + phase : float, default 0.0 Phase offset of the sine wave. Default is 0.0. + offset : float, default 0.0 + Offset of the sine wave. Default is 0.0. hz : bool, optional If True, interpret `omega` as frequency in Hz. If False, interpret as angular frequency in radians per second. Default is False. @@ -114,10 +123,11 @@ class Sine1DSimulator: Default is False. """ - def __init__(self, omega, amp=1.0, phase=0.0, hz=False, phase_degrees=False): + def __init__(self, omega, amp=1.0, phase=0.0, offset=0.0, hz=False, phase_degrees=False): self._w = 2.0 * np.pi * omega if hz else omega self._amp = amp self._phase = np.deg2rad(phase) if phase_degrees else phase + self._offset = offset def __call__(self, fs, n): """ @@ -133,37 +143,37 @@ def __call__(self, fs, n): dt = 1.0 / fs t = dt * np.arange(n) - y = self._amp * np.sin(self._w * t + self._phase) + y = self._amp * np.sin(self._w * t + self._phase) + self._offset dydt = self._amp * self._w * np.cos(self._w * t + self._phase) - return y, dydt + return t, y, dydt -class Constant1DSimulator: - """ - Constant value simulator for 1D signals. +# class Constant1DSimulator: +# """ +# Constant value simulator for 1D signals. - Parameters - ---------- - const : float - Constant value to simulate. - """ +# Parameters +# ---------- +# const : float +# Constant value to simulate. +# """ - def __init__(self, const): - self._const = const +# def __init__(self, const): +# self._const = const - def __call__(self, fs, n): - """ - Generate a constant signal and its derivative (always zero). +# def __call__(self, fs, n): +# """ +# Generate a constant signal and its derivative (always zero). - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - """ - y = self._const * np.ones(int(n)) - dydt = np.zeros_like(y) +# Parameters +# ---------- +# fs : float +# Sampling frequency in Hz. +# n : int +# Number of samples to generate. +# """ +# y = self._const * np.ones(int(n)) +# dydt = np.zeros_like(y) - return y, dydt +# return y, dydt From 3463043feff03943724f01eeae5f822a7cada199 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:03:53 +0100 Subject: [PATCH 027/129] rename to sinesinal --- src/smsfusion/simulate/__init__.py | 4 ++-- src/smsfusion/simulate/_simulate.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 210f586f..99d5250c 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import GyroSimulator, Sine1DSimulator +from ._simulate import GyroSimulator, SineSignal -__all__ = ["GyroSimulator", "Sine1DSimulator"] +__all__ = ["GyroSimulator", "SineSignal"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 23be02f7..35c0f9ea 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -87,7 +87,7 @@ def __call__(self, fs: float, n: int, degrees=None): return t, euler, w_b -class Sine1DSimulator: +class SineSignal: """ 1D sine wave simulator. From 91559cec14ea051f4e96563568a26c709b1fc1aa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:33:58 +0100 Subject: [PATCH 028/129] some changes --- src/smsfusion/simulate/_simulate.py | 54 ++++++++++++++--------------- 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 35c0f9ea..4edef555 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -7,27 +7,22 @@ class GyroSimulator: Parameters ---------- - alpha_sim : callable - Roll angle and angular velocity simulator. - beta_sim : callable - Pitch angle and angular velocity simulator. - gamma_sim : callable - Yaw angle and angular velocity simulator. + alpha : SineSignal + Roll signal. + beta : SineSignal + Pitch signal + gamma : SineSignal + Yaw signal degrees: bool - Whether to interpret the simulated angles in degrees (True) or radians (False). + Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ - def __init__( - self, - alpha_sim, - beta_sim, - gamma_sim, - degrees=False, - ): - self._alpha_sim = alpha_sim - self._beta_sim = beta_sim - self._gamma_sim = gamma_sim + def __init__(self, alpha, beta, gamma, degrees=False): + self._alpha_sig = alpha + self._beta_sig = beta + self._gamma_sig = gamma self._degrees = degrees + self._rad_scale = np.pi / 180.0 if degrees else 1.0 def _angular_velocity_body(self, euler, euler_dot): alpha, beta, _ = euler.T @@ -72,12 +67,16 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # Euler angles and Euler rates - _, alpha, alpha_dot = self._alpha_sim(fs, n) - _, beta, beta_dot = self._beta_sim(fs, n) - _, gamma, gamma_dot = self._gamma_sim(fs, n) + _, alpha, alpha_dot = self._alpha_sig(fs, n) + _, beta, beta_dot = self._beta_sig(fs, n) + _, gamma, gamma_dot = self._gamma_sig(fs, n) euler = np.column_stack([alpha, beta, gamma]) euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) + if self._degrees: + euler = np.deg2rad(euler) + euler_dot = np.deg2rad(euler_dot) + w_b = self._angular_velocity_body(euler, euler_dot) if degrees: @@ -105,12 +104,11 @@ class SineSignal: Parameters ---------- - omega : float - Angular frequency of the sine wave. If `hz` is True, this is interpreted - as frequency in Hz; otherwise, it is interpreted as angular frequency in - radians per second. amp : float, default 1.0 Amplitude of the sine wave. Default is 1.0. + omega : float, default 1.0 + Angular frequency of the sine wave in rad/s. Default is 1.0 + radians per second. phase : float, default 0.0 Phase offset of the sine wave. Default is 0.0. offset : float, default 0.0 @@ -123,9 +121,9 @@ class SineSignal: Default is False. """ - def __init__(self, omega, amp=1.0, phase=0.0, offset=0.0, hz=False, phase_degrees=False): - self._w = 2.0 * np.pi * omega if hz else omega + def __init__(self, amp=1.0, omega=1.0, phase=0.0, offset=0.0, phase_degrees=False): self._amp = amp + self._omega = omega self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset @@ -143,8 +141,8 @@ def __call__(self, fs, n): dt = 1.0 / fs t = dt * np.arange(n) - y = self._amp * np.sin(self._w * t + self._phase) + self._offset - dydt = self._amp * self._w * np.cos(self._w * t + self._phase) + y = self._amp * np.sin(self._omega * t + self._phase) + self._offset + dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) return t, y, dydt From a99ce72fcb94f8e65dcbce88a8695fbc82b3cfac Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:35:47 +0100 Subject: [PATCH 029/129] docstring --- src/smsfusion/simulate/_simulate.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 4edef555..ba32c8ff 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -88,7 +88,7 @@ def __call__(self, fs: float, n: int, degrees=None): class SineSignal: """ - 1D sine wave simulator. + 1D sine wave signal generator. Defined as: @@ -97,10 +97,10 @@ class SineSignal: where, - A: Amplitude of the sine wave. - w: Angular frequency of the sine wave. - phi: Phase offset of the sine wave. - B: Offset of the sine wave. + - A : Amplitude of the sine wave. + - w : Angular frequency of the sine wave. + - phi: Phase offset of the sine wave. + - B : Offset of the sine wave. Parameters ---------- From 243ee399c6e1118ef875f10a5e51a44116397d49 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:41:51 +0100 Subject: [PATCH 030/129] some changes --- src/smsfusion/simulate/_simulate.py | 30 ----------------------------- 1 file changed, 30 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index ba32c8ff..1d064372 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -145,33 +145,3 @@ def __call__(self, fs, n): dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) return t, y, dydt - - -# class Constant1DSimulator: -# """ -# Constant value simulator for 1D signals. - -# Parameters -# ---------- -# const : float -# Constant value to simulate. -# """ - -# def __init__(self, const): -# self._const = const - -# def __call__(self, fs, n): -# """ -# Generate a constant signal and its derivative (always zero). - -# Parameters -# ---------- -# fs : float -# Sampling frequency in Hz. -# n : int -# Number of samples to generate. -# """ -# y = self._const * np.ones(int(n)) -# dydt = np.zeros_like(y) - -# return y, dydt From 06f96af357973c8590632e635f362c39a3d7cde0 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:42:58 +0100 Subject: [PATCH 031/129] docstring --- src/smsfusion/simulate/_simulate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 1d064372..35c9a48f 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -47,8 +47,8 @@ def __call__(self, fs: float, n: int, degrees=None): n : int Number of samples to generate. degrees : bool, optional - Whether to return angles and angular velocities in degrees and degrees - per second (True) or radians and radians per second (False). + Whether to return Euler angles and angular velocities in degrees and + degrees per second (True) or radians and radians per second (False). Returns ------- From 34999759f2e296876c02ed9924376dca9ed4d42a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:49:44 +0100 Subject: [PATCH 032/129] docstring --- src/smsfusion/simulate/_simulate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 35c9a48f..8a92c1e5 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -55,9 +55,9 @@ def __call__(self, fs: float, n: int, degrees=None): t : ndarray, shape (n,) Time vector in seconds. euler : ndarray, shape (n, 3) - Simulated Euler angles (roll, pitch, yaw). + Simulated (ZYX) Euler angles [roll, pitch, yaw]^T. w_b : ndarray, shape (n, 3) - Simulated angular velocities in the body frame. + Simulated angular velocities, [w_x, w_y, w_z]^T, in the body frame. """ if degrees is None: degrees = self._degrees From 9b79ee1ba58b0315fa5630fedfef983f74e9a0aa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 13:51:51 +0100 Subject: [PATCH 033/129] docstring --- src/smsfusion/simulate/_simulate.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 8a92c1e5..71f0625d 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -25,6 +25,17 @@ def __init__(self, alpha, beta, gamma, degrees=False): self._rad_scale = np.pi / 180.0 if degrees else 1.0 def _angular_velocity_body(self, euler, euler_dot): + """ + Angular velocity in the body frame. + + Parameters + ---------- + euler : ndarray, shape (n, 3) + Euler angles [alpha, beta, gamma]^T in radians. + euler_dot : ndarray, shape (n, 3) + Time derivatives of Euler angles [alpha_dot, beta_dot, gamma_dot]^T + in radians per second. + """ alpha, beta, _ = euler.T alpha_dot, beta_dot, gamma_dot = euler_dot.T From fd92b05f7e4508cbc326b1511b180c4f06ff57f7 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 14:01:15 +0100 Subject: [PATCH 034/129] allow float angles --- src/smsfusion/simulate/_simulate.py | 63 +++++++++++++++++++++++++---- 1 file changed, 56 insertions(+), 7 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 71f0625d..ea38ad64 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -7,22 +7,30 @@ class GyroSimulator: Parameters ---------- - alpha : SineSignal + alpha : float or SineSignal, default 0.0 Roll signal. - beta : SineSignal + beta : float or SineSignal, default 0.0 Pitch signal - gamma : SineSignal + gamma : float or SineSignal, default 0.0 Yaw signal degrees: bool Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ def __init__(self, alpha, beta, gamma, degrees=False): - self._alpha_sig = alpha - self._beta_sig = beta - self._gamma_sig = gamma self._degrees = degrees - self._rad_scale = np.pi / 180.0 if degrees else 1.0 + if isinstance(alpha, (int, float)): + self._alpha_sig = ConstantSignal(alpha) + else: + self._alpha_sig = alpha + if isinstance(beta, (int, float)): + self._beta_sig = ConstantSignal(beta) + else: + self._beta_sig = beta + if isinstance(gamma, (int, float)): + self._gamma_sig = ConstantSignal(gamma) + else: + self._gamma_sig = gamma def _angular_velocity_body(self, euler, euler_dot): """ @@ -156,3 +164,44 @@ def __call__(self, fs, n): dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) return t, y, dydt + + +class ConstantSignal: + """ + 1D constant signal generator. + + Defined as: + + y(t) = C + dy(t)/dt = 0 + + where, + + - C : Constant value of the signal. + + Parameters + ---------- + value : float, default 0.0 + Constant value of the signal. Default is 0.0. + """ + + def __init__(self, value=0.0): + self._value = value + + def __call__(self, fs, n): + """ + Generate a constant signal and its derivative. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ + n = int(n) + t = np.arange(n) / fs + y = np.full(n, self._value) + dydt = np.zeros(n) + + return t, y, dydt \ No newline at end of file From 4d269bee73ba569298956692712ebd5ba9204221 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 14:14:42 +0100 Subject: [PATCH 035/129] IMUSimulator --- src/smsfusion/simulate/_simulate.py | 318 +++++++++++++++++++++------- 1 file changed, 238 insertions(+), 80 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index ea38ad64..6d8bf295 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -1,7 +1,114 @@ import numpy as np +from smsfusion._transforms import _rot_matrix_from_euler -class GyroSimulator: +class SineSignal: + """ + 1D sine wave signal generator. + + Defined as: + + y(t) = A * sin(w * t + phi) + B + dy(t)/dt = A * w * cos(w * t + phi) + d2y(t)/dt2 = -A * w^2 * sin(w * t + phi) + + where, + + - A : Amplitude of the sine wave. + - w : Angular frequency of the sine wave. + - phi: Phase offset of the sine wave. + - B : Offset of the sine wave. + + Parameters + ---------- + amp : float, default 1.0 + Amplitude of the sine wave. Default is 1.0. + omega : float, default 1.0 + Angular frequency of the sine wave in rad/s. Default is 1.0 + radians per second. + phase : float, default 0.0 + Phase offset of the sine wave. Default is 0.0. + offset : float, default 0.0 + Offset of the sine wave. Default is 0.0. + hz : bool, optional + If True, interpret `omega` as frequency in Hz. If False, interpret as angular + frequency in radians per second. Default is False. + phase_degrees : bool, optional + If True, interpret `phase` in degrees. If False, interpret in radians. + Default is False. + """ + + def __init__(self, amp=1.0, omega=1.0, phase=0.0, offset=0.0, phase_degrees=False): + self._amp = amp + self._omega = omega + self._phase = np.deg2rad(phase) if phase_degrees else phase + self._offset = offset + + def __call__(self, fs, n): + """ + Generate a sine wave and its derivative. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ + dt = 1.0 / fs + t = dt * np.arange(n) + + y = self._amp * np.sin(self._omega * t + self._phase) + self._offset + dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) + d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) + + return t, y, dydt, d2ydt2 + + +class ConstantSignal: + """ + 1D constant signal generator. + + Defined as: + + y(t) = C + dy(t)/dt = 0 + d2y(t)/dt2 = 0 + + where, + + - C : Constant value of the signal. + + Parameters + ---------- + value : float, default 0.0 + Constant value of the signal. Default is 0.0. + """ + + def __init__(self, value=0.0): + self._value = value + + def __call__(self, fs, n): + """ + Generate a constant signal and its derivative. + + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ + n = int(n) + t = np.arange(n) / fs + y = np.full(n, self._value) + dydt = np.zeros(n) + d2ydt2 = np.zeros(n) + + return t, y, dydt, d2ydt2 + + +class IMUSimulator: """ Gyroscope simulator. @@ -17,8 +124,23 @@ class GyroSimulator: Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ - def __init__(self, alpha, beta, gamma, degrees=False): + def __init__(self, pos_x, pos_y, pos_z, alpha, beta, gamma, degrees=False, g=9.80665, nav_frame="NED"): self._degrees = degrees + self._nav_frame = nav_frame.lower() + if self._nav_frame == "ned": + self._g_n = np.array([0.0, 0.0, g]) + if isinstance(pos_x, (int, float)): + self._pos_x_sig = ConstantSignal(pos_x) + else: + self._pos_x_sig = pos_x + if isinstance(pos_y, (int, float)): + self._pos_y_sig = ConstantSignal(pos_y) + else: + self._pos_y_sig = pos_y + if isinstance(pos_z, (int, float)): + self._pos_z_sig = ConstantSignal(pos_z) + else: + self._pos_z_sig = pos_z if isinstance(alpha, (int, float)): self._alpha_sig = ConstantSignal(alpha) else: @@ -32,6 +154,32 @@ def __init__(self, alpha, beta, gamma, degrees=False): else: self._gamma_sig = gamma + def _specific_force_body(self, pos, vel, acc, euler): + """ + Specific force in the body frame. + + Parameters + ---------- + pos : ndarray, shape (n, 3) + Position [x, y, z]^T in meters. + vel : ndarray, shape (n, 3) + Velocity [x_dot, y_dot, z_dot]^T in meters per second. + acc : ndarray, shape (n, 3) + Acceleration [x_ddot, y_ddot, z_ddot]^T in meters per second squared. + euler : ndarray, shape (n, 3) + Euler angles [alpha, beta, gamma]^T in radians. + """ + g = 9.80665 # m/s^2 + n = pos.shape[0] + f_b = np.zeros((n, 3)) + + for i in range(n): + euler_i = euler[i] + + R_i = _rot_matrix_from_euler(euler_i).T.dot( + acc_i + np.array([0.0, 0.0, -gravity()]) + ) + def _angular_velocity_body(self, euler, euler_dot): """ Angular velocity in the body frame. @@ -86,9 +234,16 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # Euler angles and Euler rates - _, alpha, alpha_dot = self._alpha_sig(fs, n) - _, beta, beta_dot = self._beta_sig(fs, n) - _, gamma, gamma_dot = self._gamma_sig(fs, n) + _, pos_x, pos_x_dot, pos_x_ddot = self._pos_x_sig(fs, n) + _, pos_y, pos_y_dot, pos_y_ddot = self._pos_y_sig(fs, n) + _, pos_z, pos_z_dot, pos_z_ddot = self._pos_z_sig(fs, n) + _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) + _, beta, beta_dot, _ = self._beta_sig(fs, n) + _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) + + pos = np.column_stack([pos_x, pos_y, pos_z]) + vel = np.column_stack([pos_x_dot, pos_y_dot, pos_z_dot]) + acc = np.column_stack([pos_x_ddot, pos_y_ddot, pos_z_ddot]) euler = np.column_stack([alpha, beta, gamma]) euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) @@ -96,101 +251,73 @@ def __call__(self, fs: float, n: int, degrees=None): euler = np.deg2rad(euler) euler_dot = np.deg2rad(euler_dot) + f_b = self._specific_force_body(pos, vel, acc, euler) w_b = self._angular_velocity_body(euler, euler_dot) if degrees: euler = np.rad2deg(euler) w_b = np.rad2deg(w_b) - return t, euler, w_b + return t, pos, vel, euler, f_b, w_b -class SineSignal: +class GyroSimulator: """ - 1D sine wave signal generator. - - Defined as: - - y(t) = A * sin(w * t + phi) + B - dy(t)/dt = A * w * cos(w * t + phi) - - where, - - - A : Amplitude of the sine wave. - - w : Angular frequency of the sine wave. - - phi: Phase offset of the sine wave. - - B : Offset of the sine wave. + Gyroscope simulator. Parameters ---------- - amp : float, default 1.0 - Amplitude of the sine wave. Default is 1.0. - omega : float, default 1.0 - Angular frequency of the sine wave in rad/s. Default is 1.0 - radians per second. - phase : float, default 0.0 - Phase offset of the sine wave. Default is 0.0. - offset : float, default 0.0 - Offset of the sine wave. Default is 0.0. - hz : bool, optional - If True, interpret `omega` as frequency in Hz. If False, interpret as angular - frequency in radians per second. Default is False. - phase_degrees : bool, optional - If True, interpret `phase` in degrees. If False, interpret in radians. - Default is False. + alpha : float or SineSignal, default 0.0 + Roll signal. + beta : float or SineSignal, default 0.0 + Pitch signal + gamma : float or SineSignal, default 0.0 + Yaw signal + degrees: bool + Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ - def __init__(self, amp=1.0, omega=1.0, phase=0.0, offset=0.0, phase_degrees=False): - self._amp = amp - self._omega = omega - self._phase = np.deg2rad(phase) if phase_degrees else phase - self._offset = offset + def __init__(self, alpha, beta, gamma, degrees=False): + self._degrees = degrees + if isinstance(alpha, (int, float)): + self._alpha_sig = ConstantSignal(alpha) + else: + self._alpha_sig = alpha + if isinstance(beta, (int, float)): + self._beta_sig = ConstantSignal(beta) + else: + self._beta_sig = beta + if isinstance(gamma, (int, float)): + self._gamma_sig = ConstantSignal(gamma) + else: + self._gamma_sig = gamma - def __call__(self, fs, n): + def _angular_velocity_body(self, euler, euler_dot): """ - Generate a sine wave and its derivative. + Angular velocity in the body frame. Parameters ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. + euler : ndarray, shape (n, 3) + Euler angles [alpha, beta, gamma]^T in radians. + euler_dot : ndarray, shape (n, 3) + Time derivatives of Euler angles [alpha_dot, beta_dot, gamma_dot]^T + in radians per second. """ - dt = 1.0 / fs - t = dt * np.arange(n) - - y = self._amp * np.sin(self._omega * t + self._phase) + self._offset - dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) - - return t, y, dydt - - -class ConstantSignal: - """ - 1D constant signal generator. - - Defined as: - - y(t) = C - dy(t)/dt = 0 - - where, + alpha, beta, _ = euler.T + alpha_dot, beta_dot, gamma_dot = euler_dot.T - - C : Constant value of the signal. + w_x = alpha_dot - np.sin(beta) * gamma_dot + w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot + w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot - Parameters - ---------- - value : float, default 0.0 - Constant value of the signal. Default is 0.0. - """ + w_b = np.column_stack([w_x, w_y, w_z]) - def __init__(self, value=0.0): - self._value = value + return w_b - def __call__(self, fs, n): + def __call__(self, fs: float, n: int, degrees=None): """ - Generate a constant signal and its derivative. + Generate a length-n gyroscope signal and corresponding Euler angles. Parameters ---------- @@ -198,10 +325,41 @@ def __call__(self, fs, n): Sampling frequency in Hz. n : int Number of samples to generate. + degrees : bool, optional + Whether to return Euler angles and angular velocities in degrees and + degrees per second (True) or radians and radians per second (False). + + Returns + ------- + t : ndarray, shape (n,) + Time vector in seconds. + euler : ndarray, shape (n, 3) + Simulated (ZYX) Euler angles [roll, pitch, yaw]^T. + w_b : ndarray, shape (n, 3) + Simulated angular velocities, [w_x, w_y, w_z]^T, in the body frame. """ - n = int(n) - t = np.arange(n) / fs - y = np.full(n, self._value) - dydt = np.zeros(n) + if degrees is None: + degrees = self._degrees + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + # Euler angles and Euler rates + _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) + _, beta, beta_dot, _ = self._beta_sig(fs, n) + _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) + euler = np.column_stack([alpha, beta, gamma]) + euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) + + if self._degrees: + euler = np.deg2rad(euler) + euler_dot = np.deg2rad(euler_dot) + + w_b = self._angular_velocity_body(euler, euler_dot) + + if degrees: + euler = np.rad2deg(euler) + w_b = np.rad2deg(w_b) - return t, y, dydt \ No newline at end of file + return t, euler, w_b From bc154a35f4403a0745f2a871aa90ee6dbb7088ae Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 14:26:13 +0100 Subject: [PATCH 036/129] IMU simulator --- src/smsfusion/simulate/_simulate.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 6d8bf295..993d0ff6 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -124,7 +124,7 @@ class IMUSimulator: Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ - def __init__(self, pos_x, pos_y, pos_z, alpha, beta, gamma, degrees=False, g=9.80665, nav_frame="NED"): + def __init__(self, pos_x=0.0, pos_y=0.0, pos_z=0.0, alpha=0.0, beta=0.0, gamma=0.0, degrees=False, g=9.80665, nav_frame="NED"): self._degrees = degrees self._nav_frame = nav_frame.lower() if self._nav_frame == "ned": @@ -154,7 +154,7 @@ def __init__(self, pos_x, pos_y, pos_z, alpha, beta, gamma, degrees=False, g=9.8 else: self._gamma_sig = gamma - def _specific_force_body(self, pos, vel, acc, euler): + def _specific_force_body(self, pos, acc, euler): """ Specific force in the body frame. @@ -169,16 +169,15 @@ def _specific_force_body(self, pos, vel, acc, euler): euler : ndarray, shape (n, 3) Euler angles [alpha, beta, gamma]^T in radians. """ - g = 9.80665 # m/s^2 n = pos.shape[0] f_b = np.zeros((n, 3)) for i in range(n): - euler_i = euler[i] + R_i = _rot_matrix_from_euler(euler[i]) + f_b[i] = R_i.T.dot(acc[i] + self._g_n) + + return f_b - R_i = _rot_matrix_from_euler(euler_i).T.dot( - acc_i + np.array([0.0, 0.0, -gravity()]) - ) def _angular_velocity_body(self, euler, euler_dot): """ @@ -251,7 +250,7 @@ def __call__(self, fs: float, n: int, degrees=None): euler = np.deg2rad(euler) euler_dot = np.deg2rad(euler_dot) - f_b = self._specific_force_body(pos, vel, acc, euler) + f_b = self._specific_force_body(pos, acc, euler) w_b = self._angular_velocity_body(euler, euler_dot) if degrees: From d9507094304e90bf6e2c34c1295e9cdc0a8230fc Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 14:27:34 +0100 Subject: [PATCH 037/129] style --- src/smsfusion/simulate/_simulate.py | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 993d0ff6..aed453e2 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -1,4 +1,5 @@ import numpy as np + from smsfusion._transforms import _rot_matrix_from_euler @@ -110,10 +111,16 @@ def __call__(self, fs, n): class IMUSimulator: """ - Gyroscope simulator. + IMU simulator. Parameters ---------- + pos_x : float or ConstantSignal, default 0.0 + X position signal. + pos_y : float or ConstantSignal, default 0.0 + Y position signal. + pos_z : float or ConstantSignal, default 0.0 + Z position signal. alpha : float or SineSignal, default 0.0 Roll signal. beta : float or SineSignal, default 0.0 @@ -124,7 +131,18 @@ class IMUSimulator: Whether to interpret the Euler angle signals as degrees (True) or radians (False). """ - def __init__(self, pos_x=0.0, pos_y=0.0, pos_z=0.0, alpha=0.0, beta=0.0, gamma=0.0, degrees=False, g=9.80665, nav_frame="NED"): + def __init__( + self, + pos_x=0.0, + pos_y=0.0, + pos_z=0.0, + alpha=0.0, + beta=0.0, + gamma=0.0, + degrees=False, + g=9.80665, + nav_frame="NED", + ): self._degrees = degrees self._nav_frame = nav_frame.lower() if self._nav_frame == "ned": @@ -178,7 +196,6 @@ def _specific_force_body(self, pos, acc, euler): return f_b - def _angular_velocity_body(self, euler, euler_dot): """ Angular velocity in the body frame. @@ -346,7 +363,7 @@ def __call__(self, fs: float, n: int, degrees=None): # Euler angles and Euler rates _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) - _, beta, beta_dot, _ = self._beta_sig(fs, n) + _, beta, beta_dot, _ = self._beta_sig(fs, n) _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) euler = np.column_stack([alpha, beta, gamma]) euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) From 27eaa1e8276e8e79711ac720dba7657d16f0b743 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 14:28:29 +0100 Subject: [PATCH 038/129] init --- src/smsfusion/simulate/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 99d5250c..561e4917 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import GyroSimulator, SineSignal +from ._simulate import IMUSimulator, SineSignal -__all__ = ["GyroSimulator", "SineSignal"] +__all__ = ["IMUSimulator", "SineSignal"] From a4a25c7e143c15dea6182b2d94d585c0f89005a1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 15:06:45 +0100 Subject: [PATCH 039/129] some changes --- src/smsfusion/simulate/_simulate.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index aed453e2..b75aa6b5 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -145,8 +145,14 @@ def __init__( ): self._degrees = degrees self._nav_frame = nav_frame.lower() + if self._nav_frame == "ned": self._g_n = np.array([0.0, 0.0, g]) + elif self._nav_frame == "enu": + self._g_n = np.array([0.0, 0.0, -g]) + else: + raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") + if isinstance(pos_x, (int, float)): self._pos_x_sig = ConstantSignal(pos_x) else: @@ -192,7 +198,7 @@ def _specific_force_body(self, pos, acc, euler): for i in range(n): R_i = _rot_matrix_from_euler(euler[i]) - f_b[i] = R_i.T.dot(acc[i] + self._g_n) + f_b[i] = R_i.T.dot(acc[i] - self._g_n) return f_b From 611f7b66af7d3182d1479023f42495b6d85e3f8d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 15:16:01 +0100 Subject: [PATCH 040/129] some chnages --- src/smsfusion/simulate/_simulate.py | 94 ++++++++++++++++++----------- 1 file changed, 58 insertions(+), 36 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index b75aa6b5..fcb81828 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -109,23 +109,26 @@ def __call__(self, fs, n): return t, y, dydt, d2ydt2 +Signal = SineSignal | ConstantSignal + + class IMUSimulator: """ IMU simulator. Parameters ---------- - pos_x : float or ConstantSignal, default 0.0 + pos_x : float or Signal, default 0.0 X position signal. - pos_y : float or ConstantSignal, default 0.0 + pos_y : float or Signal, default 0.0 Y position signal. - pos_z : float or ConstantSignal, default 0.0 + pos_z : float or Signal, default 0.0 Z position signal. - alpha : float or SineSignal, default 0.0 + alpha : float or Signal, default 0.0 Roll signal. - beta : float or SineSignal, default 0.0 + beta : float or Signal, default 0.0 Pitch signal - gamma : float or SineSignal, default 0.0 + gamma : float or Signal, default 0.0 Yaw signal degrees: bool Whether to interpret the Euler angle signals as degrees (True) or radians (False). @@ -133,16 +136,22 @@ class IMUSimulator: def __init__( self, - pos_x=0.0, - pos_y=0.0, - pos_z=0.0, - alpha=0.0, - beta=0.0, - gamma=0.0, + pos_x: float | Signal = 0.0, + pos_y: float | Signal = 0.0, + pos_z: float | Signal = 0.0, + alpha: float | Signal = 0.0, + beta: float | Signal = 0.0, + gamma: float | Signal = 0.0, degrees=False, g=9.80665, nav_frame="NED", ): + self._pos_x_sig = pos_x + self._pos_y_sig = pos_y + self._pos_z_sig = pos_z + self._alpha_sig = alpha + self._beta_sig = beta + self._gamma_sig = gamma self._degrees = degrees self._nav_frame = nav_frame.lower() @@ -153,30 +162,43 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if isinstance(pos_x, (int, float)): - self._pos_x_sig = ConstantSignal(pos_x) - else: - self._pos_x_sig = pos_x - if isinstance(pos_y, (int, float)): - self._pos_y_sig = ConstantSignal(pos_y) - else: - self._pos_y_sig = pos_y - if isinstance(pos_z, (int, float)): - self._pos_z_sig = ConstantSignal(pos_z) - else: - self._pos_z_sig = pos_z - if isinstance(alpha, (int, float)): - self._alpha_sig = ConstantSignal(alpha) - else: - self._alpha_sig = alpha - if isinstance(beta, (int, float)): - self._beta_sig = ConstantSignal(beta) - else: - self._beta_sig = beta - if isinstance(gamma, (int, float)): - self._gamma_sig = ConstantSignal(gamma) - else: - self._gamma_sig = gamma + if not isinstance(self._pos_x_sig, Signal): + self._pos_x_sig = ConstantSignal(self._pos_x_sig) + if not isinstance(self._pos_y_sig, Signal): + self._pos_y_sig = ConstantSignal(self._pos_y_sig) + if not isinstance(self._pos_z_sig, Signal): + self._pos_z_sig = ConstantSignal(self._pos_z_sig) + if not isinstance(self._alpha_sig, Signal): + self._alpha_sig = ConstantSignal(self._alpha_sig) + if not isinstance(self._beta_sig, Signal): + self._beta_sig = ConstantSignal(self._beta_sig) + if not isinstance(self._gamma_sig, Signal): + self._gamma_sig = ConstantSignal(self._gamma_sig) + + # if isinstance(pos_x, (int, float)): + # self._pos_x_sig = ConstantSignal(pos_x) + # else: + # self._pos_x_sig = pos_x + # if isinstance(pos_y, (int, float)): + # self._pos_y_sig = ConstantSignal(pos_y) + # else: + # self._pos_y_sig = pos_y + # if isinstance(pos_z, (int, float)): + # self._pos_z_sig = ConstantSignal(pos_z) + # else: + # self._pos_z_sig = pos_z + # if isinstance(alpha, (int, float)): + # self._alpha_sig = ConstantSignal(alpha) + # else: + # self._alpha_sig = alpha + # if isinstance(beta, (int, float)): + # self._beta_sig = ConstantSignal(beta) + # else: + # self._beta_sig = beta + # if isinstance(gamma, (int, float)): + # self._gamma_sig = ConstantSignal(gamma) + # else: + # self._gamma_sig = gamma def _specific_force_body(self, pos, acc, euler): """ From 48cf0a96e18eee6301eff6cbcbe6af6f683038b0 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Sun, 9 Nov 2025 16:32:37 +0100 Subject: [PATCH 041/129] rename to DOFSignal --- src/smsfusion/simulate/_simulate.py | 267 +++++++++++++--------------- 1 file changed, 121 insertions(+), 146 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index fcb81828..0ff769b0 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -109,7 +109,7 @@ def __call__(self, fs, n): return t, y, dydt, d2ydt2 -Signal = SineSignal | ConstantSignal +DOFSignal = SineSignal | ConstantSignal class IMUSimulator: @@ -118,17 +118,17 @@ class IMUSimulator: Parameters ---------- - pos_x : float or Signal, default 0.0 + pos_x : float or DOFSignal, default 0.0 X position signal. - pos_y : float or Signal, default 0.0 + pos_y : float or DOFSignal, default 0.0 Y position signal. - pos_z : float or Signal, default 0.0 + pos_z : float or DOFSignal, default 0.0 Z position signal. - alpha : float or Signal, default 0.0 + alpha : float or DOFSignal, default 0.0 Roll signal. - beta : float or Signal, default 0.0 + beta : float or DOFSignal, default 0.0 Pitch signal - gamma : float or Signal, default 0.0 + gamma : float or DOFSignal, default 0.0 Yaw signal degrees: bool Whether to interpret the Euler angle signals as degrees (True) or radians (False). @@ -136,12 +136,12 @@ class IMUSimulator: def __init__( self, - pos_x: float | Signal = 0.0, - pos_y: float | Signal = 0.0, - pos_z: float | Signal = 0.0, - alpha: float | Signal = 0.0, - beta: float | Signal = 0.0, - gamma: float | Signal = 0.0, + pos_x: float | DOFSignal = 0.0, + pos_y: float | DOFSignal = 0.0, + pos_z: float | DOFSignal = 0.0, + alpha: float | DOFSignal = 0.0, + beta: float | DOFSignal = 0.0, + gamma: float | DOFSignal = 0.0, degrees=False, g=9.80665, nav_frame="NED", @@ -162,44 +162,19 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if not isinstance(self._pos_x_sig, Signal): + if not isinstance(self._pos_x_sig, DOFSignal): self._pos_x_sig = ConstantSignal(self._pos_x_sig) - if not isinstance(self._pos_y_sig, Signal): + if not isinstance(self._pos_y_sig, DOFSignal): self._pos_y_sig = ConstantSignal(self._pos_y_sig) - if not isinstance(self._pos_z_sig, Signal): + if not isinstance(self._pos_z_sig, DOFSignal): self._pos_z_sig = ConstantSignal(self._pos_z_sig) - if not isinstance(self._alpha_sig, Signal): + if not isinstance(self._alpha_sig, DOFSignal): self._alpha_sig = ConstantSignal(self._alpha_sig) - if not isinstance(self._beta_sig, Signal): + if not isinstance(self._beta_sig, DOFSignal): self._beta_sig = ConstantSignal(self._beta_sig) - if not isinstance(self._gamma_sig, Signal): + if not isinstance(self._gamma_sig, DOFSignal): self._gamma_sig = ConstantSignal(self._gamma_sig) - # if isinstance(pos_x, (int, float)): - # self._pos_x_sig = ConstantSignal(pos_x) - # else: - # self._pos_x_sig = pos_x - # if isinstance(pos_y, (int, float)): - # self._pos_y_sig = ConstantSignal(pos_y) - # else: - # self._pos_y_sig = pos_y - # if isinstance(pos_z, (int, float)): - # self._pos_z_sig = ConstantSignal(pos_z) - # else: - # self._pos_z_sig = pos_z - # if isinstance(alpha, (int, float)): - # self._alpha_sig = ConstantSignal(alpha) - # else: - # self._alpha_sig = alpha - # if isinstance(beta, (int, float)): - # self._beta_sig = ConstantSignal(beta) - # else: - # self._beta_sig = beta - # if isinstance(gamma, (int, float)): - # self._gamma_sig = ConstantSignal(gamma) - # else: - # self._gamma_sig = gamma - def _specific_force_body(self, pos, acc, euler): """ Specific force in the body frame. @@ -305,105 +280,105 @@ def __call__(self, fs: float, n: int, degrees=None): return t, pos, vel, euler, f_b, w_b -class GyroSimulator: - """ - Gyroscope simulator. - - Parameters - ---------- - alpha : float or SineSignal, default 0.0 - Roll signal. - beta : float or SineSignal, default 0.0 - Pitch signal - gamma : float or SineSignal, default 0.0 - Yaw signal - degrees: bool - Whether to interpret the Euler angle signals as degrees (True) or radians (False). - """ - - def __init__(self, alpha, beta, gamma, degrees=False): - self._degrees = degrees - if isinstance(alpha, (int, float)): - self._alpha_sig = ConstantSignal(alpha) - else: - self._alpha_sig = alpha - if isinstance(beta, (int, float)): - self._beta_sig = ConstantSignal(beta) - else: - self._beta_sig = beta - if isinstance(gamma, (int, float)): - self._gamma_sig = ConstantSignal(gamma) - else: - self._gamma_sig = gamma - - def _angular_velocity_body(self, euler, euler_dot): - """ - Angular velocity in the body frame. - - Parameters - ---------- - euler : ndarray, shape (n, 3) - Euler angles [alpha, beta, gamma]^T in radians. - euler_dot : ndarray, shape (n, 3) - Time derivatives of Euler angles [alpha_dot, beta_dot, gamma_dot]^T - in radians per second. - """ - alpha, beta, _ = euler.T - alpha_dot, beta_dot, gamma_dot = euler_dot.T - - w_x = alpha_dot - np.sin(beta) * gamma_dot - w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot - w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot - - w_b = np.column_stack([w_x, w_y, w_z]) - - return w_b - - def __call__(self, fs: float, n: int, degrees=None): - """ - Generate a length-n gyroscope signal and corresponding Euler angles. - - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - degrees : bool, optional - Whether to return Euler angles and angular velocities in degrees and - degrees per second (True) or radians and radians per second (False). - - Returns - ------- - t : ndarray, shape (n,) - Time vector in seconds. - euler : ndarray, shape (n, 3) - Simulated (ZYX) Euler angles [roll, pitch, yaw]^T. - w_b : ndarray, shape (n, 3) - Simulated angular velocities, [w_x, w_y, w_z]^T, in the body frame. - """ - if degrees is None: - degrees = self._degrees - - # Time - dt = 1.0 / fs - t = dt * np.arange(n) - - # Euler angles and Euler rates - _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) - _, beta, beta_dot, _ = self._beta_sig(fs, n) - _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) - euler = np.column_stack([alpha, beta, gamma]) - euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) - - if self._degrees: - euler = np.deg2rad(euler) - euler_dot = np.deg2rad(euler_dot) - - w_b = self._angular_velocity_body(euler, euler_dot) - - if degrees: - euler = np.rad2deg(euler) - w_b = np.rad2deg(w_b) - - return t, euler, w_b +# class GyroSimulator: +# """ +# Gyroscope simulator. + +# Parameters +# ---------- +# alpha : float or SineSignal, default 0.0 +# Roll signal. +# beta : float or SineSignal, default 0.0 +# Pitch signal +# gamma : float or SineSignal, default 0.0 +# Yaw signal +# degrees: bool +# Whether to interpret the Euler angle signals as degrees (True) or radians (False). +# """ + +# def __init__(self, alpha, beta, gamma, degrees=False): +# self._degrees = degrees +# if isinstance(alpha, (int, float)): +# self._alpha_sig = ConstantSignal(alpha) +# else: +# self._alpha_sig = alpha +# if isinstance(beta, (int, float)): +# self._beta_sig = ConstantSignal(beta) +# else: +# self._beta_sig = beta +# if isinstance(gamma, (int, float)): +# self._gamma_sig = ConstantSignal(gamma) +# else: +# self._gamma_sig = gamma + +# def _angular_velocity_body(self, euler, euler_dot): +# """ +# Angular velocity in the body frame. + +# Parameters +# ---------- +# euler : ndarray, shape (n, 3) +# Euler angles [alpha, beta, gamma]^T in radians. +# euler_dot : ndarray, shape (n, 3) +# Time derivatives of Euler angles [alpha_dot, beta_dot, gamma_dot]^T +# in radians per second. +# """ +# alpha, beta, _ = euler.T +# alpha_dot, beta_dot, gamma_dot = euler_dot.T + +# w_x = alpha_dot - np.sin(beta) * gamma_dot +# w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot +# w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + +# w_b = np.column_stack([w_x, w_y, w_z]) + +# return w_b + +# def __call__(self, fs: float, n: int, degrees=None): +# """ +# Generate a length-n gyroscope signal and corresponding Euler angles. + +# Parameters +# ---------- +# fs : float +# Sampling frequency in Hz. +# n : int +# Number of samples to generate. +# degrees : bool, optional +# Whether to return Euler angles and angular velocities in degrees and +# degrees per second (True) or radians and radians per second (False). + +# Returns +# ------- +# t : ndarray, shape (n,) +# Time vector in seconds. +# euler : ndarray, shape (n, 3) +# Simulated (ZYX) Euler angles [roll, pitch, yaw]^T. +# w_b : ndarray, shape (n, 3) +# Simulated angular velocities, [w_x, w_y, w_z]^T, in the body frame. +# """ +# if degrees is None: +# degrees = self._degrees + +# # Time +# dt = 1.0 / fs +# t = dt * np.arange(n) + +# # Euler angles and Euler rates +# _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) +# _, beta, beta_dot, _ = self._beta_sig(fs, n) +# _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) +# euler = np.column_stack([alpha, beta, gamma]) +# euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) + +# if self._degrees: +# euler = np.deg2rad(euler) +# euler_dot = np.deg2rad(euler_dot) + +# w_b = self._angular_velocity_body(euler, euler_dot) + +# if degrees: +# euler = np.rad2deg(euler) +# w_b = np.rad2deg(w_b) + +# return t, euler, w_b From 50a5d23f2c04f0594035f504ca6336e851c75256 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 08:46:04 +0100 Subject: [PATCH 042/129] rename to dofsignal --- src/smsfusion/simulate/__init__.py | 4 ++-- src/smsfusion/simulate/_simulate.py | 18 +++++++++--------- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 561e4917..7aaf2004 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import IMUSimulator, SineSignal +from ._simulate import ConstantDOFSignal, IMUSimulator, SineDOFSignal -__all__ = ["IMUSimulator", "SineSignal"] +__all__ = ["IMUSimulator", "SineDOFSignal", "ConstantDOFSignal"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 0ff769b0..23277500 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -3,7 +3,7 @@ from smsfusion._transforms import _rot_matrix_from_euler -class SineSignal: +class SineDOFSignal: """ 1D sine wave signal generator. @@ -66,7 +66,7 @@ def __call__(self, fs, n): return t, y, dydt, d2ydt2 -class ConstantSignal: +class ConstantDOFSignal: """ 1D constant signal generator. @@ -109,7 +109,7 @@ def __call__(self, fs, n): return t, y, dydt, d2ydt2 -DOFSignal = SineSignal | ConstantSignal +DOFSignal = SineDOFSignal | ConstantDOFSignal class IMUSimulator: @@ -163,17 +163,17 @@ def __init__( raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") if not isinstance(self._pos_x_sig, DOFSignal): - self._pos_x_sig = ConstantSignal(self._pos_x_sig) + self._pos_x_sig = ConstantDOFSignal(self._pos_x_sig) if not isinstance(self._pos_y_sig, DOFSignal): - self._pos_y_sig = ConstantSignal(self._pos_y_sig) + self._pos_y_sig = ConstantDOFSignal(self._pos_y_sig) if not isinstance(self._pos_z_sig, DOFSignal): - self._pos_z_sig = ConstantSignal(self._pos_z_sig) + self._pos_z_sig = ConstantDOFSignal(self._pos_z_sig) if not isinstance(self._alpha_sig, DOFSignal): - self._alpha_sig = ConstantSignal(self._alpha_sig) + self._alpha_sig = ConstantDOFSignal(self._alpha_sig) if not isinstance(self._beta_sig, DOFSignal): - self._beta_sig = ConstantSignal(self._beta_sig) + self._beta_sig = ConstantDOFSignal(self._beta_sig) if not isinstance(self._gamma_sig, DOFSignal): - self._gamma_sig = ConstantSignal(self._gamma_sig) + self._gamma_sig = ConstantDOFSignal(self._gamma_sig) def _specific_force_body(self, pos, acc, euler): """ From d2e1a3dd4c8337d94f74b26aee9c3b643d54bb81 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 08:49:55 +0100 Subject: [PATCH 043/129] dont return time in signal --- src/smsfusion/simulate/_simulate.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 23277500..fb63110b 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -63,7 +63,7 @@ def __call__(self, fs, n): dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) - return t, y, dydt, d2ydt2 + return y, dydt, d2ydt2 class ConstantDOFSignal: @@ -101,12 +101,11 @@ def __call__(self, fs, n): Number of samples to generate. """ n = int(n) - t = np.arange(n) / fs y = np.full(n, self._value) dydt = np.zeros(n) d2ydt2 = np.zeros(n) - return t, y, dydt, d2ydt2 + return y, dydt, d2ydt2 DOFSignal = SineDOFSignal | ConstantDOFSignal @@ -253,12 +252,12 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # Euler angles and Euler rates - _, pos_x, pos_x_dot, pos_x_ddot = self._pos_x_sig(fs, n) - _, pos_y, pos_y_dot, pos_y_ddot = self._pos_y_sig(fs, n) - _, pos_z, pos_z_dot, pos_z_ddot = self._pos_z_sig(fs, n) - _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) - _, beta, beta_dot, _ = self._beta_sig(fs, n) - _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) + pos_x, pos_x_dot, pos_x_ddot = self._pos_x_sig(fs, n) + pos_y, pos_y_dot, pos_y_ddot = self._pos_y_sig(fs, n) + pos_z, pos_z_dot, pos_z_ddot = self._pos_z_sig(fs, n) + alpha, alpha_dot, _ = self._alpha_sig(fs, n) + beta, beta_dot, _ = self._beta_sig(fs, n) + gamma, gamma_dot, _ = self._gamma_sig(fs, n) pos = np.column_stack([pos_x, pos_y, pos_z]) vel = np.column_stack([pos_x_dot, pos_y_dot, pos_z_dot]) From b5f9d307ea7c5c791faa8ffa11beb730b889f6a3 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 08:51:55 +0100 Subject: [PATCH 044/129] comments --- src/smsfusion/simulate/_simulate.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index fb63110b..94356cd0 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -251,7 +251,7 @@ def __call__(self, fs: float, n: int, degrees=None): dt = 1.0 / fs t = dt * np.arange(n) - # Euler angles and Euler rates + # DOFs and corresponding rates and accelerations pos_x, pos_x_dot, pos_x_ddot = self._pos_x_sig(fs, n) pos_y, pos_y_dot, pos_y_ddot = self._pos_y_sig(fs, n) pos_z, pos_z_dot, pos_z_ddot = self._pos_z_sig(fs, n) @@ -269,6 +269,7 @@ def __call__(self, fs: float, n: int, degrees=None): euler = np.deg2rad(euler) euler_dot = np.deg2rad(euler_dot) + # IMU measurements (i.e., specific force and angular velocity in body frame) f_b = self._specific_force_body(pos, acc, euler) w_b = self._angular_velocity_body(euler, euler_dot) From b9f58024464c733ad39fe37b4fbb9e16910cb0fa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 10:08:59 +0100 Subject: [PATCH 045/129] delete old code --- src/smsfusion/simulate/_simulate.py | 104 ---------------------------- 1 file changed, 104 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 94356cd0..adf9cdf3 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -278,107 +278,3 @@ def __call__(self, fs: float, n: int, degrees=None): w_b = np.rad2deg(w_b) return t, pos, vel, euler, f_b, w_b - - -# class GyroSimulator: -# """ -# Gyroscope simulator. - -# Parameters -# ---------- -# alpha : float or SineSignal, default 0.0 -# Roll signal. -# beta : float or SineSignal, default 0.0 -# Pitch signal -# gamma : float or SineSignal, default 0.0 -# Yaw signal -# degrees: bool -# Whether to interpret the Euler angle signals as degrees (True) or radians (False). -# """ - -# def __init__(self, alpha, beta, gamma, degrees=False): -# self._degrees = degrees -# if isinstance(alpha, (int, float)): -# self._alpha_sig = ConstantSignal(alpha) -# else: -# self._alpha_sig = alpha -# if isinstance(beta, (int, float)): -# self._beta_sig = ConstantSignal(beta) -# else: -# self._beta_sig = beta -# if isinstance(gamma, (int, float)): -# self._gamma_sig = ConstantSignal(gamma) -# else: -# self._gamma_sig = gamma - -# def _angular_velocity_body(self, euler, euler_dot): -# """ -# Angular velocity in the body frame. - -# Parameters -# ---------- -# euler : ndarray, shape (n, 3) -# Euler angles [alpha, beta, gamma]^T in radians. -# euler_dot : ndarray, shape (n, 3) -# Time derivatives of Euler angles [alpha_dot, beta_dot, gamma_dot]^T -# in radians per second. -# """ -# alpha, beta, _ = euler.T -# alpha_dot, beta_dot, gamma_dot = euler_dot.T - -# w_x = alpha_dot - np.sin(beta) * gamma_dot -# w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot -# w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot - -# w_b = np.column_stack([w_x, w_y, w_z]) - -# return w_b - -# def __call__(self, fs: float, n: int, degrees=None): -# """ -# Generate a length-n gyroscope signal and corresponding Euler angles. - -# Parameters -# ---------- -# fs : float -# Sampling frequency in Hz. -# n : int -# Number of samples to generate. -# degrees : bool, optional -# Whether to return Euler angles and angular velocities in degrees and -# degrees per second (True) or radians and radians per second (False). - -# Returns -# ------- -# t : ndarray, shape (n,) -# Time vector in seconds. -# euler : ndarray, shape (n, 3) -# Simulated (ZYX) Euler angles [roll, pitch, yaw]^T. -# w_b : ndarray, shape (n, 3) -# Simulated angular velocities, [w_x, w_y, w_z]^T, in the body frame. -# """ -# if degrees is None: -# degrees = self._degrees - -# # Time -# dt = 1.0 / fs -# t = dt * np.arange(n) - -# # Euler angles and Euler rates -# _, alpha, alpha_dot, _ = self._alpha_sig(fs, n) -# _, beta, beta_dot, _ = self._beta_sig(fs, n) -# _, gamma, gamma_dot, _ = self._gamma_sig(fs, n) -# euler = np.column_stack([alpha, beta, gamma]) -# euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) - -# if self._degrees: -# euler = np.deg2rad(euler) -# euler_dot = np.deg2rad(euler_dot) - -# w_b = self._angular_velocity_body(euler, euler_dot) - -# if degrees: -# euler = np.rad2deg(euler) -# w_b = np.rad2deg(w_b) - -# return t, euler, w_b From f9b26cc5abc854f58940c2d241f26ee142a2a69e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 10:10:08 +0100 Subject: [PATCH 046/129] dosctring --- src/smsfusion/simulate/_simulate.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index adf9cdf3..6301e7fe 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -25,8 +25,7 @@ class SineDOFSignal: amp : float, default 1.0 Amplitude of the sine wave. Default is 1.0. omega : float, default 1.0 - Angular frequency of the sine wave in rad/s. Default is 1.0 - radians per second. + Angular frequency of the sine wave in rad/s. Default is 1.0 rad/s. phase : float, default 0.0 Phase offset of the sine wave. Default is 0.0. offset : float, default 0.0 From 7b18c763ccf99aede2ff0c028288bbbba3b0c98f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 10:14:13 +0100 Subject: [PATCH 047/129] docstring --- src/smsfusion/simulate/_simulate.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 6301e7fe..2c9610c8 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -128,8 +128,14 @@ class IMUSimulator: Pitch signal gamma : float or DOFSignal, default 0.0 Yaw signal - degrees: bool + degrees: bool, default False Whether to interpret the Euler angle signals as degrees (True) or radians (False). + Default is False. + g : float, default 9.80665 + The gravitational acceleration. Default is 'standard gravity' of 9.80665. + nav_frame : str, default "NED" + Navigation frame. Either "NED" (North-East-Down) or "ENU" (East-North-Up). + Default is "NED". """ def __init__( From 59745663934449a3aae442443230bdb6289552ab Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 12:25:56 +0100 Subject: [PATCH 048/129] some changes --- src/smsfusion/simulate/_simulate.py | 53 +++++++++++++++++++++-------- 1 file changed, 38 insertions(+), 15 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 2c9610c8..9555e6cd 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -1,9 +1,17 @@ +from abc import ABC, abstractmethod + import numpy as np from smsfusion._transforms import _rot_matrix_from_euler -class SineDOFSignal: +class _DOFSignal(ABC): + @abstractmethod + def __call__(self, fs, n): + raise NotImplementedError("Not implemented.") + + +class SineDOFSignal(_DOFSignal): """ 1D sine wave signal generator. @@ -65,7 +73,7 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -class ConstantDOFSignal: +class ConstantDOFSignal(_DOFSignal): """ 1D constant signal generator. @@ -107,7 +115,22 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -DOFSignal = SineDOFSignal | ConstantDOFSignal +class LinearRamp(_DOFSignal): + def __init__(self, dof_signal: _DOFSignal, t_start=0.0, ramp_length=1.0): + self._dof_signal = dof_signal + self._t_start = t_start + self._ramp_length = ramp_length + + def __call__(self, fs, n): + y, dydt, d2ydt2 = self._dof_signal(fs, n) + + # Time + dt = 1.0 / fs + t = dt * np.arange(n) + + ramp = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) + + return ramp*y, ramp*dydt, ramp*d2ydt2 class IMUSimulator: @@ -140,12 +163,12 @@ class IMUSimulator: def __init__( self, - pos_x: float | DOFSignal = 0.0, - pos_y: float | DOFSignal = 0.0, - pos_z: float | DOFSignal = 0.0, - alpha: float | DOFSignal = 0.0, - beta: float | DOFSignal = 0.0, - gamma: float | DOFSignal = 0.0, + pos_x: float | _DOFSignal = 0.0, + pos_y: float | _DOFSignal = 0.0, + pos_z: float | _DOFSignal = 0.0, + alpha: float | _DOFSignal = 0.0, + beta: float | _DOFSignal = 0.0, + gamma: float | _DOFSignal = 0.0, degrees=False, g=9.80665, nav_frame="NED", @@ -166,17 +189,17 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if not isinstance(self._pos_x_sig, DOFSignal): + if not isinstance(self._pos_x_sig, _DOFSignal): self._pos_x_sig = ConstantDOFSignal(self._pos_x_sig) - if not isinstance(self._pos_y_sig, DOFSignal): + if not isinstance(self._pos_y_sig, _DOFSignal): self._pos_y_sig = ConstantDOFSignal(self._pos_y_sig) - if not isinstance(self._pos_z_sig, DOFSignal): + if not isinstance(self._pos_z_sig, _DOFSignal): self._pos_z_sig = ConstantDOFSignal(self._pos_z_sig) - if not isinstance(self._alpha_sig, DOFSignal): + if not isinstance(self._alpha_sig, _DOFSignal): self._alpha_sig = ConstantDOFSignal(self._alpha_sig) - if not isinstance(self._beta_sig, DOFSignal): + if not isinstance(self._beta_sig, _DOFSignal): self._beta_sig = ConstantDOFSignal(self._beta_sig) - if not isinstance(self._gamma_sig, DOFSignal): + if not isinstance(self._gamma_sig, _DOFSignal): self._gamma_sig = ConstantDOFSignal(self._gamma_sig) def _specific_force_body(self, pos, acc, euler): From b71b1aaa2e715380f3d682976fb6e536e9010fc8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 13:01:11 +0100 Subject: [PATCH 049/129] rename to dof --- src/smsfusion/simulate/__init__.py | 4 +- src/smsfusion/simulate/_simulate.py | 79 ++++++++++++++++------------- 2 files changed, 47 insertions(+), 36 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 7aaf2004..7cd12b31 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOFSignal, IMUSimulator, SineDOFSignal +from ._simulate import ConstantDOF, IMUSimulator, SineDOF -__all__ = ["IMUSimulator", "SineDOFSignal", "ConstantDOFSignal"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 9555e6cd..29e5e56c 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -5,15 +5,19 @@ from smsfusion._transforms import _rot_matrix_from_euler -class _DOFSignal(ABC): +class _DOF(ABC): + """ + Abstract base class for degree of freedom (DOF) signals. + """ + @abstractmethod def __call__(self, fs, n): raise NotImplementedError("Not implemented.") -class SineDOFSignal(_DOFSignal): +class SineDOF(_DOF): """ - 1D sine wave signal generator. + 1D sine wave DOF signal generator. Defined as: @@ -73,9 +77,9 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -class ConstantDOFSignal(_DOFSignal): +class ConstantDOF(_DOF): """ - 1D constant signal generator. + 1D constant DOF signal generator. Defined as: @@ -115,14 +119,18 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -class LinearRamp(_DOFSignal): - def __init__(self, dof_signal: _DOFSignal, t_start=0.0, ramp_length=1.0): - self._dof_signal = dof_signal +class LinearRamp(_DOF): + """ + Linear ramp-up wrapper for a DOF signal. + """ + + def __init__(self, dof: _DOF, t_start=0.0, ramp_length=1.0): + self._dof = dof self._t_start = t_start self._ramp_length = ramp_length def __call__(self, fs, n): - y, dydt, d2ydt2 = self._dof_signal(fs, n) + y, dydt, d2ydt2 = self._dof(fs, n) # Time dt = 1.0 / fs @@ -130,7 +138,10 @@ def __call__(self, fs, n): ramp = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) - return ramp*y, ramp*dydt, ramp*d2ydt2 + return ramp * y, ramp * dydt, ramp * d2ydt2 + + +DOF = SineDOF | ConstantDOF | LinearRamp class IMUSimulator: @@ -139,17 +150,17 @@ class IMUSimulator: Parameters ---------- - pos_x : float or DOFSignal, default 0.0 + pos_x : float or DOF, default 0.0 X position signal. - pos_y : float or DOFSignal, default 0.0 + pos_y : float or DOF, default 0.0 Y position signal. - pos_z : float or DOFSignal, default 0.0 + pos_z : float or DOF, default 0.0 Z position signal. - alpha : float or DOFSignal, default 0.0 + alpha : float or DOF, default 0.0 Roll signal. - beta : float or DOFSignal, default 0.0 + beta : float or DOF, default 0.0 Pitch signal - gamma : float or DOFSignal, default 0.0 + gamma : float or DOF, default 0.0 Yaw signal degrees: bool, default False Whether to interpret the Euler angle signals as degrees (True) or radians (False). @@ -163,12 +174,12 @@ class IMUSimulator: def __init__( self, - pos_x: float | _DOFSignal = 0.0, - pos_y: float | _DOFSignal = 0.0, - pos_z: float | _DOFSignal = 0.0, - alpha: float | _DOFSignal = 0.0, - beta: float | _DOFSignal = 0.0, - gamma: float | _DOFSignal = 0.0, + pos_x: float | DOF = 0.0, + pos_y: float | DOF = 0.0, + pos_z: float | DOF = 0.0, + alpha: float | DOF = 0.0, + beta: float | DOF = 0.0, + gamma: float | DOF = 0.0, degrees=False, g=9.80665, nav_frame="NED", @@ -189,18 +200,18 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if not isinstance(self._pos_x_sig, _DOFSignal): - self._pos_x_sig = ConstantDOFSignal(self._pos_x_sig) - if not isinstance(self._pos_y_sig, _DOFSignal): - self._pos_y_sig = ConstantDOFSignal(self._pos_y_sig) - if not isinstance(self._pos_z_sig, _DOFSignal): - self._pos_z_sig = ConstantDOFSignal(self._pos_z_sig) - if not isinstance(self._alpha_sig, _DOFSignal): - self._alpha_sig = ConstantDOFSignal(self._alpha_sig) - if not isinstance(self._beta_sig, _DOFSignal): - self._beta_sig = ConstantDOFSignal(self._beta_sig) - if not isinstance(self._gamma_sig, _DOFSignal): - self._gamma_sig = ConstantDOFSignal(self._gamma_sig) + if not isinstance(self._pos_x_sig, _DOF): + self._pos_x_sig = ConstantDOF(self._pos_x_sig) + if not isinstance(self._pos_y_sig, _DOF): + self._pos_y_sig = ConstantDOF(self._pos_y_sig) + if not isinstance(self._pos_z_sig, _DOF): + self._pos_z_sig = ConstantDOF(self._pos_z_sig) + if not isinstance(self._alpha_sig, _DOF): + self._alpha_sig = ConstantDOF(self._alpha_sig) + if not isinstance(self._beta_sig, _DOF): + self._beta_sig = ConstantDOF(self._beta_sig) + if not isinstance(self._gamma_sig, _DOF): + self._gamma_sig = ConstantDOF(self._gamma_sig) def _specific_force_body(self, pos, acc, euler): """ From c591c3aa872c8ef0e3e045ab0a112e17c79e0089 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 14:06:08 +0100 Subject: [PATCH 050/129] rename to rampup --- src/smsfusion/simulate/_simulate.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 29e5e56c..64d7b54f 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -119,7 +119,7 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -class LinearRamp(_DOF): +class LinearRampUp(_DOF): """ Linear ramp-up wrapper for a DOF signal. """ @@ -141,7 +141,7 @@ def __call__(self, fs, n): return ramp * y, ramp * dydt, ramp * d2ydt2 -DOF = SineDOF | ConstantDOF | LinearRamp +DOF = SineDOF | ConstantDOF | LinearRampUp class IMUSimulator: From e5ce1dad829534d86728d1246cdd1d093a4c1c59 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 14:08:25 +0100 Subject: [PATCH 051/129] some changes --- src/smsfusion/simulate/_simulate.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 64d7b54f..ad4aded3 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -121,7 +121,17 @@ def __call__(self, fs, n): class LinearRampUp(_DOF): """ - Linear ramp-up wrapper for a DOF signal. + Linear ramp-up wrapper for DOF signals. + + Parameters + ---------- + dof : _DOF + The DOF signal to wrap with a linear ramp-up. + t_start : float, default 0.0 + The start time of the ramp-up in seconds. Default is 0.0, i.e., the ramp-up + starts immediately. + ramp_length : float, default 1.0 + The duration of the ramp-up in seconds. Default is 1.0 second. """ def __init__(self, dof: _DOF, t_start=0.0, ramp_length=1.0): @@ -136,9 +146,9 @@ def __call__(self, fs, n): dt = 1.0 / fs t = dt * np.arange(n) - ramp = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) + ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) - return ramp * y, ramp * dydt, ramp * d2ydt2 + return ramp_up * y, ramp_up * dydt, ramp_up * d2ydt2 DOF = SineDOF | ConstantDOF | LinearRampUp From 2cd937b628b1343a7e1cc396ae7a68880c5d2524 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 16:40:16 +0100 Subject: [PATCH 052/129] basedof --- src/smsfusion/simulate/_simulate.py | 121 ++++++++++++++++------------ 1 file changed, 71 insertions(+), 50 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index ad4aded3..286c6653 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -5,17 +5,46 @@ from smsfusion._transforms import _rot_matrix_from_euler -class _DOF(ABC): +class BaseDOF(ABC): """ Abstract base class for degree of freedom (DOF) signals. """ + def _time(self, fs, n): + dt = 1.0 / fs + t = dt * np.arange(n) + return t + @abstractmethod - def __call__(self, fs, n): + def _y(self, fs, n): + raise NotImplementedError("Not implemented.") + + @abstractmethod + def _dydt(self, fs, n): + raise NotImplementedError("Not implemented.") + + @abstractmethod + def _d2ydt2(self, fs, n): raise NotImplementedError("Not implemented.") + def __call__(self, fs, n): + """ + Generate a length-n signal and its first and second time derivative. -class SineDOF(_DOF): + Parameters + ---------- + fs : float + Sampling frequency in Hz. + n : int + Number of samples to generate. + """ + y = self._y(fs, n) + dydt = self._dydt(fs, n) + d2ydt2 = self._d2ydt2(fs, n) + return y, dydt, d2ydt2 + + +class SineDOF(BaseDOF): """ 1D sine wave DOF signal generator. @@ -56,28 +85,23 @@ def __init__(self, amp=1.0, omega=1.0, phase=0.0, offset=0.0, phase_degrees=Fals self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset - def __call__(self, fs, n): - """ - Generate a sine wave and its derivative. - - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - """ - dt = 1.0 / fs - t = dt * np.arange(n) - + def _y(self, fs, n): + t = self._time(fs, n) y = self._amp * np.sin(self._omega * t + self._phase) + self._offset + return y + + def _dydt(self, fs, n): + t = self._time(fs, n) dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) - d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) + return dydt - return y, dydt, d2ydt2 + def _d2ydt2(self, fs, n): + t = self._time(fs, n) + d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) + return d2ydt2 -class ConstantDOF(_DOF): +class ConstantDOF(BaseDOF): """ 1D constant DOF signal generator. @@ -100,26 +124,17 @@ class ConstantDOF(_DOF): def __init__(self, value=0.0): self._value = value - def __call__(self, fs, n): - """ - Generate a constant signal and its derivative. + def _y(self, fs, n): + return np.full(int(n), self._value) - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - """ - n = int(n) - y = np.full(n, self._value) - dydt = np.zeros(n) - d2ydt2 = np.zeros(n) + def _dydt(self, fs, n): + return np.zeros(n) - return y, dydt, d2ydt2 + def _d2ydt2(self, fs, n): + return np.zeros(n) -class LinearRampUp(_DOF): +class LinearRampUp(BaseDOF): """ Linear ramp-up wrapper for DOF signals. @@ -134,21 +149,27 @@ class LinearRampUp(_DOF): The duration of the ramp-up in seconds. Default is 1.0 second. """ - def __init__(self, dof: _DOF, t_start=0.0, ramp_length=1.0): + def __init__(self, dof: SineDOF | ConstantDOF, t_start=0.0, ramp_length=1.0): self._dof = dof self._t_start = t_start self._ramp_length = ramp_length - def __call__(self, fs, n): - y, dydt, d2ydt2 = self._dof(fs, n) + def _ramp_up(self, fs, n): + t = self._time(fs, n) + ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) + return ramp_up - # Time - dt = 1.0 / fs - t = dt * np.arange(n) + def _y(self, fs, n): + ramp_up = self._ramp_up(fs, n) + return ramp_up * self._dof(fs, n)[0] - ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) + def _dydt(self, fs, n): + ramp_up = self._ramp_up(fs, n) + return ramp_up * self._dof(fs, n)[1] - return ramp_up * y, ramp_up * dydt, ramp_up * d2ydt2 + def _d2ydt2(self, fs, n): + ramp_up = self._ramp_up(fs, n) + return ramp_up * self._dof(fs, n)[2] DOF = SineDOF | ConstantDOF | LinearRampUp @@ -210,17 +231,17 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if not isinstance(self._pos_x_sig, _DOF): + if not isinstance(self._pos_x_sig, BaseDOF): self._pos_x_sig = ConstantDOF(self._pos_x_sig) - if not isinstance(self._pos_y_sig, _DOF): + if not isinstance(self._pos_y_sig, BaseDOF): self._pos_y_sig = ConstantDOF(self._pos_y_sig) - if not isinstance(self._pos_z_sig, _DOF): + if not isinstance(self._pos_z_sig, BaseDOF): self._pos_z_sig = ConstantDOF(self._pos_z_sig) - if not isinstance(self._alpha_sig, _DOF): + if not isinstance(self._alpha_sig, BaseDOF): self._alpha_sig = ConstantDOF(self._alpha_sig) - if not isinstance(self._beta_sig, _DOF): + if not isinstance(self._beta_sig, BaseDOF): self._beta_sig = ConstantDOF(self._beta_sig) - if not isinstance(self._gamma_sig, _DOF): + if not isinstance(self._gamma_sig, BaseDOF): self._gamma_sig = ConstantDOF(self._gamma_sig) def _specific_force_body(self, pos, acc, euler): From fac75281f10813b5d7abe6cfd97a55ff20813ce0 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 16:51:54 +0100 Subject: [PATCH 053/129] docstrings --- src/smsfusion/simulate/_simulate.py | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 286c6653..b60383c1 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -7,7 +7,7 @@ class BaseDOF(ABC): """ - Abstract base class for degree of freedom (DOF) signals. + Abstract base class for degree of freedom (DOF) signal generators. """ def _time(self, fs, n): @@ -37,6 +37,15 @@ def __call__(self, fs, n): Sampling frequency in Hz. n : int Number of samples to generate. + + Returns + ------- + y : numpy.ndarray, shape (n,) + Signal. + dydt : numpy.ndarray, shape (n,) + First time derivative of signal. + d2ydt2 : numpy.ndarray, shape (n,) + Second time derivative of signal. """ y = self._y(fs, n) dydt = self._dydt(fs, n) @@ -172,7 +181,7 @@ def _d2ydt2(self, fs, n): return ramp_up * self._dof(fs, n)[2] -DOF = SineDOF | ConstantDOF | LinearRampUp +DOFSignal = SineDOF | ConstantDOF | LinearRampUp class IMUSimulator: @@ -205,12 +214,12 @@ class IMUSimulator: def __init__( self, - pos_x: float | DOF = 0.0, - pos_y: float | DOF = 0.0, - pos_z: float | DOF = 0.0, - alpha: float | DOF = 0.0, - beta: float | DOF = 0.0, - gamma: float | DOF = 0.0, + pos_x: float | DOFSignal = 0.0, + pos_y: float | DOFSignal = 0.0, + pos_z: float | DOFSignal = 0.0, + alpha: float | DOFSignal = 0.0, + beta: float | DOFSignal = 0.0, + gamma: float | DOFSignal = 0.0, degrees=False, g=9.80665, nav_frame="NED", From dffef7456221b7af02691a1c1575f5b6977c3e9e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 16:54:42 +0100 Subject: [PATCH 054/129] simplify init isinstance --- src/smsfusion/simulate/_simulate.py | 27 +++++++-------------------- 1 file changed, 7 insertions(+), 20 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index b60383c1..6b315fb5 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -22,7 +22,7 @@ def _y(self, fs, n): @abstractmethod def _dydt(self, fs, n): raise NotImplementedError("Not implemented.") - + @abstractmethod def _d2ydt2(self, fs, n): raise NotImplementedError("Not implemented.") @@ -224,12 +224,12 @@ def __init__( g=9.80665, nav_frame="NED", ): - self._pos_x_sig = pos_x - self._pos_y_sig = pos_y - self._pos_z_sig = pos_z - self._alpha_sig = alpha - self._beta_sig = beta - self._gamma_sig = gamma + self._pos_x_sig = pos_x if isinstance(pos_x, BaseDOF) else ConstantDOF(pos_x) + self._pos_y_sig = pos_y if isinstance(pos_y, BaseDOF) else ConstantDOF(pos_y) + self._pos_z_sig = pos_z if isinstance(pos_z, BaseDOF) else ConstantDOF(pos_z) + self._alpha_sig = alpha if isinstance(alpha, BaseDOF) else ConstantDOF(alpha) + self._beta_sig = beta if isinstance(beta, BaseDOF) else ConstantDOF(beta) + self._gamma_sig = gamma if isinstance(gamma, BaseDOF) else ConstantDOF(gamma) self._degrees = degrees self._nav_frame = nav_frame.lower() @@ -240,19 +240,6 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - if not isinstance(self._pos_x_sig, BaseDOF): - self._pos_x_sig = ConstantDOF(self._pos_x_sig) - if not isinstance(self._pos_y_sig, BaseDOF): - self._pos_y_sig = ConstantDOF(self._pos_y_sig) - if not isinstance(self._pos_z_sig, BaseDOF): - self._pos_z_sig = ConstantDOF(self._pos_z_sig) - if not isinstance(self._alpha_sig, BaseDOF): - self._alpha_sig = ConstantDOF(self._alpha_sig) - if not isinstance(self._beta_sig, BaseDOF): - self._beta_sig = ConstantDOF(self._beta_sig) - if not isinstance(self._gamma_sig, BaseDOF): - self._gamma_sig = ConstantDOF(self._gamma_sig) - def _specific_force_body(self, pos, acc, euler): """ Specific force in the body frame. From 6d0da8805c5e39fac53f7d106cb5abafefa7c0ff Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 10 Nov 2025 16:56:49 +0100 Subject: [PATCH 055/129] small simplification --- src/smsfusion/simulate/_simulate.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 6b315fb5..ef608e75 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -224,12 +224,12 @@ def __init__( g=9.80665, nav_frame="NED", ): - self._pos_x_sig = pos_x if isinstance(pos_x, BaseDOF) else ConstantDOF(pos_x) - self._pos_y_sig = pos_y if isinstance(pos_y, BaseDOF) else ConstantDOF(pos_y) - self._pos_z_sig = pos_z if isinstance(pos_z, BaseDOF) else ConstantDOF(pos_z) - self._alpha_sig = alpha if isinstance(alpha, BaseDOF) else ConstantDOF(alpha) - self._beta_sig = beta if isinstance(beta, BaseDOF) else ConstantDOF(beta) - self._gamma_sig = gamma if isinstance(gamma, BaseDOF) else ConstantDOF(gamma) + self._pos_x = pos_x if isinstance(pos_x, BaseDOF) else ConstantDOF(pos_x) + self._pos_y = pos_y if isinstance(pos_y, BaseDOF) else ConstantDOF(pos_y) + self._pos_z = pos_z if isinstance(pos_z, BaseDOF) else ConstantDOF(pos_z) + self._alpha = alpha if isinstance(alpha, BaseDOF) else ConstantDOF(alpha) + self._beta = beta if isinstance(beta, BaseDOF) else ConstantDOF(beta) + self._gamma = gamma if isinstance(gamma, BaseDOF) else ConstantDOF(gamma) self._degrees = degrees self._nav_frame = nav_frame.lower() @@ -318,12 +318,12 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # DOFs and corresponding rates and accelerations - pos_x, pos_x_dot, pos_x_ddot = self._pos_x_sig(fs, n) - pos_y, pos_y_dot, pos_y_ddot = self._pos_y_sig(fs, n) - pos_z, pos_z_dot, pos_z_ddot = self._pos_z_sig(fs, n) - alpha, alpha_dot, _ = self._alpha_sig(fs, n) - beta, beta_dot, _ = self._beta_sig(fs, n) - gamma, gamma_dot, _ = self._gamma_sig(fs, n) + pos_x, pos_x_dot, pos_x_ddot = self._pos_x(fs, n) + pos_y, pos_y_dot, pos_y_ddot = self._pos_y(fs, n) + pos_z, pos_z_dot, pos_z_ddot = self._pos_z(fs, n) + alpha, alpha_dot, _ = self._alpha(fs, n) + beta, beta_dot, _ = self._beta(fs, n) + gamma, gamma_dot, _ = self._gamma(fs, n) pos = np.column_stack([pos_x, pos_y, pos_z]) vel = np.column_stack([pos_x_dot, pos_y_dot, pos_z_dot]) From 034c1d531cfa081a40849f53923a5d227abdbbf1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 09:31:02 +0100 Subject: [PATCH 056/129] docstring --- src/smsfusion/simulate/_simulate.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index ef608e75..a57ff20f 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -190,17 +190,17 @@ class IMUSimulator: Parameters ---------- - pos_x : float or DOF, default 0.0 + pos_x : float or DOFSignal, default 0.0 X position signal. - pos_y : float or DOF, default 0.0 + pos_y : float or DOFSignal, default 0.0 Y position signal. - pos_z : float or DOF, default 0.0 + pos_z : float or DOFSignal, default 0.0 Z position signal. - alpha : float or DOF, default 0.0 + alpha : float or DOFSignal, default 0.0 Roll signal. - beta : float or DOF, default 0.0 + beta : float or DOFSignal, default 0.0 Pitch signal - gamma : float or DOF, default 0.0 + gamma : float or DOFSignal, default 0.0 Yaw signal degrees: bool, default False Whether to interpret the Euler angle signals as degrees (True) or radians (False). From 1d36c5a382e481ff77afcb39c6a7d586bd96f4e5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 10:40:52 +0100 Subject: [PATCH 057/129] simplify typing --- src/smsfusion/simulate/_simulate.py | 49 ++++++++++++++--------------- 1 file changed, 23 insertions(+), 26 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index a57ff20f..03cea464 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -5,7 +5,7 @@ from smsfusion._transforms import _rot_matrix_from_euler -class BaseDOF(ABC): +class DOF(ABC): """ Abstract base class for degree of freedom (DOF) signal generators. """ @@ -53,7 +53,7 @@ def __call__(self, fs, n): return y, dydt, d2ydt2 -class SineDOF(BaseDOF): +class SineDOF(DOF): """ 1D sine wave DOF signal generator. @@ -110,7 +110,7 @@ def _d2ydt2(self, fs, n): return d2ydt2 -class ConstantDOF(BaseDOF): +class ConstantDOF(DOF): """ 1D constant DOF signal generator. @@ -143,7 +143,7 @@ def _d2ydt2(self, fs, n): return np.zeros(n) -class LinearRampUp(BaseDOF): +class LinearRampUp(DOF): """ Linear ramp-up wrapper for DOF signals. @@ -158,7 +158,7 @@ class LinearRampUp(BaseDOF): The duration of the ramp-up in seconds. Default is 1.0 second. """ - def __init__(self, dof: SineDOF | ConstantDOF, t_start=0.0, ramp_length=1.0): + def __init__(self, dof: DOF, t_start=0.0, ramp_length=1.0): self._dof = dof self._t_start = t_start self._ramp_length = ramp_length @@ -181,26 +181,23 @@ def _d2ydt2(self, fs, n): return ramp_up * self._dof(fs, n)[2] -DOFSignal = SineDOF | ConstantDOF | LinearRampUp - - class IMUSimulator: """ IMU simulator. Parameters ---------- - pos_x : float or DOFSignal, default 0.0 + pos_x : float or DOF, default 0.0 X position signal. - pos_y : float or DOFSignal, default 0.0 + pos_y : float or DOF, default 0.0 Y position signal. - pos_z : float or DOFSignal, default 0.0 + pos_z : float or DOF, default 0.0 Z position signal. - alpha : float or DOFSignal, default 0.0 + alpha : float or DOF, default 0.0 Roll signal. - beta : float or DOFSignal, default 0.0 + beta : float or DOF, default 0.0 Pitch signal - gamma : float or DOFSignal, default 0.0 + gamma : float or DOF, default 0.0 Yaw signal degrees: bool, default False Whether to interpret the Euler angle signals as degrees (True) or radians (False). @@ -214,22 +211,22 @@ class IMUSimulator: def __init__( self, - pos_x: float | DOFSignal = 0.0, - pos_y: float | DOFSignal = 0.0, - pos_z: float | DOFSignal = 0.0, - alpha: float | DOFSignal = 0.0, - beta: float | DOFSignal = 0.0, - gamma: float | DOFSignal = 0.0, + pos_x: float | DOF = 0.0, + pos_y: float | DOF = 0.0, + pos_z: float | DOF = 0.0, + alpha: float | DOF = 0.0, + beta: float | DOF = 0.0, + gamma: float | DOF = 0.0, degrees=False, g=9.80665, nav_frame="NED", ): - self._pos_x = pos_x if isinstance(pos_x, BaseDOF) else ConstantDOF(pos_x) - self._pos_y = pos_y if isinstance(pos_y, BaseDOF) else ConstantDOF(pos_y) - self._pos_z = pos_z if isinstance(pos_z, BaseDOF) else ConstantDOF(pos_z) - self._alpha = alpha if isinstance(alpha, BaseDOF) else ConstantDOF(alpha) - self._beta = beta if isinstance(beta, BaseDOF) else ConstantDOF(beta) - self._gamma = gamma if isinstance(gamma, BaseDOF) else ConstantDOF(gamma) + self._pos_x = pos_x if isinstance(pos_x, DOF) else ConstantDOF(pos_x) + self._pos_y = pos_y if isinstance(pos_y, DOF) else ConstantDOF(pos_y) + self._pos_z = pos_z if isinstance(pos_z, DOF) else ConstantDOF(pos_z) + self._alpha = alpha if isinstance(alpha, DOF) else ConstantDOF(alpha) + self._beta = beta if isinstance(beta, DOF) else ConstantDOF(beta) + self._gamma = gamma if isinstance(gamma, DOF) else ConstantDOF(gamma) self._degrees = degrees self._nav_frame = nav_frame.lower() From 1220f437fc77106a2525b2091066e4cff20f0e40 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 10:42:04 +0100 Subject: [PATCH 058/129] small fix --- src/smsfusion/simulate/_simulate.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 03cea464..ba9848f8 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -10,11 +10,6 @@ class DOF(ABC): Abstract base class for degree of freedom (DOF) signal generators. """ - def _time(self, fs, n): - dt = 1.0 / fs - t = dt * np.arange(n) - return t - @abstractmethod def _y(self, fs, n): raise NotImplementedError("Not implemented.") @@ -27,6 +22,11 @@ def _dydt(self, fs, n): def _d2ydt2(self, fs, n): raise NotImplementedError("Not implemented.") + def _time(self, fs, n): + dt = 1.0 / fs + t = dt * np.arange(n) + return t + def __call__(self, fs, n): """ Generate a length-n signal and its first and second time derivative. From 7b972e5ff2bda49295cf878ef8d8482b03c577b8 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 10:43:41 +0100 Subject: [PATCH 059/129] add ramp up to init --- src/smsfusion/simulate/__init__.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 7cd12b31..e96f1f74 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOF, IMUSimulator, SineDOF +from ._simulate import ConstantDOF, IMUSimulator, LinearRampUp, SineDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp"] From 31cba87c7a28d31c023052e9f6ffc5e9ce8f9b93 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 10:58:26 +0100 Subject: [PATCH 060/129] freq_hz --- src/smsfusion/simulate/_simulate.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index ba9848f8..0f5ea452 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -80,7 +80,7 @@ class SineDOF(DOF): Phase offset of the sine wave. Default is 0.0. offset : float, default 0.0 Offset of the sine wave. Default is 0.0. - hz : bool, optional + freq_hz : bool, optional If True, interpret `omega` as frequency in Hz. If False, interpret as angular frequency in radians per second. Default is False. phase_degrees : bool, optional @@ -88,9 +88,17 @@ class SineDOF(DOF): Default is False. """ - def __init__(self, amp=1.0, omega=1.0, phase=0.0, offset=0.0, phase_degrees=False): + def __init__( + self, + amp=1.0, + omega=1.0, + phase=0.0, + offset=0.0, + freq_hz=False, + phase_degrees=False, + ): self._amp = amp - self._omega = omega + self._omega = 2 * np.pi * omega if freq_hz else omega self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset From 8242533f3d5e8538a2501589f92e06a1a8e7d18b Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 11:10:32 +0100 Subject: [PATCH 061/129] reformulate DOF to take time --- src/smsfusion/simulate/_simulate.py | 74 +++++++++++++---------------- 1 file changed, 32 insertions(+), 42 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 0f5ea452..68af55d1 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -11,23 +11,18 @@ class DOF(ABC): """ @abstractmethod - def _y(self, fs, n): + def _y(self, t): raise NotImplementedError("Not implemented.") @abstractmethod - def _dydt(self, fs, n): + def _dydt(self, t): raise NotImplementedError("Not implemented.") @abstractmethod - def _d2ydt2(self, fs, n): + def _d2ydt2(self, t): raise NotImplementedError("Not implemented.") - def _time(self, fs, n): - dt = 1.0 / fs - t = dt * np.arange(n) - return t - - def __call__(self, fs, n): + def __call__(self, t): """ Generate a length-n signal and its first and second time derivative. @@ -47,9 +42,9 @@ def __call__(self, fs, n): d2ydt2 : numpy.ndarray, shape (n,) Second time derivative of signal. """ - y = self._y(fs, n) - dydt = self._dydt(fs, n) - d2ydt2 = self._d2ydt2(fs, n) + y = self._y(t) + dydt = self._dydt(t) + d2ydt2 = self._d2ydt2(t) return y, dydt, d2ydt2 @@ -102,18 +97,15 @@ def __init__( self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset - def _y(self, fs, n): - t = self._time(fs, n) + def _y(self, t): y = self._amp * np.sin(self._omega * t + self._phase) + self._offset return y - def _dydt(self, fs, n): - t = self._time(fs, n) + def _dydt(self, t): dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) return dydt - def _d2ydt2(self, fs, n): - t = self._time(fs, n) + def _d2ydt2(self, t): d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) return d2ydt2 @@ -141,14 +133,14 @@ class ConstantDOF(DOF): def __init__(self, value=0.0): self._value = value - def _y(self, fs, n): - return np.full(int(n), self._value) + def _y(self, t): + return np.full_like(t, self._value) - def _dydt(self, fs, n): - return np.zeros(n) + def _dydt(self, t): + return np.zeros_like(t) - def _d2ydt2(self, fs, n): - return np.zeros(n) + def _d2ydt2(self, t): + return np.zeros_like(t) class LinearRampUp(DOF): @@ -171,23 +163,21 @@ def __init__(self, dof: DOF, t_start=0.0, ramp_length=1.0): self._t_start = t_start self._ramp_length = ramp_length - def _ramp_up(self, fs, n): - t = self._time(fs, n) + def _ramp_up(self, t): ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) return ramp_up - def _y(self, fs, n): - ramp_up = self._ramp_up(fs, n) - return ramp_up * self._dof(fs, n)[0] - - def _dydt(self, fs, n): - ramp_up = self._ramp_up(fs, n) - return ramp_up * self._dof(fs, n)[1] + def _y(self, t): + ramp_up = self._ramp_up(t) + return ramp_up * self._dof._y(t) - def _d2ydt2(self, fs, n): - ramp_up = self._ramp_up(fs, n) - return ramp_up * self._dof(fs, n)[2] + def _dydt(self, t): + ramp_up = self._ramp_up(t) + return ramp_up * self._dof._dydt(t) + def _d2ydt2(self, t): + ramp_up = self._ramp_up(t) + return ramp_up * self._dof._d2ydt2(t) class IMUSimulator: """ @@ -323,12 +313,12 @@ def __call__(self, fs: float, n: int, degrees=None): t = dt * np.arange(n) # DOFs and corresponding rates and accelerations - pos_x, pos_x_dot, pos_x_ddot = self._pos_x(fs, n) - pos_y, pos_y_dot, pos_y_ddot = self._pos_y(fs, n) - pos_z, pos_z_dot, pos_z_ddot = self._pos_z(fs, n) - alpha, alpha_dot, _ = self._alpha(fs, n) - beta, beta_dot, _ = self._beta(fs, n) - gamma, gamma_dot, _ = self._gamma(fs, n) + pos_x, pos_x_dot, pos_x_ddot = self._pos_x(t) + pos_y, pos_y_dot, pos_y_ddot = self._pos_y(t) + pos_z, pos_z_dot, pos_z_ddot = self._pos_z(t) + alpha, alpha_dot, _ = self._alpha(t) + beta, beta_dot, _ = self._beta(t) + gamma, gamma_dot, _ = self._gamma(t) pos = np.column_stack([pos_x, pos_y, pos_z]) vel = np.column_stack([pos_x_dot, pos_y_dot, pos_z_dot]) From 1b56a0981f92651ab0cf2a1259d13cd0ab3b7f86 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 11:13:20 +0100 Subject: [PATCH 062/129] docstring --- src/smsfusion/simulate/_simulate.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 68af55d1..9624e638 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -24,14 +24,12 @@ def _d2ydt2(self, t): def __call__(self, t): """ - Generate a length-n signal and its first and second time derivative. + Generate signal and its first and second time derivatives. Parameters ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. + t : np.ndarray, shape (n,) + Time in seconds. Returns ------- From 934ee7d845ede9c230bd58ff16ebd43b2827c22a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Tue, 11 Nov 2025 11:16:13 +0100 Subject: [PATCH 063/129] add base class to init --- src/smsfusion/simulate/__init__.py | 4 ++-- src/smsfusion/simulate/_simulate.py | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index e96f1f74..140d1993 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOF, IMUSimulator, LinearRampUp, SineDOF +from ._simulate import DOF, ConstantDOF, IMUSimulator, LinearRampUp, SineDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp", "DOF"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 9624e638..f9760d4b 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -177,6 +177,7 @@ def _d2ydt2(self, t): ramp_up = self._ramp_up(t) return ramp_up * self._dof._d2ydt2(t) + class IMUSimulator: """ IMU simulator. From b7e07bd466f4316d8d646ff84a0b08fc852215f1 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 13 Nov 2025 10:33:22 +0100 Subject: [PATCH 064/129] delete coning code --- src/smsfusion/simulate/_simulate.py | 37 ++++++++++++++++++++++------- 1 file changed, 28 insertions(+), 9 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index f9760d4b..e034ad37 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -22,27 +22,46 @@ def _dydt(self, t): def _d2ydt2(self, t): raise NotImplementedError("Not implemented.") + def y(self, t): + """ + Generates y(t) signal. + """ + return self._y(t) + + def dydt(self, t): + """ + Generates dy(t)/dt signal. + """ + return self._dydt(t) + + def d2ydt2(self, t): + """ + Generates d2y(t)/dt2 signal. + """ + return self._d2ydt2(t) + def __call__(self, t): """ - Generate signal and its first and second time derivatives. + Generates y(t), dy(t)/dt, and d2y(t)/dt2 signals. Parameters ---------- - t : np.ndarray, shape (n,) - Time in seconds. + t : ndarray, shape (n,) + Time vector in seconds. Returns ------- - y : numpy.ndarray, shape (n,) - Signal. - dydt : numpy.ndarray, shape (n,) - First time derivative of signal. - d2ydt2 : numpy.ndarray, shape (n,) - Second time derivative of signal. + y : ndarray, shape (n,) + DOF signal y(t). + dydt : ndarray, shape (n,) + Time derivative, dy(t)/dt, of DOF signal. + d2ydt2 : ndarray, shape (n,) + Second time derivative, d2y(t)/dt2, of DOF signal. """ y = self._y(t) dydt = self._dydt(t) d2ydt2 = self._d2ydt2(t) + return y, dydt, d2ydt2 From 54b37261c060e8473c08353cb38eddadba86e33e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Thu, 13 Nov 2025 10:34:32 +0100 Subject: [PATCH 065/129] delete coning code --- src/smsfusion/simulate/_coning.py | 480 ------------------------------ 1 file changed, 480 deletions(-) delete mode 100644 src/smsfusion/simulate/_coning.py diff --git a/src/smsfusion/simulate/_coning.py b/src/smsfusion/simulate/_coning.py deleted file mode 100644 index 4bcccecf..00000000 --- a/src/smsfusion/simulate/_coning.py +++ /dev/null @@ -1,480 +0,0 @@ -import numpy as np - - -class ConingSimulator: - """ - Coning trajectory generator and IMU (gyro) simulator. - - Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with - respect to the inertial frame's z-axis. The sensor rotates about its z-axis - with a constant rate (the spin rate), while also spinning around the inertial - frame's z-axis with a constant rate (the precession rate). The IMU sensor's - z-axis will thus trace out a cone shape, with the half-angle defined by beta. - - Parameters - ---------- - omega_prec : float - Precession angular velocity in rad/s. I.e., the rate at which the IMU sensor's - z-axis rotates about the inertial frame's z-axis. - omega_spin : float - Spin angular velocity in rad/s. I.e., the rate at which the IMU sensor - rotates about its own z-axis. - beta : float - Cone half-angle in radians. I.e., the angle between the IMU sensor's z-axis - and the inertial frame's z-axis. - degrees: bool - Whether to interpret beta in degrees (True) or radians (False), and angular - velocities in deg/s (True) or rad/s (False). - """ - - def __init__( - self, - omega_prec: float = 180.0, - omega_spin: float = 360.0, - beta: float = 45.0, - degrees: bool = True, - ): - self._beta = beta - self._w_prec = omega_prec - self._w_spin = omega_spin - self._psi0 = 0.0 - self._phi0 = 0.0 - - if degrees: - self._beta = np.deg2rad(self._beta) - self._w_prec = np.deg2rad(self._w_prec) - self._w_spin = np.deg2rad(self._w_spin) - - @staticmethod - def _rot_matrix_from_euler_zyz(psi, theta, phi): - """ - Euler ZYZ rotation matrix: - R = Rz(psi) @ Ry(theta) @ Rz(phi) - - Parameters - ---------- - psi, theta, phi : array_like - Euler angles (ZYZ) in radians. Broadcasting is supported. - - Returns - ------- - R : ndarray - Rotation matrix/matrices of shape (..., 3, 3). - """ - cpsi, spsi = np.cos(psi), np.sin(psi) - ctheta, stheta = np.cos(theta), np.sin(theta) - cphi, sphi = np.cos(phi), np.sin(phi) - - R11 = cpsi * ctheta * cphi - spsi * sphi - R12 = -cpsi * ctheta * sphi - spsi * cphi - R13 = cpsi * stheta - R21 = spsi * ctheta * cphi + cpsi * sphi - R22 = -spsi * ctheta * sphi + cpsi * cphi - R23 = spsi * stheta - R31 = -stheta * cphi - R32 = stheta * sphi - R33 = ctheta - - n = len(psi) - R = np.empty((n, 3, 3), dtype="float64") - - R[..., 0, 0] = R11 - R[..., 0, 1] = R12 - R[..., 0, 2] = R13 - R[..., 1, 0] = R21 - R[..., 1, 1] = R22 - R[..., 1, 2] = R23 - R[..., 2, 0] = R31 - R[..., 2, 1] = R32 - R[..., 2, 2] = R33 - - return R - - @staticmethod - def _euler_from_rot_matrix_zyx(R): - R11 = R[:, 0, 0] - R21 = R[:, 1, 0] - R31 = R[:, 2, 0] - R32 = R[:, 2, 1] - R33 = R[:, 2, 2] - yaw = np.arctan2(R21, R11) - pitch = -np.arcsin(R31) - roll = np.arctan2(R32, R33) - - euler_zyx = np.column_stack([roll, pitch, yaw]) - - return euler_zyx - - def _body_rates_from_euler_zyz(self, psi, theta, phi): - p = -self._w_prec * np.sin(theta) * np.cos(phi) - q = self._w_prec * np.sin(theta) * np.sin(phi) - r = self._w_spin + self._w_prec * np.cos(theta) # constant - r = np.full_like(p, r) - w_b = np.column_stack([p, q, r]) - - return w_b - - def __call__(self, fs: float, n: int): - """ - Generate a length-n coning trajectory and corresponding body-frame angular - velocities as measured by an IMU sensor. - - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - - Returns - ------- - t : ndarray, shape (n,) - Time vector in seconds. - euler_zyx : ndarray, shape (n, 3) - ZYX Euler angles [yaw, pitch, roll] in radians. - omega_b : ndarray, shape (n, 3) - Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). - """ - - # Time - dt = 1.0 / fs - t = dt * np.arange(n) - - # ZYZ Euler angles - psi = self._psi0 + self._w_prec * t # precession angle - theta = self._beta * np.ones_like(t) # constant cone half-angle - phi = self._phi0 + self._w_spin * t # spin angle - - # Rotation matrix (body-to-inertial) - R = self._rot_matrix_from_euler_zyz(psi, theta, phi) - - # Extract ZYX Euler angles (yaw, pitch, roll) - euler_zyx = self._euler_from_rot_matrix_zyx(R) - - # Body frame angular velocities from ZYZ Euler angle rates - w_b = self._body_rates_from_euler_zyz(psi, theta, phi) - - return t, euler_zyx, w_b - - -class ConingSimulator2: - """ - Coning trajectory generator and IMU (gyro) simulator. - - Parameters - ---------- - omega_prec : float - Precession angular velocity in rad/s. - omega_spin : float - Spin angular velocity in rad/s. - beta : float - Cone half-angle in radians. - degrees: bool - Whether to interpret beta in degrees (True) or radians (False), and angular - velocities in deg/s (True) or rad/s (False). - """ - - def __init__( - self, omega_prec: float = 1.0, omega_spin: float = 2.0, degrees: bool = True - ): - self._w_prec = omega_prec - self._w_spin = omega_spin - - if degrees: - self._w_prec = np.deg2rad(self._w_prec) - self._w_spin = np.deg2rad(self._w_spin) - - def __call__(self, fs: float, n: int): - """ - Generate a length-n coning trajectory and corresponding body-frame angular - velocities as measured by an IMU sensor. - - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - - Returns - ------- - t : ndarray, shape (n,) - Time vector in seconds. - euler_zyx : ndarray, shape (n, 3) - ZYX Euler angles [yaw, pitch, roll] in radians. - omega_b : ndarray, shape (n, 3) - Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). - """ - - # Time - dt = 1.0 / fs - t = dt * np.arange(n) - - # ZYX Euler angles - alpha0 = 0.0 - gamma0 = 0.0 - beta0 = 0.0 - alpha_dot = self._w_spin - gamma_dot = self._w_prec - alpha = alpha0 + alpha_dot * t # spin angle - gamma = gamma0 + gamma_dot * t # precession angle - beta = beta0 * np.ones_like(t) # constant - euler = np.column_stack([alpha, beta, gamma]) - - # Body frame angular velocities from ZYZ Euler angle rates - w_x = alpha_dot * np.ones_like(t) - w_y = gamma_dot * np.sin(alpha) - w_z = gamma_dot * np.cos(alpha) - w_b = np.column_stack([w_x, w_y, w_z]) - - return t, euler, w_b - - -class ConingSimulator3: - """ - Coning trajectory generator and IMU (gyro) simulator. - - Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with - respect to the inertial frame's z-axis. The sensor rotates about its z-axis - with a constant rate (the spin rate), while also spinning around the inertial - frame's z-axis with a constant rate (the precession rate). The IMU sensor's - z-axis will thus trace out a cone shape, with the half-angle defined by beta. - - Parameters - ---------- - psi_fun : callable - Precession angle in radians as a function of time. I.e., rotation of IMU - sensor's z-axis about the inertial frame's z-axis. - psi_dot_fun : callable - Precession angular velocity in rad/s as a function of time. - phi_fun : callable - Spin angle in radians as a function of time. - phi_dot_fun : callable - Spin angular velocity in rad/s as a function of time. I.e., the rate at - which the IMU sensor rotates about its own z-axis. - beta : float - Cone half-angle in radians (constant). I.e., the angle between the IMU sensor's - z-axis and the inertial frame's z-axis. - degrees: bool - Whether to interpret beta in degrees (True) or radians (False), and angular - velocities in deg/s (True) or rad/s (False). - """ - - def __init__( - self, - psi_sim, - phi_sim, - beta: float = np.pi / 4.0, - degrees: bool = False, - ): - self._beta = (np.pi / 180.0) * beta if degrees else beta - self._psi_sim = psi_sim - self._phi_sim = phi_sim - self._psi0 = 0.0 - self._phi0 = 0.0 - - @staticmethod - def _rot_matrix_from_euler_zyz(psi, theta, phi): - """ - Euler ZYZ rotation matrix: - R = Rz(psi) @ Ry(theta) @ Rz(phi) - - Parameters - ---------- - psi, theta, phi : array_like - Euler angles (ZYZ) in radians. Broadcasting is supported. - - Returns - ------- - R : ndarray - Rotation matrix/matrices of shape (..., 3, 3). - """ - cpsi, spsi = np.cos(psi), np.sin(psi) - ctheta, stheta = np.cos(theta), np.sin(theta) - cphi, sphi = np.cos(phi), np.sin(phi) - - R11 = cpsi * ctheta * cphi - spsi * sphi - R12 = -cpsi * ctheta * sphi - spsi * cphi - R13 = cpsi * stheta - R21 = spsi * ctheta * cphi + cpsi * sphi - R22 = -spsi * ctheta * sphi + cpsi * cphi - R23 = spsi * stheta - R31 = -stheta * cphi - R32 = stheta * sphi - R33 = ctheta - - n = len(psi) - R = np.empty((n, 3, 3), dtype="float64") - - R[..., 0, 0] = R11 - R[..., 0, 1] = R12 - R[..., 0, 2] = R13 - R[..., 1, 0] = R21 - R[..., 1, 1] = R22 - R[..., 1, 2] = R23 - R[..., 2, 0] = R31 - R[..., 2, 1] = R32 - R[..., 2, 2] = R33 - - return R - - @staticmethod - def _euler_from_rot_matrix_zyx(R): - R11 = R[:, 0, 0] - R21 = R[:, 1, 0] - R31 = R[:, 2, 0] - R32 = R[:, 2, 1] - R33 = R[:, 2, 2] - yaw = np.arctan2(R21, R11) - pitch = -np.arcsin(R31) - roll = np.arctan2(R32, R33) - - euler_zyx = np.column_stack([roll, pitch, yaw]) - - return euler_zyx - - def _body_rates_from_euler_zyz(self, theta, phi, psi_dot, phi_dot): - p = -psi_dot * np.sin(theta) * np.cos(phi) - q = psi_dot * np.sin(theta) * np.sin(phi) - r = phi_dot + psi_dot * np.cos(theta) - r = np.full_like(p, r) - w_b = np.column_stack([p, q, r]) - - return w_b - - def __call__(self, fs: float, n: int): - """ - Generate a length-n coning trajectory and corresponding body-frame angular - velocities as measured by an IMU sensor. - - Parameters - ---------- - fs : float - Sampling frequency in Hz. - n : int - Number of samples to generate. - - Returns - ------- - t : ndarray, shape (n,) - Time vector in seconds. - euler_zyx : ndarray, shape (n, 3) - ZYX Euler angles [yaw, pitch, roll] in radians. - omega_b : ndarray, shape (n, 3) - Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). - """ - - # Time - dt = 1.0 / fs - t = dt * np.arange(n) - - # ZYZ Euler angles - psi, psi_dot = self._psi_sim(fs, n) # precession angle and rate - phi, phi_dot = self._phi_sim(fs, n) - theta = self._beta * np.ones_like(t) # constant cone half-angle - - # Rotation matrix (body-to-inertial) - R = self._rot_matrix_from_euler_zyz(psi, theta, phi) - - # Extract ZYX Euler angles (yaw, pitch, roll) - euler_zyx = self._euler_from_rot_matrix_zyx(R) - - # Body frame angular velocities from ZYZ Euler angle rates - w_b = self._body_rates_from_euler_zyz(theta, phi, psi_dot, phi_dot) - - return t, euler_zyx, w_b - - -# class IMUSimulator: -# """ -# Coning trajectory generator and IMU (gyro) simulator. - -# Simulates an IMU sensor with its z-axis tilted an angle beta (constant) with -# respect to the inertial frame's z-axis. The sensor rotates about its z-axis -# with a constant rate (the spin rate), while also spinning around the inertial -# frame's z-axis with a constant rate (the precession rate). The IMU sensor's -# z-axis will thus trace out a cone shape, with the half-angle defined by beta. - -# Parameters -# ---------- -# alpha_sim : callable -# Roll angle and angular velocity simulator. -# beta_sim : callable -# Pitch angle and angular velocity simulator. -# gamma_sim : callable -# Yaw angle and angular velocity simulator. -# """ - -# def __init__( -# self, -# alpha_sim, -# beta_sim, -# gamma_sim, -# ): -# self._alpha_sim = alpha_sim -# self._beta_sim = beta_sim -# self._gamma_sim = gamma_sim - -# def _angular_velocity_body(self, euler, euler_dot): -# alpha, beta, _ = euler.T -# alpha_dot, beta_dot, gamma_dot = euler_dot.T - -# w_x = alpha_dot - np.sin(beta) * gamma_dot -# w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot -# w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot - -# w_b = np.column_stack([w_x, w_y, w_z]) - -# return w_b - -# def __call__(self, fs: float, n: int): -# """ -# Generate a length-n coning trajectory and corresponding body-frame angular -# velocities as measured by an IMU sensor. - -# Parameters -# ---------- -# fs : float -# Sampling frequency in Hz. -# n : int -# Number of samples to generate. - -# Returns -# ------- -# t : ndarray, shape (n,) -# Time vector in seconds. -# euler_zyx : ndarray, shape (n, 3) -# ZYX Euler angles [yaw, pitch, roll] in radians. -# omega_b : ndarray, shape (n, 3) -# Body angular velocity [p, q, r] in rad/s (IMU gyro measurements). -# """ - -# # Time -# dt = 1.0 / fs -# t = dt * np.arange(n) - -# # Euler angles and Euler rates -# alpha, alpha_dot = self._alpha_sim(fs, n) -# beta, beta_dot = self._beta_sim(fs, n) -# gamma, gamma_dot = self._gamma_sim(fs, n) -# euler = np.column_stack([alpha, beta, gamma]) -# euler_dot = np.column_stack([alpha_dot, beta_dot, gamma_dot]) - -# w_b = self._angular_velocity_body(euler, euler_dot) - -# return t, euler, w_b - - -# class Sine1DSimulator: -# def __init__(self, omega, phase, freq_hz=False, phase_degrees=False): -# self._w = 2.0 * np.pi * omega if freq_hz else omega -# self._phase = np.deg2rad(phase) if phase_degrees else phase - -# def __call__(self, fs, n): -# dt = 1.0 / fs -# t = dt * np.arange(n) - -# y = np.sin(self._w * t + self._phase) -# dydt = self._w * np.cos(self._w * t + self._phase) - -# return y, dydt From 0777fcc831593720dd44006935709ed2ee747c74 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 10:56:53 +0100 Subject: [PATCH 066/129] test DOF --- src/smsfusion/simulate/__init__.py | 4 +-- tests/test_simulate.py | 43 ++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+), 2 deletions(-) create mode 100644 tests/test_simulate.py diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 140d1993..e96f1f74 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import DOF, ConstantDOF, IMUSimulator, LinearRampUp, SineDOF +from ._simulate import ConstantDOF, IMUSimulator, LinearRampUp, SineDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp", "DOF"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp"] diff --git a/tests/test_simulate.py b/tests/test_simulate.py new file mode 100644 index 00000000..e3cdbabf --- /dev/null +++ b/tests/test_simulate.py @@ -0,0 +1,43 @@ +import pytest +import numpy as np + +from smsfusion.simulate._simulate import DOF + + +class Test_DOF: + @pytest.fixture + def some_dof(self): + class SomeDOF(DOF): + + def _y(self, t): + return np.ones_like(t) + + def _dydt(self, t): + return 2 * np.ones_like(t) + + def _d2ydt2(self, t): + return 3 *np.ones_like(t) + + return SomeDOF() + + def test_y(self, some_dof): + t = np.linspace(0, 10, 100) + y = some_dof.y(t) + np.testing.assert_allclose(y, np.ones(100)) + + def test_dydt(self, some_dof): + t = np.linspace(0, 10, 100) + dydt = some_dof.dydt(t) + np.testing.assert_allclose(dydt, 2 * np.ones(100)) + + def test_d2ydt2(self, some_dof): + t = np.linspace(0, 10, 100) + d2ydt2 = some_dof.d2ydt2(t) + np.testing.assert_allclose(d2ydt2, 3 * np.ones(100)) + + def test__call__(self, some_dof): + t = np.linspace(0, 10, 100) + y, dydt, dy2dt2 = some_dof(t) + np.testing.assert_allclose(y, np.ones(100)) + np.testing.assert_allclose(dydt, 2 * np.ones(100)) + np.testing.assert_allclose(dy2dt2, 3 * np.ones(100)) \ No newline at end of file From 9f1da49b9a71e0818ecc9e411b39933a3f3f89be Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:01:31 +0100 Subject: [PATCH 067/129] test constant dof --- tests/test_simulate.py | 30 +++++++++++++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index e3cdbabf..c0eb260f 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,6 +1,7 @@ import pytest import numpy as np +from smsfusion.simulate import ConstantDOF from smsfusion.simulate._simulate import DOF @@ -40,4 +41,31 @@ def test__call__(self, some_dof): y, dydt, dy2dt2 = some_dof(t) np.testing.assert_allclose(y, np.ones(100)) np.testing.assert_allclose(dydt, 2 * np.ones(100)) - np.testing.assert_allclose(dy2dt2, 3 * np.ones(100)) \ No newline at end of file + np.testing.assert_allclose(dy2dt2, 3 * np.ones(100)) + +class Test_ConstantDOF: + @pytest.fixture + def constant_dof(self): + return ConstantDOF(value=5.0) + + def test_y(self, constant_dof): + t = np.linspace(0, 10, 100) + y = constant_dof.y(t) + np.testing.assert_allclose(y, 5.0 * np.ones(100)) + + def test_dydt(self, constant_dof): + t = np.linspace(0, 10, 100) + dydt = constant_dof.dydt(t) + np.testing.assert_allclose(dydt, np.zeros(100)) + + def test_d2ydt2(self, constant_dof): + t = np.linspace(0, 10, 100) + d2ydt2 = constant_dof.d2ydt2(t) + np.testing.assert_allclose(d2ydt2, np.zeros(100)) + + def test__call__(self, constant_dof): + t = np.linspace(0, 10, 100) + y, dydt, dy2dt2 = constant_dof(t) + np.testing.assert_allclose(y, 5.0 * np.ones(100)) + np.testing.assert_allclose(dydt, np.zeros(100)) + np.testing.assert_allclose(dy2dt2, np.zeros(100)) From fb4a891ccce31e8ab15e971e2a33bbf58d3dfe1c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:09:06 +0100 Subject: [PATCH 068/129] testsine dof --- src/smsfusion/simulate/_simulate.py | 10 ++++---- tests/test_simulate.py | 36 ++++++++++++++++++++++++++++- 2 files changed, 40 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index e034ad37..0f0c8e20 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -103,27 +103,27 @@ class SineDOF(DOF): def __init__( self, amp=1.0, - omega=1.0, + freq=1.0, phase=0.0, offset=0.0, freq_hz=False, phase_degrees=False, ): self._amp = amp - self._omega = 2 * np.pi * omega if freq_hz else omega + self._w = 2.0 * np.pi * freq if freq_hz else freq self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset def _y(self, t): - y = self._amp * np.sin(self._omega * t + self._phase) + self._offset + y = self._amp * np.sin(self._w * t + self._phase) + self._offset return y def _dydt(self, t): - dydt = self._amp * self._omega * np.cos(self._omega * t + self._phase) + dydt = self._amp * self._w * np.cos(self._w * t + self._phase) return dydt def _d2ydt2(self, t): - d2ydt2 = -self._amp * self._omega**2 * np.sin(self._omega * t + self._phase) + d2ydt2 = -self._amp * self._w**2 * np.sin(self._w * t + self._phase) return d2ydt2 diff --git a/tests/test_simulate.py b/tests/test_simulate.py index c0eb260f..d72368e1 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,7 +1,7 @@ import pytest import numpy as np -from smsfusion.simulate import ConstantDOF +from smsfusion.simulate import ConstantDOF, SineDOF from smsfusion.simulate._simulate import DOF @@ -69,3 +69,37 @@ def test__call__(self, constant_dof): np.testing.assert_allclose(y, 5.0 * np.ones(100)) np.testing.assert_allclose(dydt, np.zeros(100)) np.testing.assert_allclose(dy2dt2, np.zeros(100)) + + +class Test_SineDOF: + @pytest.fixture + def sine_dof(self): + return SineDOF(2.0, 1.0) + + def test_y(self, sine_dof): + t = np.linspace(0, 2 * np.pi, 100) + y = sine_dof.y(t) + expected_y = 2.0 * np.sin(1.0 * t + 0.0) + np.testing.assert_allclose(y, expected_y) + + def test_dydt(self, sine_dof): + t = np.linspace(0, 2 * np.pi, 100) + dydt = sine_dof.dydt(t) + expected_dydt = 2.0 * 1.0 * np.cos(1.0 * t + 0.0) + np.testing.assert_allclose(dydt, expected_dydt) + + def test_d2ydt2(self, sine_dof): + t = np.linspace(0, 2 * np.pi, 100) + d2ydt2 = sine_dof.d2ydt2(t) + expected_d2ydt2 = -2.0 * (1.0 ** 2) * np.sin(1.0 * t + 0.0) + np.testing.assert_allclose(d2ydt2, expected_d2ydt2) + + def test__call__(self, sine_dof): + t = np.linspace(0, 2 * np.pi, 100) + y, dydt, dy2dt2 = sine_dof(t) + expected_y = 2.0 * np.sin(1.0 * t + 0.0) + expected_dydt = 2.0 * 1.0 * np.cos(1.0 * t + 0.0) + expected_d2ydt2 = -2.0 * (1.0 ** 2) * np.sin(1.0 * t + 0.0) + np.testing.assert_allclose(y, expected_y) + np.testing.assert_allclose(dydt, expected_dydt) + np.testing.assert_allclose(dy2dt2, expected_d2ydt2) From f8a821370370d5acf953a4c8da7f4abce164ae99 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:17:27 +0100 Subject: [PATCH 069/129] test sine --- tests/test_simulate.py | 48 ++++++++++++++++++++++++++++++------------ 1 file changed, 35 insertions(+), 13 deletions(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index d72368e1..a2854971 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,5 +1,5 @@ -import pytest import numpy as np +import pytest from smsfusion.simulate import ConstantDOF, SineDOF from smsfusion.simulate._simulate import DOF @@ -17,8 +17,8 @@ def _dydt(self, t): return 2 * np.ones_like(t) def _d2ydt2(self, t): - return 3 *np.ones_like(t) - + return 3 * np.ones_like(t) + return SomeDOF() def test_y(self, some_dof): @@ -43,6 +43,7 @@ def test__call__(self, some_dof): np.testing.assert_allclose(dydt, 2 * np.ones(100)) np.testing.assert_allclose(dy2dt2, 3 * np.ones(100)) + class Test_ConstantDOF: @pytest.fixture def constant_dof(self): @@ -75,31 +76,52 @@ class Test_SineDOF: @pytest.fixture def sine_dof(self): return SineDOF(2.0, 1.0) + + @pytest.fixture + def t(self): + return np.linspace(0, 10, 100) - def test_y(self, sine_dof): - t = np.linspace(0, 2 * np.pi, 100) + def test_y(self, sine_dof, t): y = sine_dof.y(t) expected_y = 2.0 * np.sin(1.0 * t + 0.0) np.testing.assert_allclose(y, expected_y) - def test_dydt(self, sine_dof): - t = np.linspace(0, 2 * np.pi, 100) + def test_dydt(self, sine_dof, t): dydt = sine_dof.dydt(t) expected_dydt = 2.0 * 1.0 * np.cos(1.0 * t + 0.0) np.testing.assert_allclose(dydt, expected_dydt) - def test_d2ydt2(self, sine_dof): - t = np.linspace(0, 2 * np.pi, 100) + def test_d2ydt2(self, sine_dof, t): d2ydt2 = sine_dof.d2ydt2(t) - expected_d2ydt2 = -2.0 * (1.0 ** 2) * np.sin(1.0 * t + 0.0) + expected_d2ydt2 = -2.0 * (1.0**2) * np.sin(1.0 * t + 0.0) np.testing.assert_allclose(d2ydt2, expected_d2ydt2) - def test__call__(self, sine_dof): - t = np.linspace(0, 2 * np.pi, 100) + def test__call__(self, sine_dof, t): y, dydt, dy2dt2 = sine_dof(t) expected_y = 2.0 * np.sin(1.0 * t + 0.0) expected_dydt = 2.0 * 1.0 * np.cos(1.0 * t + 0.0) - expected_d2ydt2 = -2.0 * (1.0 ** 2) * np.sin(1.0 * t + 0.0) + expected_d2ydt2 = -2.0 * (1.0**2) * np.sin(1.0 * t + 0.0) np.testing.assert_allclose(y, expected_y) np.testing.assert_allclose(dydt, expected_dydt) np.testing.assert_allclose(dy2dt2, expected_d2ydt2) + + def test_amp(self): + sine_dof = SineDOF(amp=3.0) + t = np.linspace(0, 2 * np.pi, 100) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = 3.0 * np.sin(t) + dydt_expect = 3.0 * np.cos(t) + dy2dt2_expect = -3.0 * np.sin(t) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + def test_freq_hz(self, t): + sine_dof = SineDOF(freq=0.5, freq_hz=True) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = np.sin(np.pi * t) + dydt_expect = np.pi * np.cos(np.pi * t) + dy2dt2_expect = -np.pi**2 * np.sin(np.pi * t) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) From df92bd304d406355200b974225dba202dcf28379 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:19:19 +0100 Subject: [PATCH 070/129] more sine tests --- tests/test_simulate.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index a2854971..8b936c16 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -125,3 +125,33 @@ def test_freq_hz(self, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + def test_freq_rads(self, t): + sine_dof = SineDOF(freq=np.pi, freq_hz=False) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = np.sin(np.pi * t) + dydt_expect = np.pi * np.cos(np.pi * t) + dy2dt2_expect = -np.pi**2 * np.sin(np.pi * t) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + def test_phase_degrees(self, t): + sine_dof = SineDOF(phase=90.0, phase_degrees=True) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = np.sin(t + np.pi / 2) + dydt_expect = np.cos(t + np.pi / 2) + dy2dt2_expect = -np.sin(t + np.pi / 2) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + def test_phase_radians(self, t): + sine_dof = SineDOF(phase=np.pi / 2, phase_degrees=False) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = np.sin(t + np.pi / 2) + dydt_expect = np.cos(t + np.pi / 2) + dy2dt2_expect = -np.sin(t + np.pi / 2) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) From 5cb3d939da1321c161b02bae0ffb17e32e3fb90d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:20:11 +0100 Subject: [PATCH 071/129] test sine offset --- tests/test_simulate.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 8b936c16..e6835229 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -155,3 +155,13 @@ def test_phase_radians(self, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + def test_offset(self, t): + sine_dof = SineDOF(offset=2.0) + y, dydt, dy2dt2 = sine_dof(t) + y_expect = 2.0 + np.sin(t) + dydt_expect = np.cos(t) + dy2dt2_expect = -np.sin(t) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(dy2dt2, dy2dt2_expect) From ebf4fb1069699c667e2d156aa93dd3cb71dd7884 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:25:46 +0100 Subject: [PATCH 072/129] test init --- tests/test_simulate.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index e6835229..9a9f505d 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -49,6 +49,11 @@ class Test_ConstantDOF: def constant_dof(self): return ConstantDOF(value=5.0) + def test__init__(self): + constant_dof = ConstantDOF(value=123.0) + assert isinstance(constant_dof, DOF) + assert constant_dof._value == 123.0 + def test_y(self, constant_dof): t = np.linspace(0, 10, 100) y = constant_dof.y(t) @@ -76,11 +81,22 @@ class Test_SineDOF: @pytest.fixture def sine_dof(self): return SineDOF(2.0, 1.0) - + @pytest.fixture def t(self): return np.linspace(0, 10, 100) + def test__init__(self): + sine_dof = SineDOF( + amp=2.0, freq=3.0, freq_hz=True, phase=4.0, phase_degrees=True, offset=5.0 + ) + + assert isinstance(sine_dof, DOF) + assert sine_dof._amp == 2.0 + assert sine_dof._w == pytest.approx(2.0 * np.pi * 3.0) + assert sine_dof._phase == pytest.approx((np.pi / 180.0) * 4.0) + assert sine_dof._offset == 5.0 + def test_y(self, sine_dof, t): y = sine_dof.y(t) expected_y = 2.0 * np.sin(1.0 * t + 0.0) From 2182935d90b6fecfbf74b9012bf4d845b60a8420 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:43:22 +0100 Subject: [PATCH 073/129] test linear ramp --- tests/test_simulate.py | 85 +++++++++++++++++++++++++++++++----------- 1 file changed, 64 insertions(+), 21 deletions(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 9a9f505d..ea765f19 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,10 +1,15 @@ import numpy as np import pytest -from smsfusion.simulate import ConstantDOF, SineDOF +from smsfusion.simulate import ConstantDOF, SineDOF, LinearRampUp from smsfusion.simulate._simulate import DOF +@pytest.fixture +def t(): + return np.linspace(0, 10, 100) + + class Test_DOF: @pytest.fixture def some_dof(self): @@ -21,23 +26,19 @@ def _d2ydt2(self, t): return SomeDOF() - def test_y(self, some_dof): - t = np.linspace(0, 10, 100) + def test_y(self, some_dof, t): y = some_dof.y(t) np.testing.assert_allclose(y, np.ones(100)) - def test_dydt(self, some_dof): - t = np.linspace(0, 10, 100) + def test_dydt(self, some_dof, t): dydt = some_dof.dydt(t) np.testing.assert_allclose(dydt, 2 * np.ones(100)) - def test_d2ydt2(self, some_dof): - t = np.linspace(0, 10, 100) + def test_d2ydt2(self, some_dof, t): d2ydt2 = some_dof.d2ydt2(t) np.testing.assert_allclose(d2ydt2, 3 * np.ones(100)) - def test__call__(self, some_dof): - t = np.linspace(0, 10, 100) + def test__call__(self, some_dof, t): y, dydt, dy2dt2 = some_dof(t) np.testing.assert_allclose(y, np.ones(100)) np.testing.assert_allclose(dydt, 2 * np.ones(100)) @@ -54,23 +55,19 @@ def test__init__(self): assert isinstance(constant_dof, DOF) assert constant_dof._value == 123.0 - def test_y(self, constant_dof): - t = np.linspace(0, 10, 100) + def test_y(self, constant_dof, t): y = constant_dof.y(t) np.testing.assert_allclose(y, 5.0 * np.ones(100)) - def test_dydt(self, constant_dof): - t = np.linspace(0, 10, 100) + def test_dydt(self, constant_dof, t): dydt = constant_dof.dydt(t) np.testing.assert_allclose(dydt, np.zeros(100)) - def test_d2ydt2(self, constant_dof): - t = np.linspace(0, 10, 100) + def test_d2ydt2(self, constant_dof, t): d2ydt2 = constant_dof.d2ydt2(t) np.testing.assert_allclose(d2ydt2, np.zeros(100)) - def test__call__(self, constant_dof): - t = np.linspace(0, 10, 100) + def test__call__(self, constant_dof, t): y, dydt, dy2dt2 = constant_dof(t) np.testing.assert_allclose(y, 5.0 * np.ones(100)) np.testing.assert_allclose(dydt, np.zeros(100)) @@ -82,10 +79,6 @@ class Test_SineDOF: def sine_dof(self): return SineDOF(2.0, 1.0) - @pytest.fixture - def t(self): - return np.linspace(0, 10, 100) - def test__init__(self): sine_dof = SineDOF( amp=2.0, freq=3.0, freq_hz=True, phase=4.0, phase_degrees=True, offset=5.0 @@ -181,3 +174,53 @@ def test_offset(self, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + + +class Test_LinearRampUp: + @pytest.fixture + def dof(self): + return SineDOF(amp=1.0, freq=1.0, freq_hz=True) + + @pytest.fixture + def dof_with_ramp(self, dof): + return LinearRampUp(dof, t_start=1.0, ramp_length=2.0) + + def test__init__(self, dof): + dof_ramp = LinearRampUp(dof, t_start=0.5, ramp_length=3.0) + assert isinstance(dof_ramp, DOF) + assert dof_ramp._dof is dof + assert dof_ramp._t_start == 0.5 + assert dof_ramp._ramp_length == 3.0 + + def test_y(self, dof_with_ramp, dof, t): + y = dof_with_ramp.y(t) + + before_ramp = t < 1.0 + during_ramp = (t >= 1.0) & (t < 3.0) + after_ramp = t >= 3.0 + + y_dof, dydt_dof, d2ydt2_dof = dof(t) + y_expect = np.where( + before_ramp, 0.0, np.where(during_ramp, (t - 1.0) / 2.0 * y_dof, y_dof) + ) + dydt_expect = np.where( + before_ramp, + 0.0, + np.where( + during_ramp, + ((t - 1.0) / 2.0) * dydt_dof, + dydt_dof, + ), + ) + d2ydt2_expect = np.where( + before_ramp, + 0.0, + np.where( + during_ramp, + ((t - 1.0) / 2.0) * d2ydt2_dof, + d2ydt2_dof, + ), + ) + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dof_with_ramp.dydt(t), dydt_expect) + np.testing.assert_allclose(dof_with_ramp.d2ydt2(t), d2ydt2_expect) From 36d51d9614777678bc8a441c94749ccf7f531507 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 11:52:39 +0100 Subject: [PATCH 074/129] remove linearramp --- src/smsfusion/simulate/__init__.py | 4 +- src/smsfusion/simulate/_simulate.py | 84 +++++++++++++++++------------ tests/test_simulate.py | 52 +----------------- 3 files changed, 52 insertions(+), 88 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index e96f1f74..7cd12b31 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOF, IMUSimulator, LinearRampUp, SineDOF +from ._simulate import ConstantDOF, IMUSimulator, SineDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "LinearRampUp"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 0f0c8e20..15389586 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -160,41 +160,55 @@ def _d2ydt2(self, t): return np.zeros_like(t) -class LinearRampUp(DOF): - """ - Linear ramp-up wrapper for DOF signals. - - Parameters - ---------- - dof : _DOF - The DOF signal to wrap with a linear ramp-up. - t_start : float, default 0.0 - The start time of the ramp-up in seconds. Default is 0.0, i.e., the ramp-up - starts immediately. - ramp_length : float, default 1.0 - The duration of the ramp-up in seconds. Default is 1.0 second. - """ - - def __init__(self, dof: DOF, t_start=0.0, ramp_length=1.0): - self._dof = dof - self._t_start = t_start - self._ramp_length = ramp_length - - def _ramp_up(self, t): - ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) - return ramp_up - - def _y(self, t): - ramp_up = self._ramp_up(t) - return ramp_up * self._dof._y(t) - - def _dydt(self, t): - ramp_up = self._ramp_up(t) - return ramp_up * self._dof._dydt(t) - - def _d2ydt2(self, t): - ramp_up = self._ramp_up(t) - return ramp_up * self._dof._d2ydt2(t) +# class LinearRampUp(DOF): +# """ +# Linear ramp-up wrapper for DOF signals. + +# Parameters +# ---------- +# dof : _DOF +# The DOF signal to wrap with a linear ramp-up. +# t_start : float, default 0.0 +# The start time of the ramp-up in seconds. Default is 0.0, i.e., the ramp-up +# starts immediately. +# ramp_length : float, default 1.0 +# The duration of the ramp-up in seconds. Default is 1.0 second. +# """ + +# def __init__(self, dof: DOF, t_start=0.0, ramp_length=1.0): +# self._dof = dof +# self._t_start = t_start +# self._ramp_length = ramp_length +# self._t_end = t_start + ramp_length + +# def _y_ramp(self, t): +# ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) +# return ramp_up + +# def _dydt_ramp(self, t): + +# # dydt_ramp = np.ones_like(t) +# # dydt_ramp = np.where(t < self._t_start, 0.0, dydt_ramp) +# # dydt_ramp = np.where(t > self._t_start + self._ramp_length, 0.0, dydt_ramp) + +# # dydt_ramp = np.where( +# # (t >= self._t_start) & (t <= self._t_start + self._ramp_length), +# # 1.0 / self._ramp_length, +# # 0.0, +# # ) +# # return dydt_ramp + +# def _y(self, t): +# ramp_up = self._ramp_up(t) +# return ramp_up * self._dof._y(t) + +# def _dydt(self, t): +# ramp_up = self._ramp_up(t) +# return ramp_up * self._dof._dydt(t) + +# def _d2ydt2(self, t): +# ramp_up = self._ramp_up(t) +# return ramp_up * self._dof._d2ydt2(t) class IMUSimulator: diff --git a/tests/test_simulate.py b/tests/test_simulate.py index ea765f19..701c87ca 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from smsfusion.simulate import ConstantDOF, SineDOF, LinearRampUp +from smsfusion.simulate import ConstantDOF, SineDOF from smsfusion.simulate._simulate import DOF @@ -174,53 +174,3 @@ def test_offset(self, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) - - -class Test_LinearRampUp: - @pytest.fixture - def dof(self): - return SineDOF(amp=1.0, freq=1.0, freq_hz=True) - - @pytest.fixture - def dof_with_ramp(self, dof): - return LinearRampUp(dof, t_start=1.0, ramp_length=2.0) - - def test__init__(self, dof): - dof_ramp = LinearRampUp(dof, t_start=0.5, ramp_length=3.0) - assert isinstance(dof_ramp, DOF) - assert dof_ramp._dof is dof - assert dof_ramp._t_start == 0.5 - assert dof_ramp._ramp_length == 3.0 - - def test_y(self, dof_with_ramp, dof, t): - y = dof_with_ramp.y(t) - - before_ramp = t < 1.0 - during_ramp = (t >= 1.0) & (t < 3.0) - after_ramp = t >= 3.0 - - y_dof, dydt_dof, d2ydt2_dof = dof(t) - y_expect = np.where( - before_ramp, 0.0, np.where(during_ramp, (t - 1.0) / 2.0 * y_dof, y_dof) - ) - dydt_expect = np.where( - before_ramp, - 0.0, - np.where( - during_ramp, - ((t - 1.0) / 2.0) * dydt_dof, - dydt_dof, - ), - ) - d2ydt2_expect = np.where( - before_ramp, - 0.0, - np.where( - during_ramp, - ((t - 1.0) / 2.0) * d2ydt2_dof, - d2ydt2_dof, - ), - ) - np.testing.assert_allclose(y, y_expect) - np.testing.assert_allclose(dof_with_ramp.dydt(t), dydt_expect) - np.testing.assert_allclose(dof_with_ramp.d2ydt2(t), d2ydt2_expect) From 29fcb2c297188511ef95152ce24fb7b4570a0edf Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:08:44 +0100 Subject: [PATCH 075/129] test imusim init --- tests/test_simulate.py | 52 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 51 insertions(+), 1 deletion(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 701c87ca..8f7248de 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from smsfusion.simulate import ConstantDOF, SineDOF +from smsfusion.simulate import ConstantDOF, SineDOF, IMUSimulator from smsfusion.simulate._simulate import DOF @@ -174,3 +174,53 @@ def test_offset(self, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + +class Test_IMUSimulator: + + def test__init__default(self): + sim = IMUSimulator() + assert isinstance(sim._pos_x, ConstantDOF) + assert isinstance(sim._pos_y, ConstantDOF) + assert isinstance(sim._pos_z, ConstantDOF) + assert isinstance(sim._alpha, ConstantDOF) + assert isinstance(sim._beta, ConstantDOF) + assert isinstance(sim._gamma, ConstantDOF) + assert sim._pos_x._value == 0.0 + assert sim._pos_y._value == 0.0 + assert sim._pos_z._value == 0.0 + assert sim._alpha._value == 0.0 + assert sim._beta._value == 0.0 + assert sim._gamma._value == 0.0 + assert sim._degrees is False + assert sim._nav_frame == "ned" + np.testing.assert_allclose(sim._g_n, np.array([0.0, 0.0, 9.80665])) + + def test__init__(self): + pos_x = SineDOF(1.0, 1.0) + pos_y = ConstantDOF(2.0) + pos_z = SineDOF(0.5, 0.5) + alpha = ConstantDOF(10.0) + beta = SineDOF(5.0, 2.0) + gamma = ConstantDOF(-5.0) + + sim = IMUSimulator( + pos_x=pos_x, + pos_y=pos_y, + pos_z=pos_z, + alpha=alpha, + beta=beta, + gamma=gamma, + degrees=True, + g=9.84, + nav_frame="ENU" + ) + + assert sim._pos_x is pos_x + assert sim._pos_y is pos_y + assert sim._pos_z is pos_z + assert sim._alpha is alpha + assert sim._beta is beta + assert sim._gamma is gamma + assert sim._degrees is True + assert sim._nav_frame == "enu" + np.testing.assert_allclose(sim._g_n, np.array([0.0, 0.0, -9.84])) From 320b33e1303d1795fa7683705af016d4432bbd06 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:11:08 +0100 Subject: [PATCH 076/129] test loat --- tests/test_simulate.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 8f7248de..7ade8e2f 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -195,6 +195,21 @@ def test__init__default(self): assert sim._nav_frame == "ned" np.testing.assert_allclose(sim._g_n, np.array([0.0, 0.0, 9.80665])) + def test__init__float(self): + sim = IMUSimulator(1.0, 2.0, 3.0, 4.0, 5.0, 6.0) + assert isinstance(sim._pos_x, ConstantDOF) + assert isinstance(sim._pos_y, ConstantDOF) + assert isinstance(sim._pos_z, ConstantDOF) + assert isinstance(sim._alpha, ConstantDOF) + assert isinstance(sim._beta, ConstantDOF) + assert isinstance(sim._gamma, ConstantDOF) + assert sim._pos_x._value == 1.0 + assert sim._pos_y._value == 2.0 + assert sim._pos_z._value == 3.0 + assert sim._alpha._value == 4.0 + assert sim._beta._value == 5.0 + assert sim._gamma._value == 6.0 + def test__init__(self): pos_x = SineDOF(1.0, 1.0) pos_y = ConstantDOF(2.0) From 91f3049887b3c9969a41e8d50dd57b23c20422c5 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:11:56 +0100 Subject: [PATCH 077/129] test init float --- tests/test_simulate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 7ade8e2f..aaad8efa 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -210,7 +210,7 @@ def test__init__float(self): assert sim._beta._value == 5.0 assert sim._gamma._value == 6.0 - def test__init__(self): + def test__init__dof(self): pos_x = SineDOF(1.0, 1.0) pos_y = ConstantDOF(2.0) pos_z = SineDOF(0.5, 0.5) From a3685aee39228e492ccacc26cea1de0b5a7bb439 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:26:07 +0100 Subject: [PATCH 078/129] test call --- tests/test_simulate.py | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index aaad8efa..ac1d8713 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -177,6 +177,17 @@ def test_offset(self, t): class Test_IMUSimulator: + @pytest.fixture + def sim(self): + pos_x = SineDOF(1.0, 1.0) + pos_y = SineDOF(2.0, 0.5) + pos_z = SineDOF(3.0, 0.1) + alpha = SineDOF(4.0, 1.0) + beta = SineDOF(5.0, 0.5) + gamma = SineDOF(6.0, 0.1) + sim = IMUSimulator(pos_x, pos_y, pos_z, alpha, beta, gamma, degrees=True) + return sim + def test__init__default(self): sim = IMUSimulator() assert isinstance(sim._pos_x, ConstantDOF) @@ -239,3 +250,25 @@ def test__init__dof(self): assert sim._degrees is True assert sim._nav_frame == "enu" np.testing.assert_allclose(sim._g_n, np.array([0.0, 0.0, -9.84])) + + def test__call__(self, sim): + fs = 10.24 + n = 100 + t, pos, vel, euler, f, w = sim(fs, n) + + np.testing.assert_allclose(t, np.arange(n) / fs) + + assert pos.shape == (n, 3) + np.testing.assert_allclose(pos[:, 0], sim._pos_x.y(t)) + np.testing.assert_allclose(pos[:, 1], sim._pos_y.y(t)) + np.testing.assert_allclose(pos[:, 2], sim._pos_z.y(t)) + + assert vel.shape == (n, 3) + np.testing.assert_allclose(vel[:, 0], sim._pos_x.dydt(t)) + np.testing.assert_allclose(vel[:, 1], sim._pos_y.dydt(t)) + np.testing.assert_allclose(vel[:, 2], sim._pos_z.dydt(t)) + + assert euler.shape == (n, 3) + np.testing.assert_allclose(euler[:, 0], sim._alpha.y(t)) + np.testing.assert_allclose(euler[:, 1], sim._beta.y(t)) + np.testing.assert_allclose(euler[:, 2], sim._gamma.y(t)) \ No newline at end of file From 84b55ef35dcfe0fb2a56cbcff75eb730770bd70e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:42:50 +0100 Subject: [PATCH 079/129] test w --- tests/test_simulate.py | 35 ++++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index ac1d8713..6cfefa2d 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -1,7 +1,8 @@ import numpy as np import pytest -from smsfusion.simulate import ConstantDOF, SineDOF, IMUSimulator +import smsfusion as sf +from smsfusion.simulate import ConstantDOF, IMUSimulator, SineDOF from smsfusion.simulate._simulate import DOF @@ -175,6 +176,7 @@ def test_offset(self, t): np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(dy2dt2, dy2dt2_expect) + class Test_IMUSimulator: @pytest.fixture @@ -238,7 +240,7 @@ def test__init__dof(self): gamma=gamma, degrees=True, g=9.84, - nav_frame="ENU" + nav_frame="ENU", ) assert sim._pos_x is pos_x @@ -258,17 +260,44 @@ def test__call__(self, sim): np.testing.assert_allclose(t, np.arange(n) / fs) + # Position assert pos.shape == (n, 3) np.testing.assert_allclose(pos[:, 0], sim._pos_x.y(t)) np.testing.assert_allclose(pos[:, 1], sim._pos_y.y(t)) np.testing.assert_allclose(pos[:, 2], sim._pos_z.y(t)) + # Velocity assert vel.shape == (n, 3) np.testing.assert_allclose(vel[:, 0], sim._pos_x.dydt(t)) np.testing.assert_allclose(vel[:, 1], sim._pos_y.dydt(t)) np.testing.assert_allclose(vel[:, 2], sim._pos_z.dydt(t)) + # Euler angles assert euler.shape == (n, 3) np.testing.assert_allclose(euler[:, 0], sim._alpha.y(t)) np.testing.assert_allclose(euler[:, 1], sim._beta.y(t)) - np.testing.assert_allclose(euler[:, 2], sim._gamma.y(t)) \ No newline at end of file + np.testing.assert_allclose(euler[:, 2], sim._gamma.y(t)) + + # Specific force + assert f.shape == (n, 3) + acc_x = sim._pos_x.d2ydt2(t) + acc_y = sim._pos_y.d2ydt2(t) + acc_z = sim._pos_z.d2ydt2(t) + acc_expect = np.column_stack((acc_x, acc_y, acc_z)) + for f_i, euler_i, acc_i in zip(f, euler, acc_expect): + R_nb_i = sf._transforms._rot_matrix_from_euler(np.radians(euler_i)) + f_i_expect = R_nb_i.T.dot(acc_i - sim._g_n) + np.testing.assert_allclose(f_i, f_i_expect) + + # Angular rate + assert w.shape == (n, 3) + alpha = np.radians(euler[:, 0]) + beta = np.radians(euler[:, 1]) + alpha_dot = sim._alpha.dydt(t) + beta_dot = sim._beta.dydt(t) + gamma_dot = sim._gamma.dydt(t) + w_x = alpha_dot - np.sin(beta) * gamma_dot + w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot + w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + w_b = np.column_stack([w_x, w_y, w_z]) + np.testing.assert_allclose(w, w_b) From 19e9a9f4225f6e7fb0a13197b1fea90f32c17750 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:44:08 +0100 Subject: [PATCH 080/129] some changes to test --- src/smsfusion/simulate/_simulate.py | 2 +- tests/test_simulate.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 15389586..e18fdf0b 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -184,7 +184,7 @@ def _d2ydt2(self, t): # def _y_ramp(self, t): # ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) # return ramp_up - + # def _dydt_ramp(self, t): # # dydt_ramp = np.ones_like(t) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 6cfefa2d..d8e812cd 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -291,8 +291,7 @@ def test__call__(self, sim): # Angular rate assert w.shape == (n, 3) - alpha = np.radians(euler[:, 0]) - beta = np.radians(euler[:, 1]) + alpha, beta = np.radians(euler[:, 0:2]).T alpha_dot = sim._alpha.dydt(t) beta_dot = sim._beta.dydt(t) gamma_dot = sim._gamma.dydt(t) From 62f94b5b96aaad8b0e9b491d2b39681019c6fc0e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:48:36 +0100 Subject: [PATCH 081/129] test call radians --- tests/test_simulate.py | 99 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 98 insertions(+), 1 deletion(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index d8e812cd..41cbca4a 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -253,7 +253,7 @@ def test__init__dof(self): assert sim._nav_frame == "enu" np.testing.assert_allclose(sim._g_n, np.array([0.0, 0.0, -9.84])) - def test__call__(self, sim): + def test__call__default(self, sim): fs = 10.24 n = 100 t, pos, vel, euler, f, w = sim(fs, n) @@ -300,3 +300,100 @@ def test__call__(self, sim): w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot w_b = np.column_stack([w_x, w_y, w_z]) np.testing.assert_allclose(w, w_b) + + def test__call__degrees(self, sim): + fs = 10.24 + n = 100 + t, pos, vel, euler, f, w = sim(fs, n, degrees=True) + + np.testing.assert_allclose(t, np.arange(n) / fs) + + # Position + assert pos.shape == (n, 3) + np.testing.assert_allclose(pos[:, 0], sim._pos_x.y(t)) + np.testing.assert_allclose(pos[:, 1], sim._pos_y.y(t)) + np.testing.assert_allclose(pos[:, 2], sim._pos_z.y(t)) + + # Velocity + assert vel.shape == (n, 3) + np.testing.assert_allclose(vel[:, 0], sim._pos_x.dydt(t)) + np.testing.assert_allclose(vel[:, 1], sim._pos_y.dydt(t)) + np.testing.assert_allclose(vel[:, 2], sim._pos_z.dydt(t)) + + # Euler angles + assert euler.shape == (n, 3) + np.testing.assert_allclose(euler[:, 0], sim._alpha.y(t)) + np.testing.assert_allclose(euler[:, 1], sim._beta.y(t)) + np.testing.assert_allclose(euler[:, 2], sim._gamma.y(t)) + + # Specific force + assert f.shape == (n, 3) + acc_x = sim._pos_x.d2ydt2(t) + acc_y = sim._pos_y.d2ydt2(t) + acc_z = sim._pos_z.d2ydt2(t) + acc_expect = np.column_stack((acc_x, acc_y, acc_z)) + for f_i, euler_i, acc_i in zip(f, euler, acc_expect): + R_nb_i = sf._transforms._rot_matrix_from_euler(np.radians(euler_i)) + f_i_expect = R_nb_i.T.dot(acc_i - sim._g_n) + np.testing.assert_allclose(f_i, f_i_expect) + + # Angular rate + assert w.shape == (n, 3) + alpha, beta = np.radians(euler[:, 0:2]).T + alpha_dot = sim._alpha.dydt(t) + beta_dot = sim._beta.dydt(t) + gamma_dot = sim._gamma.dydt(t) + w_x = alpha_dot - np.sin(beta) * gamma_dot + w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot + w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + w_b = np.column_stack([w_x, w_y, w_z]) + np.testing.assert_allclose(w, w_b) + + def test__call__radians(self, sim): + fs = 10.24 + n = 100 + t, pos, vel, euler, f, w = sim(fs, n, degrees=False) # radians + + np.testing.assert_allclose(t, np.arange(n) / fs) + + # Position + assert pos.shape == (n, 3) + np.testing.assert_allclose(pos[:, 0], sim._pos_x.y(t)) + np.testing.assert_allclose(pos[:, 1], sim._pos_y.y(t)) + np.testing.assert_allclose(pos[:, 2], sim._pos_z.y(t)) + + # Velocity + assert vel.shape == (n, 3) + np.testing.assert_allclose(vel[:, 0], sim._pos_x.dydt(t)) + np.testing.assert_allclose(vel[:, 1], sim._pos_y.dydt(t)) + np.testing.assert_allclose(vel[:, 2], sim._pos_z.dydt(t)) + + # Euler angles + assert euler.shape == (n, 3) + np.testing.assert_allclose(euler[:, 0], np.radians(sim._alpha.y(t))) + np.testing.assert_allclose(euler[:, 1], np.radians(sim._beta.y(t))) + np.testing.assert_allclose(euler[:, 2], np.radians(sim._gamma.y(t))) + + # Specific force + assert f.shape == (n, 3) + acc_x = sim._pos_x.d2ydt2(t) + acc_y = sim._pos_y.d2ydt2(t) + acc_z = sim._pos_z.d2ydt2(t) + acc_expect = np.column_stack((acc_x, acc_y, acc_z)) + for f_i, euler_i, acc_i in zip(f, euler, acc_expect): + R_nb_i = sf._transforms._rot_matrix_from_euler(euler_i) + f_i_expect = R_nb_i.T.dot(acc_i - sim._g_n) + np.testing.assert_allclose(f_i, f_i_expect) + + # Angular rate + assert w.shape == (n, 3) + alpha, beta = euler[:, 0:2].T + alpha_dot = np.radians(sim._alpha.dydt(t)) + beta_dot = np.radians(sim._beta.dydt(t)) + gamma_dot = np.radians(sim._gamma.dydt(t)) + w_x = alpha_dot - np.sin(beta) * gamma_dot + w_y = np.cos(alpha) * beta_dot + np.sin(alpha) * np.cos(beta) * gamma_dot + w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot + w_b = np.column_stack([w_x, w_y, w_z]) + np.testing.assert_allclose(w, w_b) + From f09111b01dff35e018f3136bda6a0dc649a920f9 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:57:19 +0100 Subject: [PATCH 082/129] typing --- src/smsfusion/simulate/_simulate.py | 84 +++++++++++++++++++---------- tests/test_simulate.py | 1 - 2 files changed, 56 insertions(+), 29 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index e18fdf0b..d8c1239c 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -1,6 +1,7 @@ from abc import ABC, abstractmethod import numpy as np +from numpy.typing import ArrayLike, NDArray from smsfusion._transforms import _rot_matrix_from_euler @@ -11,42 +12,62 @@ class DOF(ABC): """ @abstractmethod - def _y(self, t): + def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: raise NotImplementedError("Not implemented.") @abstractmethod - def _dydt(self, t): + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: raise NotImplementedError("Not implemented.") @abstractmethod - def _d2ydt2(self, t): + def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: raise NotImplementedError("Not implemented.") - def y(self, t): + def y(self, t: ArrayLike) -> NDArray[np.float64]: """ Generates y(t) signal. + + Parameters + ---------- + t : array_like, shape (n,) + Time vector in seconds. """ + t = np.asarray_chkfinite(t) return self._y(t) - def dydt(self, t): + def dydt(self, t: ArrayLike) -> NDArray[np.float64]: """ Generates dy(t)/dt signal. + + Parameters + ---------- + t : array_like, shape (n,) + Time vector in seconds. """ + t = np.asarray_chkfinite(t) return self._dydt(t) def d2ydt2(self, t): """ Generates d2y(t)/dt2 signal. + + Parameters + ---------- + t : array_like, shape (n,) + Time vector in seconds. """ + t = np.asarray_chkfinite(t) return self._d2ydt2(t) - def __call__(self, t): + def __call__( + self, t: ArrayLike + ) -> tuple[NDArray[np.float64], NDArray[np.float64], NDArray[np.float64]]: """ Generates y(t), dy(t)/dt, and d2y(t)/dt2 signals. Parameters ---------- - t : ndarray, shape (n,) + t : array_like, shape (n,) Time vector in seconds. Returns @@ -102,27 +123,27 @@ class SineDOF(DOF): def __init__( self, - amp=1.0, - freq=1.0, - phase=0.0, - offset=0.0, - freq_hz=False, - phase_degrees=False, - ): + amp: float = 1.0, + freq: float = 1.0, + phase: float = 0.0, + offset: float = 0.0, + freq_hz: bool = False, + phase_degrees: bool = False, + ) -> None: self._amp = amp self._w = 2.0 * np.pi * freq if freq_hz else freq self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset - def _y(self, t): + def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: y = self._amp * np.sin(self._w * t + self._phase) + self._offset return y - def _dydt(self, t): + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: dydt = self._amp * self._w * np.cos(self._w * t + self._phase) return dydt - def _d2ydt2(self, t): + def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: d2ydt2 = -self._amp * self._w**2 * np.sin(self._w * t + self._phase) return d2ydt2 @@ -147,16 +168,16 @@ class ConstantDOF(DOF): Constant value of the signal. Default is 0.0. """ - def __init__(self, value=0.0): + def __init__(self, value: float = 0.0) -> None: self._value = value - def _y(self, t): + def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return np.full_like(t, self._value) - def _dydt(self, t): + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return np.zeros_like(t) - def _d2ydt2(self, t): + def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return np.zeros_like(t) @@ -247,10 +268,10 @@ def __init__( alpha: float | DOF = 0.0, beta: float | DOF = 0.0, gamma: float | DOF = 0.0, - degrees=False, - g=9.80665, - nav_frame="NED", - ): + degrees: bool = False, + g: float = 9.80665, + nav_frame: str = "NED", + ) -> None: self._pos_x = pos_x if isinstance(pos_x, DOF) else ConstantDOF(pos_x) self._pos_y = pos_y if isinstance(pos_y, DOF) else ConstantDOF(pos_y) self._pos_z = pos_z if isinstance(pos_z, DOF) else ConstantDOF(pos_z) @@ -267,7 +288,12 @@ def __init__( else: raise ValueError("Invalid navigation frame. Must be 'NED' or 'ENU'.") - def _specific_force_body(self, pos, acc, euler): + def _specific_force_body( + self, + pos: NDArray[np.float64], + acc: NDArray[np.float64], + euler: NDArray[np.float64], + ) -> NDArray[np.float64]: """ Specific force in the body frame. @@ -291,7 +317,9 @@ def _specific_force_body(self, pos, acc, euler): return f_b - def _angular_velocity_body(self, euler, euler_dot): + def _angular_velocity_body( + self, euler: NDArray[np.float64], euler_dot: NDArray[np.float64] + ) -> NDArray[np.float64]: """ Angular velocity in the body frame. @@ -314,7 +342,7 @@ def _angular_velocity_body(self, euler, euler_dot): return w_b - def __call__(self, fs: float, n: int, degrees=None): + def __call__(self, fs: float, n: int, degrees: bool | None = None): """ Generate a length-n gyroscope signal and corresponding Euler angles. diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 41cbca4a..4328de53 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -396,4 +396,3 @@ def test__call__radians(self, sim): w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot w_b = np.column_stack([w_x, w_y, w_z]) np.testing.assert_allclose(w, w_b) - From 2ff64225a9919610f33f30e55f301fcbacc78d76 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 12:59:37 +0100 Subject: [PATCH 083/129] docstring --- src/smsfusion/simulate/_simulate.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index d8c1239c..e101b85e 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -355,6 +355,7 @@ def __call__(self, fs: float, n: int, degrees: bool | None = None): degrees : bool, optional Whether to return Euler angles and angular velocities in degrees and degrees per second (True) or radians and radians per second (False). + Defaults to the value specified at initialization. Returns ------- From edd1813b8e769379e4dafd9379125eb1c9bbb67a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:09:58 +0100 Subject: [PATCH 084/129] reorder sine params --- src/smsfusion/simulate/_simulate.py | 42 +++++++++++++++++++++++------ 1 file changed, 34 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index e101b85e..68a4170e 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -107,28 +107,28 @@ class SineDOF(DOF): ---------- amp : float, default 1.0 Amplitude of the sine wave. Default is 1.0. - omega : float, default 1.0 - Angular frequency of the sine wave in rad/s. Default is 1.0 rad/s. - phase : float, default 0.0 - Phase offset of the sine wave. Default is 0.0. - offset : float, default 0.0 - Offset of the sine wave. Default is 0.0. + freq : float, default 1.0 + Frequency of the sine wave in rad/s. Default is 1.0 rad/s. freq_hz : bool, optional If True, interpret `omega` as frequency in Hz. If False, interpret as angular frequency in radians per second. Default is False. + phase : float, default 0.0 + Phase offset of the sine wave. Default is 0.0. phase_degrees : bool, optional If True, interpret `phase` in degrees. If False, interpret in radians. Default is False. + offset : float, default 0.0 + Offset of the sine wave. Default is 0.0. """ def __init__( self, amp: float = 1.0, freq: float = 1.0, - phase: float = 0.0, - offset: float = 0.0, freq_hz: bool = False, + phase: float = 0.0, phase_degrees: bool = False, + offset: float = 0.0, ) -> None: self._amp = amp self._w = 2.0 * np.pi * freq if freq_hz else freq @@ -181,6 +181,32 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return np.zeros_like(t) +class BeatDOF(DOF): + """ + 1D beat signal DOF generator. + + Defined as: + + y = sin(f_beat / 2.0 * t) * cos(f_main * t + phase) + + where, + + - A : Amplitude of the sine waves. + - w_main : Angular frequency of the main sine wave. + - w_beat : Angular frequency of the beat sine wave. + + Parameters + ---------- + amp : float, default 1.0 + Amplitude of the sine waves. Default is 1.0. + freq1 : float, default 1.0 + Frequency of the first sine wave in Hz. Default is 1.0 Hz. + freq2 : float, default 1.1 + Frequency of the second sine wave in Hz. Default is 1.1 Hz. + """ + pass + + # class LinearRampUp(DOF): # """ # Linear ramp-up wrapper for DOF signals. From f276bf85b8ce53b4d4d5f2f28a7e569318717bf4 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:16:27 +0100 Subject: [PATCH 085/129] dosctring --- src/smsfusion/simulate/_simulate.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 68a4170e..3fb9d6e9 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -160,7 +160,7 @@ class ConstantDOF(DOF): where, - - C : Constant value of the signal. + - C : Constant value of the signal. Parameters ---------- @@ -187,23 +187,25 @@ class BeatDOF(DOF): Defined as: - y = sin(f_beat / 2.0 * t) * cos(f_main * t + phase) + y = A * sin(f_beat / 2.0 * t) * cos(f_main * t + phi) where, - - A : Amplitude of the sine waves. - - w_main : Angular frequency of the main sine wave. - - w_beat : Angular frequency of the beat sine wave. + - A : Amplitude of the sine waves. + - w_main : Angular frequency of the main sine wave. + - w_beat : Angular frequency of the beat sine wave. + - phi : Phase offset of the main sine wave. Parameters ---------- - amp : float, default 1.0 - Amplitude of the sine waves. Default is 1.0. - freq1 : float, default 1.0 - Frequency of the first sine wave in Hz. Default is 1.0 Hz. - freq2 : float, default 1.1 - Frequency of the second sine wave in Hz. Default is 1.1 Hz. + f_main : float + The main frequency of the sinusoidal signal, y(t). + f_beat : float + The beating frequency, which controls the variation in amplitude. + freq_hz : bool, default True. + Whether the frequencies, ``f_main`` and ``f_beat``, are in Hz or rad/s (default). """ + pass From 5bf32beffe12fb44cd82e192384c1a98d9bbbd42 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:20:33 +0100 Subject: [PATCH 086/129] prep beat dof --- src/smsfusion/simulate/_simulate.py | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 3fb9d6e9..0a134eb6 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -101,7 +101,7 @@ class SineDOF(DOF): - A : Amplitude of the sine wave. - w : Angular frequency of the sine wave. - phi: Phase offset of the sine wave. - - B : Offset of the sine wave. + - B : Constant offset of the sine wave. Parameters ---------- @@ -187,7 +187,7 @@ class BeatDOF(DOF): Defined as: - y = A * sin(f_beat / 2.0 * t) * cos(f_main * t + phi) + y = A * sin(f_beat / 2.0 * t) * cos(f_main * t + phi) + B where, @@ -195,6 +195,7 @@ class BeatDOF(DOF): - w_main : Angular frequency of the main sine wave. - w_beat : Angular frequency of the beat sine wave. - phi : Phase offset of the main sine wave. + - B : Constant offset of the beat signal. Parameters ---------- @@ -206,7 +207,21 @@ class BeatDOF(DOF): Whether the frequencies, ``f_main`` and ``f_beat``, are in Hz or rad/s (default). """ - pass + def __init__( + self, + amp: float = 1.0, + freq_main: float = 1.0, + freq_beat: float = 0.1, + freq_hz: bool = False, + phase: float = 0.0, + phase_degrees: bool = False, + offset: float = 0.0, + ) -> None: + self._amp = amp + self._w_main = 2.0 * np.pi * freq_main if freq_hz else freq_main + self._w_beat = 2.0 * np.pi * freq_beat if freq_hz else freq_beat + self._phase = np.deg2rad(phase) if phase_degrees else phase + self._offset = offset # class LinearRampUp(DOF): From aa8627cd97646d39f89d22ccf8ba2ededc12fc15 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:47:23 +0100 Subject: [PATCH 087/129] implement beat methods --- src/smsfusion/simulate/__init__.py | 4 ++-- src/smsfusion/simulate/_simulate.py | 27 +++++++++++++++++++++++++++ tests/test_simulate.py | 26 +++++++++++++++++++++++++- 3 files changed, 54 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 7cd12b31..d4d8edaa 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOF, IMUSimulator, SineDOF +from ._simulate import ConstantDOF, IMUSimulator, SineDOF, BeatDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "BeatDOF"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 0a134eb6..52f017d3 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -223,6 +223,33 @@ def __init__( self._phase = np.deg2rad(phase) if phase_degrees else phase self._offset = offset + def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + main = np.cos(self._w_main * t + self._phase) + beat = np.sin(self._w_beat / 2.0 * t) + y = beat * main + return y # type: ignore[no-any-return] + + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + main = np.cos(self._f_main * t + self._phase) + beat = np.sin(self._f_beat / 2.0 * t) + dmain = -self._f_main * np.sin(self._f_main * t + self._phase) + dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) + + dydt = dbeat * main + beat * dmain + return dydt # type: ignore[no-any-return] + + def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + main = np.cos(self._f_main * t + self._phase) + beat = np.sin(self._f_beat / 2.0 * t) + dmain = -self._f_main * np.sin(self._f_main * t + self._phase) + dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) + d2main = -((self._f_main) ** 2) * np.cos(self._f_main * t + self._phase) + d2beat = -((self._f_beat / 2.0) ** 2) * np.sin(self._f_beat / 2.0 * t) + + d2ydt2 = dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain + + return d2ydt2 # type: ignore[no-any-return] + # class LinearRampUp(DOF): # """ diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 4328de53..d297ea5e 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -2,7 +2,7 @@ import pytest import smsfusion as sf -from smsfusion.simulate import ConstantDOF, IMUSimulator, SineDOF +from smsfusion.simulate import ConstantDOF, IMUSimulator, SineDOF, BeatDOF from smsfusion.simulate._simulate import DOF @@ -396,3 +396,27 @@ def test__call__radians(self, sim): w_z = -np.sin(alpha) * beta_dot + np.cos(alpha) * np.cos(beta) * gamma_dot w_b = np.column_stack([w_x, w_y, w_z]) np.testing.assert_allclose(w, w_b) + + +class Test_BeatDOF: + @pytest.fixture + def beat_dof(self): + return BeatDOF(amp=2.0, freq_main=1.0, freq_beat=0.1) + + def test__init__(self): + beat_dof = BeatDOF( + amp=3.0, + freq_main=2.0, + freq_beat=0.2, + freq_hz=True, + phase=4.0, + phase_degrees=True, + offset=5.0, + ) + + assert isinstance(beat_dof, DOF) + assert beat_dof._amp == 3.0 + assert beat_dof._w_main == pytest.approx(2.0 * np.pi * 2.0) + assert beat_dof._w_beat == pytest.approx(2.0 * np.pi * 0.2) + assert beat_dof._phase == pytest.approx((np.pi / 180.0) * 4.0) + assert beat_dof._offset == 5.0 From 4a4db1bd8da08df6204d1cf5d55a930028433369 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:47:46 +0100 Subject: [PATCH 088/129] style --- src/smsfusion/simulate/_simulate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 52f017d3..64da9c01 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -228,7 +228,7 @@ def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: beat = np.sin(self._w_beat / 2.0 * t) y = beat * main return y # type: ignore[no-any-return] - + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: main = np.cos(self._f_main * t + self._phase) beat = np.sin(self._f_beat / 2.0 * t) From 408ca41b859d382bb0838ede4d02e7cbdf32e81d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:48:01 +0100 Subject: [PATCH 089/129] style --- src/smsfusion/simulate/__init__.py | 2 +- tests/test_simulate.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index d4d8edaa..1f2ebff9 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import ConstantDOF, IMUSimulator, SineDOF, BeatDOF +from ._simulate import BeatDOF, ConstantDOF, IMUSimulator, SineDOF __all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "BeatDOF"] diff --git a/tests/test_simulate.py b/tests/test_simulate.py index d297ea5e..4e865ddf 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -2,7 +2,7 @@ import pytest import smsfusion as sf -from smsfusion.simulate import ConstantDOF, IMUSimulator, SineDOF, BeatDOF +from smsfusion.simulate import BeatDOF, ConstantDOF, IMUSimulator, SineDOF from smsfusion.simulate._simulate import DOF From fa8e3bb28d9f30f7ef5b43b40109ec66e18d2de2 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:51:35 +0100 Subject: [PATCH 090/129] testiit default --- tests/test_simulate.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 4e865ddf..526d0409 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -420,3 +420,13 @@ def test__init__(self): assert beat_dof._w_beat == pytest.approx(2.0 * np.pi * 0.2) assert beat_dof._phase == pytest.approx((np.pi / 180.0) * 4.0) assert beat_dof._offset == 5.0 + + def test__init__default(self): + beat_dof = BeatDOF() + + assert isinstance(beat_dof, DOF) + assert beat_dof._amp == 1.0 + assert beat_dof._w_main == pytest.approx(1.0) + assert beat_dof._w_beat == pytest.approx(0.1) + assert beat_dof._phase == pytest.approx(0.0) + assert beat_dof._offset == 0.0 From d37360d11eb1349440d1385ac3e8de6dbeca6f38 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 13:58:21 +0100 Subject: [PATCH 091/129] offset fix + test --- src/smsfusion/simulate/_simulate.py | 2 +- tests/test_simulate.py | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 64da9c01..2f5be2a3 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -226,7 +226,7 @@ def __init__( def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: main = np.cos(self._w_main * t + self._phase) beat = np.sin(self._w_beat / 2.0 * t) - y = beat * main + y = beat * main + self._offset return y # type: ignore[no-any-return] def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 526d0409..48b47c8e 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -401,7 +401,8 @@ def test__call__radians(self, sim): class Test_BeatDOF: @pytest.fixture def beat_dof(self): - return BeatDOF(amp=2.0, freq_main=1.0, freq_beat=0.1) + dof = BeatDOF(amp=1.0, freq_main=1.0, freq_beat=0.1, freq_hz=False, offset=1.0) + return dof def test__init__(self): beat_dof = BeatDOF( @@ -430,3 +431,17 @@ def test__init__default(self): assert beat_dof._w_beat == pytest.approx(0.1) assert beat_dof._phase == pytest.approx(0.0) assert beat_dof._offset == 0.0 + + def test_y(self, beat_dof, t): + y = beat_dof.y(t) + + amp = beat_dof._amp + w_main = beat_dof._w_main + w_beat = beat_dof._w_beat + phase = beat_dof._phase + offset = beat_dof._offset + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + y_expect = amp * beat * main + offset + + np.testing.assert_allclose(y, y_expect) \ No newline at end of file From 141ed40f9908ab0021604120302d1500ffc7269e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:03:11 +0100 Subject: [PATCH 092/129] test dydt --- src/smsfusion/simulate/_simulate.py | 12 ++++++------ tests/test_simulate.py | 19 +++++++++++++++++-- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 2f5be2a3..5418df64 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -226,16 +226,16 @@ def __init__( def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: main = np.cos(self._w_main * t + self._phase) beat = np.sin(self._w_beat / 2.0 * t) - y = beat * main + self._offset + y = self._amp * beat * main + self._offset return y # type: ignore[no-any-return] def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: - main = np.cos(self._f_main * t + self._phase) - beat = np.sin(self._f_beat / 2.0 * t) - dmain = -self._f_main * np.sin(self._f_main * t + self._phase) - dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) + main = np.cos(self._w_main * t + self._phase) + beat = np.sin(self._w_beat / 2.0 * t) + dmain = -self._w_main * np.sin(self._w_main * t + self._phase) + dbeat = self._w_beat / 2.0 * np.cos(self._w_beat / 2.0 * t) - dydt = dbeat * main + beat * dmain + dydt = self._amp * (dbeat * main + beat * dmain) return dydt # type: ignore[no-any-return] def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 48b47c8e..79e76b38 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -401,7 +401,7 @@ def test__call__radians(self, sim): class Test_BeatDOF: @pytest.fixture def beat_dof(self): - dof = BeatDOF(amp=1.0, freq_main=1.0, freq_beat=0.1, freq_hz=False, offset=1.0) + dof = BeatDOF(amp=2.0, freq_main=1.0, freq_beat=0.1, freq_hz=False, offset=1.0) return dof def test__init__(self): @@ -444,4 +444,19 @@ def test_y(self, beat_dof, t): beat = np.sin(w_beat / 2.0 * t) y_expect = amp * beat * main + offset - np.testing.assert_allclose(y, y_expect) \ No newline at end of file + np.testing.assert_allclose(y, y_expect) + + def test_dydt(self, beat_dof, t): + dydt = beat_dof.dydt(t) + + amp = beat_dof._amp + w_main = beat_dof._w_main + w_beat = beat_dof._w_beat + phase = beat_dof._phase + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + dmain = -w_main * np.sin(w_main * t + phase) + dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) + dydt_expect = amp * (dbeat * main + beat * dmain) + + np.testing.assert_allclose(dydt, dydt_expect) \ No newline at end of file From 78f0fe766629291d4945bdc473c5860788b9da2f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:06:36 +0100 Subject: [PATCH 093/129] test call --- src/smsfusion/simulate/_simulate.py | 13 ++++---- tests/test_simulate.py | 48 ++++++++++++++++++++++++++--- 2 files changed, 51 insertions(+), 10 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 5418df64..772d43ad 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -239,14 +239,15 @@ def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return dydt # type: ignore[no-any-return] def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: - main = np.cos(self._f_main * t + self._phase) - beat = np.sin(self._f_beat / 2.0 * t) - dmain = -self._f_main * np.sin(self._f_main * t + self._phase) - dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) - d2main = -((self._f_main) ** 2) * np.cos(self._f_main * t + self._phase) - d2beat = -((self._f_beat / 2.0) ** 2) * np.sin(self._f_beat / 2.0 * t) + main = np.cos(self._w_main * t + self._phase) + beat = np.sin(self._w_beat / 2.0 * t) + dmain = -self._w_main * np.sin(self._w_main * t + self._phase) + dbeat = self._w_beat / 2.0 * np.cos(self._w_beat / 2.0 * t) + d2main = -((self._w_main) ** 2) * np.cos(self._w_main * t + self._phase) + d2beat = -((self._w_beat / 2.0) ** 2) * np.sin(self._w_beat / 2.0 * t) d2ydt2 = dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain + d2ydt2 *= self._amp return d2ydt2 # type: ignore[no-any-return] diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 79e76b38..31c69509 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -443,7 +443,7 @@ def test_y(self, beat_dof, t): main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) y_expect = amp * beat * main + offset - + np.testing.assert_allclose(y, y_expect) def test_dydt(self, beat_dof, t): @@ -457,6 +457,46 @@ def test_dydt(self, beat_dof, t): beat = np.sin(w_beat / 2.0 * t) dmain = -w_main * np.sin(w_main * t + phase) dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) - dydt_expect = amp * (dbeat * main + beat * dmain) - - np.testing.assert_allclose(dydt, dydt_expect) \ No newline at end of file + dydt_expect = amp * (dbeat * main + beat * dmain) + + np.testing.assert_allclose(dydt, dydt_expect) + + def test_d2ydt2(self, beat_dof, t): + d2ydt2 = beat_dof.d2ydt2(t) + + amp = beat_dof._amp + w_main = beat_dof._w_main + w_beat = beat_dof._w_beat + phase = beat_dof._phase + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + dmain = -w_main * np.sin(w_main * t + phase) + dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) + d2main = -(w_main**2) * np.cos(w_main * t + phase) + d2beat = -(w_beat**2 / 4.0) * np.sin(w_beat / 2.0 * t) + d2ydt2_expect = amp * (d2beat * main + 2.0 * dbeat * dmain + beat * d2main) + + np.testing.assert_allclose(d2ydt2, d2ydt2_expect) + + def test__call__(self, beat_dof, t): + y, dydt, d2ydt2 = beat_dof(t) + + amp = beat_dof._amp + w_main = beat_dof._w_main + w_beat = beat_dof._w_beat + phase = beat_dof._phase + offset = beat_dof._offset + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + dmain = -w_main * np.sin(w_main * t + phase) + dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) + d2main = -(w_main**2) * np.cos(w_main * t + phase) + d2beat = -(w_beat**2 / 4.0) * np.sin(w_beat / 2.0 * t) + + y_expect = amp * beat * main + offset + dydt_expect = amp * (dbeat * main + beat * dmain) + d2ydt2_expect = amp * (d2beat * main + 2.0 * dbeat * dmain + beat * d2main) + + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(d2ydt2, d2ydt2_expect) From f49cb76d841754bb4c28c24da6a2e1a46bba4329 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:09:40 +0100 Subject: [PATCH 094/129] refactor --- src/smsfusion/simulate/_simulate.py | 49 +++++++++++++++++++---------- tests/test_simulate.py | 1 + 2 files changed, 33 insertions(+), 17 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 772d43ad..e56575eb 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -224,30 +224,45 @@ def __init__( self._offset = offset def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: - main = np.cos(self._w_main * t + self._phase) - beat = np.sin(self._w_beat / 2.0 * t) - y = self._amp * beat * main + self._offset + amp = self._amp + w_main = self._w_main + w_beat = self._w_beat + phase = self._phase + offset = self._offset + + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + y = amp * beat * main + offset return y # type: ignore[no-any-return] def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: - main = np.cos(self._w_main * t + self._phase) - beat = np.sin(self._w_beat / 2.0 * t) - dmain = -self._w_main * np.sin(self._w_main * t + self._phase) - dbeat = self._w_beat / 2.0 * np.cos(self._w_beat / 2.0 * t) + amp = self._amp + w_main = self._w_main + w_beat = self._w_beat + phase = self._phase - dydt = self._amp * (dbeat * main + beat * dmain) + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + dmain = -w_main * np.sin(w_main * t + phase) + dbeat = w_beat / 2.0 * np.cos(w_beat / 2.0 * t) + + dydt = amp * (dbeat * main + beat * dmain) return dydt # type: ignore[no-any-return] def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: - main = np.cos(self._w_main * t + self._phase) - beat = np.sin(self._w_beat / 2.0 * t) - dmain = -self._w_main * np.sin(self._w_main * t + self._phase) - dbeat = self._w_beat / 2.0 * np.cos(self._w_beat / 2.0 * t) - d2main = -((self._w_main) ** 2) * np.cos(self._w_main * t + self._phase) - d2beat = -((self._w_beat / 2.0) ** 2) * np.sin(self._w_beat / 2.0 * t) - - d2ydt2 = dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain - d2ydt2 *= self._amp + + amp = self._amp + w_main = self._w_main + w_beat = self._w_beat + phase = self._phase + + main = np.cos(w_main * t + phase) + beat = np.sin(w_beat / 2.0 * t) + dmain = -w_main * np.sin(w_main * t + phase) + dbeat = w_beat / 2.0 * np.cos(w_beat / 2.0 * t) + d2main = -(w_main ** 2) * np.cos(w_main * t + phase) + d2beat = -((w_beat / 2.0) ** 2) * np.sin(w_beat / 2.0 * t) + d2ydt2 = amp * (dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain) return d2ydt2 # type: ignore[no-any-return] diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 31c69509..0a71958c 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -486,6 +486,7 @@ def test__call__(self, beat_dof, t): w_beat = beat_dof._w_beat phase = beat_dof._phase offset = beat_dof._offset + main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) dmain = -w_main * np.sin(w_main * t + phase) From 1ce2c1d5ec104ad29a599abad988ed27ea69e461 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:10:44 +0100 Subject: [PATCH 095/129] small fix --- tests/test_simulate.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 0a71958c..5931ef4d 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -440,8 +440,10 @@ def test_y(self, beat_dof, t): w_beat = beat_dof._w_beat phase = beat_dof._phase offset = beat_dof._offset + main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) + y_expect = amp * beat * main + offset np.testing.assert_allclose(y, y_expect) @@ -453,10 +455,12 @@ def test_dydt(self, beat_dof, t): w_main = beat_dof._w_main w_beat = beat_dof._w_beat phase = beat_dof._phase + main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) dmain = -w_main * np.sin(w_main * t + phase) dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) + dydt_expect = amp * (dbeat * main + beat * dmain) np.testing.assert_allclose(dydt, dydt_expect) @@ -468,12 +472,14 @@ def test_d2ydt2(self, beat_dof, t): w_main = beat_dof._w_main w_beat = beat_dof._w_beat phase = beat_dof._phase + main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) dmain = -w_main * np.sin(w_main * t + phase) dbeat = (w_beat / 2.0) * np.cos(w_beat / 2.0 * t) d2main = -(w_main**2) * np.cos(w_main * t + phase) d2beat = -(w_beat**2 / 4.0) * np.sin(w_beat / 2.0 * t) + d2ydt2_expect = amp * (d2beat * main + 2.0 * dbeat * dmain + beat * d2main) np.testing.assert_allclose(d2ydt2, d2ydt2_expect) From a568c6d2ba5506378f6623ae5e1dcc09f8b6d51f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:11:05 +0100 Subject: [PATCH 096/129] style --- src/smsfusion/simulate/_simulate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index e56575eb..d9cd7bd6 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -260,7 +260,7 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: beat = np.sin(w_beat / 2.0 * t) dmain = -w_main * np.sin(w_main * t + phase) dbeat = w_beat / 2.0 * np.cos(w_beat / 2.0 * t) - d2main = -(w_main ** 2) * np.cos(w_main * t + phase) + d2main = -(w_main**2) * np.cos(w_main * t + phase) d2beat = -((w_beat / 2.0) ** 2) * np.sin(w_beat / 2.0 * t) d2ydt2 = amp * (dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain) From 7013fccf336e8e74136a97267888ead887ae8339 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:24:23 +0100 Subject: [PATCH 097/129] update benchmark pure attitude beat to use new framework --- src/smsfusion/benchmark/_benchmark.py | 18 +++++++++++------- tests/test_benchmark.py | 4 +++- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 1c66446d..d55e0e5d 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -7,6 +7,7 @@ from smsfusion._ins import gravity from smsfusion._transforms import _inv_angular_matrix_from_euler, _rot_matrix_from_euler +from smsfusion.simulate import BeatDOF, IMUSimulator class _Signal(abc.ABC): @@ -396,17 +397,20 @@ def benchmark_pure_attitude_beat_202311A( within the frame. """ duration = 1800.0 # 30 minutes - amplitude = np.radians(np.array([0.0, 0.0, 0.0, 5.0, 5.0, 5.0])) - mean = np.radians(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) - phase = np.radians(np.array([0.0, 0.0, 0.0, 0.0, 45.0, 90.0])) f_main = 0.1 f_beat = 0.01 - t, _, _, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, BeatSignal(f_main, f_beat), fs - ) - return t, euler, acc, gyro + amp = np.radians(5.0) + alpha = BeatDOF(amp, f_main, f_beat, freq_hz=True, phase=0.0) + beta = BeatDOF(amp, f_main, f_beat, freq_hz=True, phase=45.0, phase_degrees=True) + gamma = BeatDOF(amp, f_main, f_beat, freq_hz=True, phase=90.0, phase_degrees=True) + sim = IMUSimulator(alpha=alpha, beta=beta, gamma=gamma) + + n = int(duration * fs) + t, _, _, euler, f, w = sim(fs, n, degrees=False) + + return t, euler, f, w def benchmark_pure_attitude_chirp_202311A( diff --git a/tests/test_benchmark.py b/tests/test_benchmark.py index d8fc1566..a5c82214 100644 --- a/tests/test_benchmark.py +++ b/tests/test_benchmark.py @@ -312,7 +312,9 @@ def test_benchmark_pure_attitude_beat_202311A(): assert acc.shape == (len(t), 3) assert gyro.shape == (len(t), 3) - np.testing.assert_array_equal(euler[:, 0], np.radians(5.0) * signature_signal) + np.testing.assert_allclose( + euler[:, 0], np.radians(5.0) * signature_signal, atol=1e-12 + ) def test_benchmark_pure_attitude_chirp_202311A(): From 860abcf4511d9561cb02ff8cbbfebcdea1e69b52 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:29:46 +0100 Subject: [PATCH 098/129] update bench pure att beat --- src/smsfusion/benchmark/_benchmark.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index d55e0e5d..08d2b24f 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -552,17 +552,26 @@ def benchmark_full_pva_beat_202311A( within the frame. """ duration = 1800.0 # 30 minutes - amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) f_main = 0.1 f_beat = 0.01 - t, pos, vel, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, BeatSignal(f_main, f_beat), fs + amp_p = 0.5 + amp_r = np.radians(5.0) + pos_x = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=0.0) + pos_y = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=30.0, phase_degrees=True) + pos_z = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=60.0, phase_degrees=True) + alpha = BeatDOF(amp_r, f_main, f_beat, freq_hz=True, phase=90.0) + beta = BeatDOF(amp_r, f_main, f_beat, freq_hz=True, phase=120.0, phase_degrees=True) + gamma = BeatDOF( + amp_r, f_main, f_beat, freq_hz=True, phase=150.0, phase_degrees=True ) - return t, pos, vel, euler, acc, gyro + sim = IMUSimulator(pos_x, pos_y, pos_z, alpha, beta, gamma) + + n = int(duration * fs) + t, pos, vel, euler, f, w = sim(fs, n, degrees=False) + + return t, pos, vel, euler, f, w def benchmark_full_pva_chirp_202311A( From 4c8597665114e016cd0cf9e774c0b96e1cd1d5e0 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:37:26 +0100 Subject: [PATCH 099/129] deprecation warning beat dof --- src/smsfusion/benchmark/_benchmark.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 08d2b24f..40c77a65 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -1,6 +1,7 @@ from __future__ import annotations import abc +from warnings import warn import numpy as np from numpy.typing import ArrayLike, NDArray @@ -87,6 +88,7 @@ class BeatSignal(_Signal): """ def __init__(self, f_main: float, f_beat: float, freq_hz: bool = True) -> None: + warn("`BeatSignal`` is deprecated, use ``simulate.BeatDOF`` instead.") self._f_main = f_main self._f_beat = f_beat From c89b281250470b6ccf1cfb838bb050e68c7c8566 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 14:41:31 +0100 Subject: [PATCH 100/129] docstring --- src/smsfusion/simulate/_simulate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index d9cd7bd6..7f5ce503 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -183,7 +183,7 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: class BeatDOF(DOF): """ - 1D beat signal DOF generator. + 1D beating sinusoidal DOF signal generator. Defined as: From dbfff210b0060881906c759b96762d10be89c822 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:00:34 +0100 Subject: [PATCH 101/129] testchirp init --- src/smsfusion/simulate/__init__.py | 4 +- src/smsfusion/simulate/_simulate.py | 94 +++++++++++++++++++++++++++++ tests/test_simulate.py | 14 ++++- 3 files changed, 109 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/simulate/__init__.py b/src/smsfusion/simulate/__init__.py index 1f2ebff9..d9b7dec5 100644 --- a/src/smsfusion/simulate/__init__.py +++ b/src/smsfusion/simulate/__init__.py @@ -1,3 +1,3 @@ -from ._simulate import BeatDOF, ConstantDOF, IMUSimulator, SineDOF +from ._simulate import BeatDOF, ChirpDOF, ConstantDOF, IMUSimulator, SineDOF -__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "BeatDOF"] +__all__ = ["IMUSimulator", "SineDOF", "ConstantDOF", "BeatDOF", "ChirpDOF"] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 7f5ce503..507d6e13 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -205,6 +205,13 @@ class BeatDOF(DOF): The beating frequency, which controls the variation in amplitude. freq_hz : bool, default True. Whether the frequencies, ``f_main`` and ``f_beat``, are in Hz or rad/s (default). + phase : float, default 0.0 + Phase offset of the beat signal. Default is 0.0. + phase_degrees : bool, optional + If True, interpret `phase` in degrees. If False, interpret in radians. + Default is False. + offset : float, default 0.0 + Offset of the beat signal. Default is 0.0. """ def __init__( @@ -267,6 +274,93 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return d2ydt2 # type: ignore[no-any-return] +class ChirpDOF(DOF): + """ + 1D chirp sinusoidal DOF signal generator. + + This class creates a signal with a frequency that appears to vary in time. + The frequency oscillates between 0. and a maximum frequency at a specific + rate. + + Defined as: + + phi = 2 * f_max / f_os * sin(f_os * t) + y = sin(phi + phase) + + where, + + - f_max : Maximum frequency the signal ramps up to, before ramping down to zero. + - f_os : Frequency oscillation rate. + - phase : Phase offset of the chirp signal. + + Parameters + ---------- + f_max : float + The maximum frequency of the chirp signal, y(t). + f_os : float + The frequency oscillation rate. + freq_hz : bool, default True. + Whether the frequencies, ``f_max`` and ``f_os``, are in Hz or rad/s (default). + phase : float, default 0.0 + Phase offset of the chirp signal. Default is 0.0. + phase_degrees : bool, optional + If True, interpret `phase` in degrees. If False, interpret in radians. + Default is False. + offset : float, default 0.0 + Offset of the chirp signal. Default is 0.0. + """ + + def __init__( + self, + amp: float = 1.0, + f_max: float = 0.25, + f_os: float = 0.01, + freq_hz: bool = True, + phase: float = 0.0, + phase_degrees: bool = False, + offset: float = 0.0, + ) -> None: + self._amp = amp + self._w_max = 2.0 * np.pi * f_max if freq_hz else f_max + self._w_os = 2.0 * np.pi * f_os if freq_hz else f_os + self._phase = np.deg2rad(phase) if phase_degrees else phase + self._offset = offset + + def _y(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + amp = self._amp + w_max = self._w_max + w_os = self._w_os + phase = self._phase + offset = self._offset + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + y = amp * np.sin(phi + phase) + offset + return y # type: ignore[no-any-return] + + def _dydt(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + amp = self._amp + w_max = self._w_max + w_os = self._w_os + phase = self._phase + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + dphi = w_max * np.cos(w_os / 2.0 * t) + dydt = amp * dphi * np.cos(phi + phase) + return dydt # type: ignore[no-any-return] + + def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: + amp = self._amp + w_max = self._w_max + w_os = self._w_os + phase = self._phase + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + dphi = w_max * np.cos(w_os / 2.0 * t) + d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) + d2ydt2 = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + return d2ydt2 # type: ignore[no-any-return] + + # class LinearRampUp(DOF): # """ # Linear ramp-up wrapper for DOF signals. diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 5931ef4d..61b92571 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -2,7 +2,7 @@ import pytest import smsfusion as sf -from smsfusion.simulate import BeatDOF, ConstantDOF, IMUSimulator, SineDOF +from smsfusion.simulate import BeatDOF, ChirpDOF, ConstantDOF, IMUSimulator, SineDOF from smsfusion.simulate._simulate import DOF @@ -507,3 +507,15 @@ def test__call__(self, beat_dof, t): np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) np.testing.assert_allclose(d2ydt2, d2ydt2_expect) + + +class Test_ChirpDOF: + def test__init__(self): + chirp_dof = ChirpDOF( + 3.0, 2.0, 1.0, freq_hz=True, phase=4.0, phase_degrees=True, offset=5.0 + ) + + assert isinstance(chirp_dof, DOF) + assert chirp_dof._amp == 3.0 + assert chirp_dof._w_max == pytest.approx(2.0 * np.pi * 2.0) + assert chirp_dof._w_os == pytest.approx(2.0 * np.pi * 1.0) From 8e4e63ac3376e17919731d2bd88c9d340211f8dc Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:02:22 +0100 Subject: [PATCH 102/129] test init default --- tests/test_simulate.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 61b92571..a3151ded 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -519,3 +519,15 @@ def test__init__(self): assert chirp_dof._amp == 3.0 assert chirp_dof._w_max == pytest.approx(2.0 * np.pi * 2.0) assert chirp_dof._w_os == pytest.approx(2.0 * np.pi * 1.0) + assert chirp_dof._phase == pytest.approx((np.pi / 180.0) * 4.0) + assert chirp_dof._offset == 5.0 + + def test__init__default(self): + chirp_dof = ChirpDOF() + + assert isinstance(chirp_dof, DOF) + assert chirp_dof._amp == 1.0 + assert chirp_dof._w_max == pytest.approx(2.0 * np.pi * 0.25) + assert chirp_dof._w_os == pytest.approx(2.0 * np.pi * 0.01) + assert chirp_dof._phase == pytest.approx(0.0) + assert chirp_dof._offset == 0.0 From 962e3cc7f82e7fabfe35f399b75f3f33c54e0675 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:04:27 +0100 Subject: [PATCH 103/129] update default beat freqs --- src/smsfusion/simulate/_simulate.py | 4 ++-- tests/test_simulate.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 507d6e13..5aa8492a 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -217,8 +217,8 @@ class BeatDOF(DOF): def __init__( self, amp: float = 1.0, - freq_main: float = 1.0, - freq_beat: float = 0.1, + freq_main: float = 0.1, + freq_beat: float = 0.01, freq_hz: bool = False, phase: float = 0.0, phase_degrees: bool = False, diff --git a/tests/test_simulate.py b/tests/test_simulate.py index a3151ded..6983b487 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -427,8 +427,8 @@ def test__init__default(self): assert isinstance(beat_dof, DOF) assert beat_dof._amp == 1.0 - assert beat_dof._w_main == pytest.approx(1.0) - assert beat_dof._w_beat == pytest.approx(0.1) + assert beat_dof._w_main == pytest.approx(0.1) + assert beat_dof._w_beat == pytest.approx(0.01) assert beat_dof._phase == pytest.approx(0.0) assert beat_dof._offset == 0.0 From a80c5008c97fddb479a30fbf2533f0cbd7522f0c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:08:48 +0100 Subject: [PATCH 104/129] test y --- tests/test_simulate.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 6983b487..ad07d002 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -510,6 +510,11 @@ def test__call__(self, beat_dof, t): class Test_ChirpDOF: + @pytest.fixture + def chirp(self): + chirp = ChirpDOF(amp=2.0, f_max=0.25, f_os=0.01, freq_hz=True, offset=1.0) + return chirp + def test__init__(self): chirp_dof = ChirpDOF( 3.0, 2.0, 1.0, freq_hz=True, phase=4.0, phase_degrees=True, offset=5.0 @@ -531,3 +536,17 @@ def test__init__default(self): assert chirp_dof._w_os == pytest.approx(2.0 * np.pi * 0.01) assert chirp_dof._phase == pytest.approx(0.0) assert chirp_dof._offset == 0.0 + + def test_y(self, chirp, t): + y = chirp.y(t) + + amp = chirp._amp + w_max = chirp._w_max + w_os = chirp._w_os + phase = chirp._phase + offset = chirp._offset + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + y_expect = amp * np.sin(phi + phase) + offset + + np.testing.assert_allclose(y, y_expect) From a8ce4e4851e0715396d8b343c86d0ad2b1167914 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:10:45 +0100 Subject: [PATCH 105/129] test d2ydt2 --- tests/test_simulate.py | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index ad07d002..739ce4f4 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -550,3 +550,33 @@ def test_y(self, chirp, t): y_expect = amp * np.sin(phi + phase) + offset np.testing.assert_allclose(y, y_expect) + + def test_dydt(self, chirp, t): + dydt = chirp.dydt(t) + + amp = chirp._amp + w_max = chirp._w_max + w_os = chirp._w_os + phase = chirp._phase + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + dphi_dt = w_max * np.cos(w_os / 2.0 * t) + + dydt_expect = amp * dphi_dt * np.cos(phi + phase) + + np.testing.assert_allclose(dydt, dydt_expect) + + def test_d2ydt2(self, chirp, t): + d2ydt2 = chirp.d2ydt2(t) + + amp = chirp._amp + w_max = chirp._w_max + w_os = chirp._w_os + phase = chirp._phase + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + dphi = w_max * np.cos(w_os / 2.0 * t) + d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + + np.testing.assert_allclose(d2ydt2, d2ydt2_expect) From dca7e3a0587138b3b6e89321acb33431b5d01efd Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:11:15 +0100 Subject: [PATCH 106/129] test call --- tests/test_simulate.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 739ce4f4..bdca4ba3 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -580,3 +580,24 @@ def test_d2ydt2(self, chirp, t): d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) np.testing.assert_allclose(d2ydt2, d2ydt2_expect) + + def test__call__(self, chirp, t): + y, dydt, d2ydt2 = chirp(t) + + amp = chirp._amp + w_max = chirp._w_max + w_os = chirp._w_os + phase = chirp._phase + offset = chirp._offset + + phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) + dphi = w_max * np.cos(w_os / 2.0 * t) + d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) + + y_expect = amp * np.sin(phi + phase) + offset + dydt_expect = amp * dphi * np.cos(phi + phase) + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + + np.testing.assert_allclose(y, y_expect) + np.testing.assert_allclose(dydt, dydt_expect) + np.testing.assert_allclose(d2ydt2, d2ydt2_expect) From dc5fc5b670b13f33919018578d99f5c1ba3b5d53 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:13:42 +0100 Subject: [PATCH 107/129] rename fixture to beat --- tests/test_simulate.py | 68 +++++++++++++++++++++--------------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index bdca4ba3..de80e71c 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -400,12 +400,12 @@ def test__call__radians(self, sim): class Test_BeatDOF: @pytest.fixture - def beat_dof(self): + def beat(self): dof = BeatDOF(amp=2.0, freq_main=1.0, freq_beat=0.1, freq_hz=False, offset=1.0) return dof def test__init__(self): - beat_dof = BeatDOF( + beat = BeatDOF( amp=3.0, freq_main=2.0, freq_beat=0.2, @@ -415,12 +415,12 @@ def test__init__(self): offset=5.0, ) - assert isinstance(beat_dof, DOF) - assert beat_dof._amp == 3.0 - assert beat_dof._w_main == pytest.approx(2.0 * np.pi * 2.0) - assert beat_dof._w_beat == pytest.approx(2.0 * np.pi * 0.2) - assert beat_dof._phase == pytest.approx((np.pi / 180.0) * 4.0) - assert beat_dof._offset == 5.0 + assert isinstance(beat, DOF) + assert beat._amp == 3.0 + assert beat._w_main == pytest.approx(2.0 * np.pi * 2.0) + assert beat._w_beat == pytest.approx(2.0 * np.pi * 0.2) + assert beat._phase == pytest.approx((np.pi / 180.0) * 4.0) + assert beat._offset == 5.0 def test__init__default(self): beat_dof = BeatDOF() @@ -432,14 +432,14 @@ def test__init__default(self): assert beat_dof._phase == pytest.approx(0.0) assert beat_dof._offset == 0.0 - def test_y(self, beat_dof, t): - y = beat_dof.y(t) + def test_y(self, beat, t): + y = beat.y(t) - amp = beat_dof._amp - w_main = beat_dof._w_main - w_beat = beat_dof._w_beat - phase = beat_dof._phase - offset = beat_dof._offset + amp = beat._amp + w_main = beat._w_main + w_beat = beat._w_beat + phase = beat._phase + offset = beat._offset main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) @@ -448,13 +448,13 @@ def test_y(self, beat_dof, t): np.testing.assert_allclose(y, y_expect) - def test_dydt(self, beat_dof, t): - dydt = beat_dof.dydt(t) + def test_dydt(self, beat, t): + dydt = beat.dydt(t) - amp = beat_dof._amp - w_main = beat_dof._w_main - w_beat = beat_dof._w_beat - phase = beat_dof._phase + amp = beat._amp + w_main = beat._w_main + w_beat = beat._w_beat + phase = beat._phase main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) @@ -465,13 +465,13 @@ def test_dydt(self, beat_dof, t): np.testing.assert_allclose(dydt, dydt_expect) - def test_d2ydt2(self, beat_dof, t): - d2ydt2 = beat_dof.d2ydt2(t) + def test_d2ydt2(self, beat, t): + d2ydt2 = beat.d2ydt2(t) - amp = beat_dof._amp - w_main = beat_dof._w_main - w_beat = beat_dof._w_beat - phase = beat_dof._phase + amp = beat._amp + w_main = beat._w_main + w_beat = beat._w_beat + phase = beat._phase main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) @@ -484,14 +484,14 @@ def test_d2ydt2(self, beat_dof, t): np.testing.assert_allclose(d2ydt2, d2ydt2_expect) - def test__call__(self, beat_dof, t): - y, dydt, d2ydt2 = beat_dof(t) + def test__call__(self, beat, t): + y, dydt, d2ydt2 = beat(t) - amp = beat_dof._amp - w_main = beat_dof._w_main - w_beat = beat_dof._w_beat - phase = beat_dof._phase - offset = beat_dof._offset + amp = beat._amp + w_main = beat._w_main + w_beat = beat._w_beat + phase = beat._phase + offset = beat._offset main = np.cos(w_main * t + phase) beat = np.sin(w_beat / 2.0 * t) From 17683408df90d29e9d4f32c5cd5d700fde89be69 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:14:01 +0100 Subject: [PATCH 108/129] black --- tests/test_simulate.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tests/test_simulate.py b/tests/test_simulate.py index de80e71c..78ae7f43 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -577,7 +577,9 @@ def test_d2ydt2(self, chirp, t): phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) dphi = w_max * np.cos(w_os / 2.0 * t) d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) - d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos( + phi + phase + ) np.testing.assert_allclose(d2ydt2, d2ydt2_expect) @@ -596,7 +598,9 @@ def test__call__(self, chirp, t): y_expect = amp * np.sin(phi + phase) + offset dydt_expect = amp * dphi * np.cos(phi + phase) - d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos( + phi + phase + ) np.testing.assert_allclose(y, y_expect) np.testing.assert_allclose(dydt, dydt_expect) From f9dce4317e25614829f2165fbf98d105c1cc9332 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:19:00 +0100 Subject: [PATCH 109/129] update chirp pure atti --- src/smsfusion/benchmark/_benchmark.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 40c77a65..9f0d4b64 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -8,7 +8,7 @@ from smsfusion._ins import gravity from smsfusion._transforms import _inv_angular_matrix_from_euler, _rot_matrix_from_euler -from smsfusion.simulate import BeatDOF, IMUSimulator +from smsfusion.simulate import BeatDOF, IMUSimulator, ChirpDOF class _Signal(abc.ABC): @@ -470,17 +470,20 @@ def benchmark_pure_attitude_chirp_202311A( within the frame. """ duration = 1800.0 # 30 minutes - amplitude = np.radians(np.array([0.0, 0.0, 0.0, 5.0, 5.0, 5.0])) - mean = np.radians(np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])) - phase = np.radians(np.array([0.0, 0.0, 0.0, 0.0, 45.0, 90.0])) f_max = 0.25 f_os = 0.01 - t, _, _, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, ChirpSignal(f_max, f_os), fs - ) - return t, euler, acc, gyro + amp = np.radians(5.0) + alpha = ChirpDOF(amp, f_max, f_os, freq_hz=True, phase=0.0) + beta = ChirpDOF(amp, f_max, f_os, freq_hz=True, phase=45.0, phase_degrees=True) + gamma = ChirpDOF(amp, f_max, f_os, freq_hz=True, phase=90.0, phase_degrees=True) + sim = IMUSimulator(alpha=alpha, beta=beta, gamma=gamma) + + n = int(duration * fs) + t, _, _, euler, f, w = sim(fs, n, degrees=False) + + return t, euler, f, w def benchmark_full_pva_beat_202311A( From 180f6729e48c03a17511193e8f3b8517dab90c5c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:20:59 +0100 Subject: [PATCH 110/129] update chirp full pva --- src/smsfusion/benchmark/_benchmark.py | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 9f0d4b64..c7f8665a 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -8,7 +8,7 @@ from smsfusion._ins import gravity from smsfusion._transforms import _inv_angular_matrix_from_euler, _rot_matrix_from_euler -from smsfusion.simulate import BeatDOF, IMUSimulator, ChirpDOF +from smsfusion.simulate import BeatDOF, ChirpDOF, IMUSimulator class _Signal(abc.ABC): @@ -650,14 +650,21 @@ def benchmark_full_pva_chirp_202311A( within the frame. """ duration = 1800.0 # 30 minutes - amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) f_max = 0.25 f_os = 0.01 - t, pos, vel, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, ChirpSignal(f_max, f_os), fs - ) - return t, pos, vel, euler, acc, gyro + amp_p = 0.5 + amp_r = np.radians(5.0) + pos_x = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=0.0) + pos_y = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=30.0, phase_degrees=True) + pos_z = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=60.0, phase_degrees=True) + alpha = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=90.0) + beta = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=120.0, phase_degrees=True) + gamma = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=150.0, phase_degrees=True) + sim = IMUSimulator(pos_x, pos_y, pos_z, alpha, beta, gamma) + + n = int(duration * fs) + t, pos, vel, euler, f, w = sim(fs, n, degrees=False) + + return t, pos, vel, euler, f, w From 063aa556d5d1ef9f390ac5081b7047bbfcde907a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 15:46:47 +0100 Subject: [PATCH 111/129] fix amp bug --- src/smsfusion/benchmark/_benchmark.py | 84 +++++++++++++++++++++++++++ src/smsfusion/simulate/_simulate.py | 2 +- tests/test_simulate.py | 4 +- 3 files changed, 87 insertions(+), 3 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index c7f8665a..e49801a8 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -668,3 +668,87 @@ def benchmark_full_pva_chirp_202311A( t, pos, vel, euler, f, w = sim(fs, n, degrees=False) return t, pos, vel, euler, f, w + + +def benchmark_full_pva_chirp_202311A2( + fs: float = 10.24, +) -> tuple[ + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], +]: + """ + Generate a benchmark with full position, velocity, and attitude (PVA) for + performance testing of INS/AHRS/VRU sensor fusion algorithms. + + The benchmark scenario is 30 minutes long. It generates full position, velocity + and attitude (PVA), and the corresponding accelerometer and gyroscope signals. + + The generated position reference signals are characterized by: + + * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. + * Maximum amplitude of 0.5 m. + * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. + + The generated Euler reference signals are characterized by: + + * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. + * Maximum amplitude of 5 degrees. + * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. + + The other reference signals will be exact analythical derivatives of these signals. + + Parameters + ---------- + fs : float, default 10.24 + Sampling frequency in hertz of the generated signals. + + Returns + ------- + t : numpy.ndarray, shape (N,) + Time in seconds. + pos : numpy.ndarray, shape (N, 3) + Position [m] of the body relative to the NED frame. + vel : numpy.ndarray, shape (N, 3) + Velocity [m/s] of the body relative to the NED frame. + euler : numpy.ndarray, shape (N, 3) + Attitude of the body in Euler angles [rad], see Notes. + acc : numpy.ndarray, shape (N, 3) + Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements). + gyro : numpy.ndarray, shape (N, 3) + Angular rates [rad/s] in body frame (corresponding to gyroscope measurements). + + Notes + ----- + The Euler angles describe how to transition from the 'NED' frame to the 'body' + frame through three consecutive intrinsic and passive rotations in the ZYX order: + + #. A rotation by an angle gamma (often called yaw) about the z-axis. + #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. + #. A final rotation by an angle alpha (often called roll) about the x-axis. + + This sequence of rotations is used to describe the orientation of the 'body' frame + relative to the 'NED' frame in 3D space. + + Intrinsic rotations mean that the rotations are with respect to the changing + coordinate system; as one rotation is applied, the next is about the axis of + the newly rotated system. + + Passive rotations mean that the frame itself is rotating, not the object + within the frame. + """ + duration = 1800.0 # 30 minutes + amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) + mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) + + f_max = 0.25 + f_os = 0.01 + + t, pos, vel, euler, acc, gyro = _benchmark_helper( + duration, amplitude, mean, phase, ChirpSignal(f_max, f_os), fs + ) + return t, pos, vel, euler, acc, gyro diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 5aa8492a..c74e8390 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -357,7 +357,7 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) dphi = w_max * np.cos(w_os / 2.0 * t) d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) - d2ydt2 = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + d2ydt2 = -amp * (dphi**2) * np.sin(phi + phase) + amp * d2phi * np.cos(phi + phase) return d2ydt2 # type: ignore[no-any-return] diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 78ae7f43..7beb27fc 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -577,7 +577,7 @@ def test_d2ydt2(self, chirp, t): phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) dphi = w_max * np.cos(w_os / 2.0 * t) d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) - d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos( + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + amp * d2phi * np.cos( phi + phase ) @@ -598,7 +598,7 @@ def test__call__(self, chirp, t): y_expect = amp * np.sin(phi + phase) + offset dydt_expect = amp * dphi * np.cos(phi + phase) - d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + d2phi * np.cos( + d2ydt2_expect = -amp * (dphi**2) * np.sin(phi + phase) + amp * d2phi * np.cos( phi + phase ) From 0f2e280261186e7babb49e7448859ed212ae5ffa Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 16:08:10 +0100 Subject: [PATCH 112/129] tag old --- src/smsfusion/benchmark/_benchmark.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index e49801a8..3a1c67f9 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -670,7 +670,7 @@ def benchmark_full_pva_chirp_202311A( return t, pos, vel, euler, f, w -def benchmark_full_pva_chirp_202311A2( +def benchmark_full_pva_chirp_202311A_old( fs: float = 10.24, ) -> tuple[ NDArray[np.float64], From f2073e8aed73ccfe961be8a79723787a1085724a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 16:12:30 +0100 Subject: [PATCH 113/129] old beat --- src/smsfusion/benchmark/_benchmark.py | 84 +++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 3a1c67f9..db75b4a9 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -670,6 +670,90 @@ def benchmark_full_pva_chirp_202311A( return t, pos, vel, euler, f, w +def benchmark_full_pva_beat_202311A_old( + fs: float = 10.24, +) -> tuple[ + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], + NDArray[np.float64], +]: + """ + Generate a benchmark with full position, velocity, and attitude (PVA) for + performance testing of INS/AHRS/VRU sensor fusion algorithms. + + The benchmark scenario is 30 minutes long. It generates full position, velocity + and attitude (PVA), and the corresponding accelerometer and gyroscope signals. + + The generated position reference signals are characterized by: + + * "Beating" signal. See ``BeatSignal`` for details. + * Maximum amplitude of 0.5 m. + * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. + + The generated Euler reference signals are characterized by: + + * "Beating" signal. See :class:`smsfusion.benchmark.BeatSignal` for details. + * Maximum amplitude of 5 degrees. + * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. + + The other reference signals will be exact analythical derivatives of these signals. + + Parameters + ---------- + fs : float, default 10.24 + Sampling frequency in hertz of the generated signals. + + Returns + ------- + t : numpy.ndarray, shape (N,) + Time in seconds. + pos : numpy.ndarray, shape (N, 3) + Position [m] of the body relative to the NED frame. + vel : numpy.ndarray, shape (N, 3) + Velocity [m/s] of the body relative to the NED frame. + euler : numpy.ndarray, shape (N, 3) + Attitude of the body in Euler angles [rad], see Notes. + acc : numpy.ndarray, shape (N, 3) + Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements). + gyro : numpy.ndarray, shape (N, 3) + Angular rates [rad/s] in body frame (corresponding to gyroscope measurements). + + Notes + ----- + The Euler angles describe how to transition from the 'NED' frame to the 'body' + frame through three consecutive intrinsic and passive rotations in the ZYX order: + + #. A rotation by an angle gamma (often called yaw) about the z-axis. + #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. + #. A final rotation by an angle alpha (often called roll) about the x-axis. + + This sequence of rotations is used to describe the orientation of the 'body' frame + relative to the 'NED' frame in 3D space. + + Intrinsic rotations mean that the rotations are with respect to the changing + coordinate system; as one rotation is applied, the next is about the axis of + the newly rotated system. + + Passive rotations mean that the frame itself is rotating, not the object + within the frame. + """ + duration = 1800.0 # 30 minutes + amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) + mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) + + f_main = 0.1 + f_beat = 0.01 + + t, pos, vel, euler, acc, gyro = _benchmark_helper( + duration, amplitude, mean, phase, BeatSignal(f_main, f_beat), fs + ) + return t, pos, vel, euler, acc, gyro + + def benchmark_full_pva_chirp_202311A_old( fs: float = 10.24, ) -> tuple[ From 6f7881ebd32862d2b4eed55d4caa42bdc974975d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 19 Dec 2025 16:13:48 +0100 Subject: [PATCH 114/129] warning --- src/smsfusion/benchmark/_benchmark.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index db75b4a9..f0587729 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -160,6 +160,7 @@ class ChirpSignal(_Signal): """ def __init__(self, f_max: float, f_os: float, freq_hz: bool = True) -> None: + warn("`ChirpSignal` is deprecated, use `simulate.ChirpDOF` instead.") self._f_max = f_max self._f_os = f_os From bd54c0eedd81b68d25df7d1e363e959294635a03 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 08:59:35 +0100 Subject: [PATCH 115/129] fix phase bug --- src/smsfusion/benchmark/_benchmark.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index f0587729..175e28a2 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -564,10 +564,10 @@ def benchmark_full_pva_beat_202311A( amp_p = 0.5 amp_r = np.radians(5.0) - pos_x = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=0.0) + pos_x = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=0.0, phase_degrees=True) pos_y = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=30.0, phase_degrees=True) pos_z = BeatDOF(amp_p, f_main, f_beat, freq_hz=True, phase=60.0, phase_degrees=True) - alpha = BeatDOF(amp_r, f_main, f_beat, freq_hz=True, phase=90.0) + alpha = BeatDOF(amp_r, f_main, f_beat, freq_hz=True, phase=90.0, phase_degrees=True) beta = BeatDOF(amp_r, f_main, f_beat, freq_hz=True, phase=120.0, phase_degrees=True) gamma = BeatDOF( amp_r, f_main, f_beat, freq_hz=True, phase=150.0, phase_degrees=True @@ -657,10 +657,10 @@ def benchmark_full_pva_chirp_202311A( amp_p = 0.5 amp_r = np.radians(5.0) - pos_x = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=0.0) + pos_x = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=0.0, phase_degrees=True) pos_y = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=30.0, phase_degrees=True) pos_z = ChirpDOF(amp_p, f_max, f_os, freq_hz=True, phase=60.0, phase_degrees=True) - alpha = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=90.0) + alpha = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=90.0, phase_degrees=True) beta = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=120.0, phase_degrees=True) gamma = ChirpDOF(amp_r, f_max, f_os, freq_hz=True, phase=150.0, phase_degrees=True) sim = IMUSimulator(pos_x, pos_y, pos_z, alpha, beta, gamma) From 534fbb63ead41fc42042a2f1dd3fa8d01f813507 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:03:38 +0100 Subject: [PATCH 116/129] revert test atol --- tests/test_benchmark.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/tests/test_benchmark.py b/tests/test_benchmark.py index a5c82214..27bb0e8b 100644 --- a/tests/test_benchmark.py +++ b/tests/test_benchmark.py @@ -312,9 +312,7 @@ def test_benchmark_pure_attitude_beat_202311A(): assert acc.shape == (len(t), 3) assert gyro.shape == (len(t), 3) - np.testing.assert_allclose( - euler[:, 0], np.radians(5.0) * signature_signal, atol=1e-12 - ) + np.testing.assert_allclose(euler[:, 0], np.radians(5.0) * signature_signal) def test_benchmark_pure_attitude_chirp_202311A(): From 3c77ff0f5861e935d9be9ef11314c0e9c50e9f90 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:04:09 +0100 Subject: [PATCH 117/129] black --- src/smsfusion/simulate/_simulate.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index c74e8390..b6e0021a 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -357,7 +357,9 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: phi = 2.0 * w_max / w_os * np.sin(w_os / 2.0 * t) dphi = w_max * np.cos(w_os / 2.0 * t) d2phi = -w_max * w_os / 2.0 * np.sin(w_os / 2.0 * t) - d2ydt2 = -amp * (dphi**2) * np.sin(phi + phase) + amp * d2phi * np.cos(phi + phase) + d2ydt2 = -amp * (dphi**2) * np.sin(phi + phase) + amp * d2phi * np.cos( + phi + phase + ) return d2ydt2 # type: ignore[no-any-return] From 43eb0b2f002c9691424c8bf539ac07624869747e Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:04:42 +0100 Subject: [PATCH 118/129] delete old benchmarks --- src/smsfusion/benchmark/_benchmark.py | 168 -------------------------- 1 file changed, 168 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 175e28a2..12f109fb 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -669,171 +669,3 @@ def benchmark_full_pva_chirp_202311A( t, pos, vel, euler, f, w = sim(fs, n, degrees=False) return t, pos, vel, euler, f, w - - -def benchmark_full_pva_beat_202311A_old( - fs: float = 10.24, -) -> tuple[ - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], -]: - """ - Generate a benchmark with full position, velocity, and attitude (PVA) for - performance testing of INS/AHRS/VRU sensor fusion algorithms. - - The benchmark scenario is 30 minutes long. It generates full position, velocity - and attitude (PVA), and the corresponding accelerometer and gyroscope signals. - - The generated position reference signals are characterized by: - - * "Beating" signal. See ``BeatSignal`` for details. - * Maximum amplitude of 0.5 m. - * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. - - The generated Euler reference signals are characterized by: - - * "Beating" signal. See :class:`smsfusion.benchmark.BeatSignal` for details. - * Maximum amplitude of 5 degrees. - * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. - - The other reference signals will be exact analythical derivatives of these signals. - - Parameters - ---------- - fs : float, default 10.24 - Sampling frequency in hertz of the generated signals. - - Returns - ------- - t : numpy.ndarray, shape (N,) - Time in seconds. - pos : numpy.ndarray, shape (N, 3) - Position [m] of the body relative to the NED frame. - vel : numpy.ndarray, shape (N, 3) - Velocity [m/s] of the body relative to the NED frame. - euler : numpy.ndarray, shape (N, 3) - Attitude of the body in Euler angles [rad], see Notes. - acc : numpy.ndarray, shape (N, 3) - Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements). - gyro : numpy.ndarray, shape (N, 3) - Angular rates [rad/s] in body frame (corresponding to gyroscope measurements). - - Notes - ----- - The Euler angles describe how to transition from the 'NED' frame to the 'body' - frame through three consecutive intrinsic and passive rotations in the ZYX order: - - #. A rotation by an angle gamma (often called yaw) about the z-axis. - #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. - #. A final rotation by an angle alpha (often called roll) about the x-axis. - - This sequence of rotations is used to describe the orientation of the 'body' frame - relative to the 'NED' frame in 3D space. - - Intrinsic rotations mean that the rotations are with respect to the changing - coordinate system; as one rotation is applied, the next is about the axis of - the newly rotated system. - - Passive rotations mean that the frame itself is rotating, not the object - within the frame. - """ - duration = 1800.0 # 30 minutes - amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) - - f_main = 0.1 - f_beat = 0.01 - - t, pos, vel, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, BeatSignal(f_main, f_beat), fs - ) - return t, pos, vel, euler, acc, gyro - - -def benchmark_full_pva_chirp_202311A_old( - fs: float = 10.24, -) -> tuple[ - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], -]: - """ - Generate a benchmark with full position, velocity, and attitude (PVA) for - performance testing of INS/AHRS/VRU sensor fusion algorithms. - - The benchmark scenario is 30 minutes long. It generates full position, velocity - and attitude (PVA), and the corresponding accelerometer and gyroscope signals. - - The generated position reference signals are characterized by: - - * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. - * Maximum amplitude of 0.5 m. - * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. - - The generated Euler reference signals are characterized by: - - * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. - * Maximum amplitude of 5 degrees. - * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. - - The other reference signals will be exact analythical derivatives of these signals. - - Parameters - ---------- - fs : float, default 10.24 - Sampling frequency in hertz of the generated signals. - - Returns - ------- - t : numpy.ndarray, shape (N,) - Time in seconds. - pos : numpy.ndarray, shape (N, 3) - Position [m] of the body relative to the NED frame. - vel : numpy.ndarray, shape (N, 3) - Velocity [m/s] of the body relative to the NED frame. - euler : numpy.ndarray, shape (N, 3) - Attitude of the body in Euler angles [rad], see Notes. - acc : numpy.ndarray, shape (N, 3) - Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements). - gyro : numpy.ndarray, shape (N, 3) - Angular rates [rad/s] in body frame (corresponding to gyroscope measurements). - - Notes - ----- - The Euler angles describe how to transition from the 'NED' frame to the 'body' - frame through three consecutive intrinsic and passive rotations in the ZYX order: - - #. A rotation by an angle gamma (often called yaw) about the z-axis. - #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. - #. A final rotation by an angle alpha (often called roll) about the x-axis. - - This sequence of rotations is used to describe the orientation of the 'body' frame - relative to the 'NED' frame in 3D space. - - Intrinsic rotations mean that the rotations are with respect to the changing - coordinate system; as one rotation is applied, the next is about the axis of - the newly rotated system. - - Passive rotations mean that the frame itself is rotating, not the object - within the frame. - """ - duration = 1800.0 # 30 minutes - amplitude = (0.5, 0.5, 0.5, np.radians(5.0), np.radians(5.0), np.radians(5.0)) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = np.radians(np.array([0.0, 30.0, 60.0, 90.0, 120.0, 150.0])) - - f_max = 0.25 - f_os = 0.01 - - t, pos, vel, euler, acc, gyro = _benchmark_helper( - duration, amplitude, mean, phase, ChirpSignal(f_max, f_os), fs - ) - return t, pos, vel, euler, acc, gyro From 2cf7943a6aca27df548c1c9f81cd0dd491ee094a Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:09:06 +0100 Subject: [PATCH 119/129] docstring --- src/smsfusion/benchmark/_benchmark.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 12f109fb..4a4c312f 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -506,13 +506,13 @@ def benchmark_full_pva_beat_202311A( The generated position reference signals are characterized by: - * "Beating" signal. See ``BeatSignal`` for details. + * "Beating" signal. See ``smsfusion.simulate.BeatDOF`` for details. * Maximum amplitude of 0.5 m. * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. The generated Euler reference signals are characterized by: - * "Beating" signal. See :class:`smsfusion.benchmark.BeatSignal` for details. + * "Beating" signal. See :class:`smsfusion.simulate.BeatDOF` for details. * Maximum amplitude of 5 degrees. * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. @@ -599,13 +599,13 @@ def benchmark_full_pva_chirp_202311A( The generated position reference signals are characterized by: - * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. + * "Chirp" signal. See :class:`smsfusion.simulate.ChirpDOF` for details. * Maximum amplitude of 0.5 m. * The phases for x-, y-, and z-axis are 0.0, 30.0, and 60.0 degrees respectively. The generated Euler reference signals are characterized by: - * "Chirp" signal. See :class:`smsfusion.benchmark.ChirpSignal` for details. + * "Chirp" signal. See :class:`smsfusion.simulate.ChirpDOF` for details. * Maximum amplitude of 5 degrees. * The phases for roll, pitch, and yaw are 90.0, 120.0, and 150.0 degrees respectively. From 77f4a022844e36d8b09b33987f6a4694d0efbd81 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:10:06 +0100 Subject: [PATCH 120/129] docstring fix --- src/smsfusion/benchmark/_benchmark.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 4a4c312f..de4a16ed 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -362,7 +362,7 @@ def benchmark_pure_attitude_beat_202311A( The generated signals are "beating" with maximum amplitudes corresponding to 5 degrees. The main signal frequency is 0.1 Hz while the "beating" frequency is 0.01 Hz. The phases for roll, pitch, and yaw are 0.0, 45.0, and 90.0 degrees - respectively. See :class:`smsfusion.benchmark.BeatSignal` for details. + respectively. See :class:`smsfusion.simulate.BeatDOF` for details. Parameters ---------- @@ -433,7 +433,7 @@ def benchmark_pure_attitude_chirp_202311A( amplitudes corresponding to 5 degrees. The signal frequency oscillates between 0.0 Hz and 0.25 Hz every 100 seconds. The phases for roll, pitch, and yaw are 0.0, 45.0, and 90.0 degrees respectively. See - :class:`smsfusion.benchmark.ChirpSignal` for details. + :class:`smsfusion.simulate.ChirpDOF` for details. Parameters ---------- From ec32ae6cef02dbf5c763f96517b1daa487db1766 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:14:47 +0100 Subject: [PATCH 121/129] delete obsolete benchmark helper --- src/smsfusion/benchmark/__init__.py | 2 - src/smsfusion/benchmark/_benchmark.py | 148 ------------------- tests/test_benchmark.py | 205 -------------------------- 3 files changed, 355 deletions(-) diff --git a/src/smsfusion/benchmark/__init__.py b/src/smsfusion/benchmark/__init__.py index 40e4ab88..848a4b83 100644 --- a/src/smsfusion/benchmark/__init__.py +++ b/src/smsfusion/benchmark/__init__.py @@ -1,7 +1,6 @@ from ._benchmark import ( BeatSignal, ChirpSignal, - _benchmark_helper, benchmark_full_pva_beat_202311A, benchmark_full_pva_chirp_202311A, benchmark_pure_attitude_beat_202311A, @@ -9,7 +8,6 @@ ) __all__ = [ - "_benchmark_helper", "BeatSignal", "benchmark_full_pva_beat_202311A", "benchmark_full_pva_chirp_202311A", diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index de4a16ed..fd99bc88 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -198,154 +198,6 @@ def _d2ydt2(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: return d2ydt2 # type: ignore[no-any-return] -def _benchmark_helper( - duration: float, - amplitude: tuple[float, float, float, float, float, float], - mean: tuple[float, float, float, float, float, float], - phase: tuple[float, float, float, float, float, float], - signal_family: _Signal, - fs: float, -) -> tuple[ - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], - NDArray[np.float64], -]: - """ - Generate benchmark scenarios for INS sensor fusion performance evaluation. - - The general helper function works by first generating position and attitude - based on the provided input, and then derives the remaining variables. The - final output consists of: - - * Time [s] - * Position [m] - * Velocity [m/s] - * Attitude [rad] (Euler angles, see Notes) - * Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements) - * Angular rates [rad/s] in body frame (corresponding to gyroscope measurements) - - Position, velocity, and attitude are relative to the *NED* frame and expressed - in the *NED* frame. The position origin is at (0., 0., 0.). - - Parameters - ---------- - duration : float - Duration of the generated signals in seconds. - amplitude : tuple of (float, float, float, float, float, float) - Amplitude of the signals generated by the generator defined in ``signal_family``. - The values represent the amplitude of postions in x, y, and z directions in - meters, and Euler angles as roll, pitch, and yaw in radians (see Notes). - The order is given as [pos_x, pos_y, pos_z, roll, pitch, yaw]. - mean : tuple of (float, float, float, float, float, float) - Mean values of the generated signals. Follows the same conventions as - ``amplitude``. - phase : tuple of (float, float, float, float, float, float) - Phase values in radians passed on to the signal generator in - ``signal_family``. Otherwise, follows the same conventions as - ``amplitude``. - signal_family : {:clas:`smsfusion.benchmark.BeatSignal`, :clas:`smsfusion.benchmark.BeatSignal`} - Instance of a class used to generate the signals. - fs : float - Sampling rate of the outputs in hertz. - - Returns - ------- - t : numpy.ndarray, shape (N,) - Time in seconds. - position : numpy.ndarray, shape (N, 3) - Position [m] of the body. - velocity : numpy.ndarray, shape (N, 3) - Velocity [m/s] of the body. - euler : numpy.ndarray, shape (N, 3) - Attitude of the body in Euler angles [rad], see Notes. - acc : numpy.ndarray, shape (N, 3) - Accelerations [m/s**2] in body frame (corresponding to accelerometer measurements). - gyro : numpy.ndarray, shape (N, 3) - Angular rates [rad/s] in body frame (corresponding to gyroscope measurements). - - Notes - ----- - The length of the signals are determined by ``np.arange(0.0, duration, 1.0 / fs)``. - - The Euler angles describe how to transition from the 'NED' frame to the 'body' - frame through three consecutive intrinsic and passive rotations in the ZYX order: - - #. A rotation by an angle gamma (often called yaw) about the z-axis. - #. A subsequent rotation by an angle beta (often called pitch) about the y-axis. - #. A final rotation by an angle alpha (often called roll) about the x-axis. - - This sequence of rotations is used to describe the orientation of the 'body' frame - relative to the 'NED' frame in 3D space. - - Intrinsic rotations mean that the rotations are with respect to the changing - coordinate system; as one rotation is applied, the next is about the axis of - the newly rotated system. - - Passive rotations mean that the frame itself is rotating, not the object - within the frame. - """ - t = np.arange(0.0, duration, 1.0 / fs) - - amp_x, amp_y, amp_z, amp_roll, amp_pitch, amp_yaw = amplitude - mean_x, mean_y, mean_z, mean_roll, mean_pitch, mean_yaw = mean - phase_x, phase_y, phase_z, phase_roll, phase_pitch, phase_yaw = phase - - pos_x_, vel_x_, acc_x_ = signal_family(t, phase_x, phase_degrees=False) - pos_x = mean_x + amp_x * pos_x_ - vel_x = amp_x * vel_x_ - acc_x = amp_x * acc_x_ - - pos_y_, vel_y_, acc_y_ = signal_family(t, phase_y, phase_degrees=False) - pos_y = mean_y + amp_y * pos_y_ - vel_y = amp_y * vel_y_ - acc_y = amp_y * acc_y_ - - pos_z_, vel_z_, acc_z_ = signal_family(t, phase_z, phase_degrees=False) - pos_z = mean_z + amp_z * pos_z_ - vel_z = amp_z * vel_z_ - acc_z = amp_z * acc_z_ - - roll_, droll_, _ = signal_family(t, phase_roll, phase_degrees=False) - roll = mean_roll + amp_roll * roll_ - droll = amp_roll * droll_ - - pitch_, dpitch_, _ = signal_family(t, phase_pitch, phase_degrees=False) - pitch = mean_pitch + amp_pitch * pitch_ - dpitch = amp_pitch * dpitch_ - - yaw_, dyaw_, _ = signal_family(t, phase_yaw, phase_degrees=False) - yaw = mean_yaw + amp_yaw * yaw_ - dyaw = amp_yaw * dyaw_ - - position = np.column_stack((pos_x, pos_y, pos_z)) - velocity = np.column_stack((vel_x, vel_y, vel_z)) - acceleration = np.column_stack((acc_x, acc_y, acc_z)) - attitude = np.column_stack((roll, pitch, yaw)) - dattitude = np.column_stack((droll, dpitch, dyaw)) - - accelerometer = [] - gyroscope = [] - for euler_i, deuler_i, acc_i in zip(attitude, dattitude, acceleration): - gyroscope.append(_inv_angular_matrix_from_euler(euler_i).dot(deuler_i)) - accelerometer.append( - _rot_matrix_from_euler(euler_i).T.dot( - acc_i + np.array([0.0, 0.0, -gravity()]) - ) - ) - - return ( - t, - position, - velocity, - attitude, - np.asarray(accelerometer), - np.asarray(gyroscope), - ) - - def benchmark_pure_attitude_beat_202311A( fs: float = 10.24, ) -> tuple[ diff --git a/tests/test_benchmark.py b/tests/test_benchmark.py index 27bb0e8b..6ab0575d 100644 --- a/tests/test_benchmark.py +++ b/tests/test_benchmark.py @@ -95,211 +95,6 @@ def test_signal(self): assert not np.array_equal(d2ydt2, d2ydt2_phase) -class Test__benchmark_helper: - def test_signal_family(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (0.5, 0.5, 0.5, *np.radians((5.0, 5.0, 5.0))) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = tuple(np.radians((0.0, 22.5, 45.0, 67.5, 90.0, 112.5))) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose( - pos[:, 0], amplitude[0] * signal_family(t, phase[0], phase_degrees=False)[0] - ) - np.testing.assert_allclose( - pos[:, 1], amplitude[1] * signal_family(t, phase[1], phase_degrees=False)[0] - ) - np.testing.assert_allclose( - pos[:, 2], amplitude[2] * signal_family(t, phase[2], phase_degrees=False)[0] - ) - np.testing.assert_allclose( - euler[:, 0], - amplitude[3] * signal_family(t, phase[3], phase_degrees=False)[0], - ) - np.testing.assert_allclose( - euler[:, 1], - amplitude[4] * signal_family(t, phase[4], phase_degrees=False)[0], - ) - np.testing.assert_allclose( - euler[:, 2], - amplitude[5] * signal_family(t, phase[5], phase_degrees=False)[0], - ) - - def test_pure_roll(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (0.0, 0.0, 0.0, 0.1, 0.0, 0.0) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose(acc[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(acc[:, 1], -9.80665 * np.sin(euler[:, 0])) - np.testing.assert_allclose(acc[:, 2], -9.80665 * np.cos(euler[:, 0])) - - np.testing.assert_allclose(gyro[:, 0], np.gradient(euler[:, 0], t), atol=0.003) - np.testing.assert_allclose(gyro[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 2], np.zeros_like(t)) - - def test_pure_pitch(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (0.0, 0.0, 0.0, 0.0, 0.1, 0.0) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose(acc[:, 0], 9.80665 * np.sin(euler[:, 1])) - np.testing.assert_allclose(acc[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(acc[:, 2], -9.80665 * np.cos(euler[:, 1])) - - np.testing.assert_allclose(gyro[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 1], np.gradient(euler[:, 1], t), atol=0.003) - np.testing.assert_allclose(gyro[:, 2], np.zeros_like(t)) - - def test_pure_yaw(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (0.0, 0.0, 0.0, 0.0, 0.0, 0.1) - mean = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - phase = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose(acc[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(acc[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(acc[:, 2], -9.80665 * np.ones_like(t)) - - np.testing.assert_allclose(gyro[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 2], np.gradient(euler[:, 2], t), atol=0.003) - - def test_attitude_mean(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - mean = (0.0, 0.0, 0.0, 0.1, 0.2, 0.3) - phase = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose(acc[:, 0], -9.80665 * -np.sin(0.2)) - np.testing.assert_allclose(acc[:, 1], -9.80665 * np.sin(0.1) * np.cos(0.2)) - np.testing.assert_allclose(acc[:, 2], -9.80665 * np.cos(0.1) * np.cos(0.2)) - - np.testing.assert_allclose( - np.sqrt(acc[:, 0] ** 2 + acc[:, 1] ** 2 + acc[:, 2] ** 2), - 9.80665 * np.ones_like(t), - ) - - np.testing.assert_allclose(gyro[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 2], np.zeros_like(t)) - - def test_pure_translation(self): - f_main = 0.1 - f_beat = 0.01 - signal_family = benchmark.BeatSignal(f_main, f_beat) - - duration = 100.0 - fs = 10.0 - amplitude = (1.0, 2.0, 3.0, 0.0, 0.0, 0.0) - mean = (-1.0, -2.0, -3.0, 0.0, 0.0, 0.0) - phase = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - t, pos, vel, euler, acc, gyro = benchmark._benchmark_helper( - duration, amplitude, mean, phase, signal_family, fs - ) - - assert len(t) == int(duration * fs) - assert pos.shape == (len(t), 3) - assert vel.shape == (len(t), 3) - assert euler.shape == (len(t), 3) - assert acc.shape == (len(t), 3) - assert gyro.shape == (len(t), 3) - - np.testing.assert_allclose(pos[:, 0].mean(), -1.0, atol=0.1) - np.testing.assert_allclose(pos[:, 1].mean(), -2.0, atol=0.1) - np.testing.assert_allclose(pos[:, 2].mean(), -3.0, atol=0.1) - - np.testing.assert_allclose(vel[:, 0], np.gradient(pos[:, 0], t), atol=0.005) - np.testing.assert_allclose(vel[:, 1], np.gradient(pos[:, 1], t), atol=0.005) - np.testing.assert_allclose(vel[:, 2], np.gradient(pos[:, 2], t), atol=0.005) - - np.testing.assert_allclose(acc[:, 0], np.gradient(vel[:, 0], t), atol=0.01) - np.testing.assert_allclose(acc[:, 1], np.gradient(vel[:, 1], t), atol=0.01) - np.testing.assert_allclose( - acc[:, 2], -9.80665 + np.gradient(vel[:, 2], t), atol=0.01 - ) - - np.testing.assert_allclose(gyro[:, 0], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 1], np.zeros_like(t)) - np.testing.assert_allclose(gyro[:, 2], np.zeros_like(t)) - - def test_benchmark_pure_attitude_beat_202311A(): signature_signal, _, _ = benchmark.BeatSignal(0.1, 0.01)( np.arange(0.0, 1800.0, 1.0 / 10.24) From bedf7bcf69ade31ea55aabeaa3b5c38d1ad9eb99 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:20:21 +0100 Subject: [PATCH 122/129] refactor beatsignal to use beatdof --- src/smsfusion/benchmark/_benchmark.py | 29 +++++---------------------- 1 file changed, 5 insertions(+), 24 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index fd99bc88..2b2a8d19 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -89,47 +89,28 @@ class BeatSignal(_Signal): def __init__(self, f_main: float, f_beat: float, freq_hz: bool = True) -> None: warn("`BeatSignal`` is deprecated, use ``simulate.BeatDOF`` instead.") - self._f_main = f_main - self._f_beat = f_beat - - if freq_hz: - self._f_main = self._f_main * 2.0 * np.pi - self._f_beat = self._f_beat * 2.0 * np.pi + self._f_main = 2.0 * np.pi * f_main if freq_hz else f_main + self._f_beat = f_beat * 2.0 * np.pi if freq_hz else f_beat def _y(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: """ Generate a unit amplitude beating signal. """ - main = np.cos(self._f_main * t + phase) - beat = np.sin(self._f_beat / 2.0 * t) - y = beat * main + y = BeatDOF(1.0, self._f_main, self._f_beat, phase=phase)._y(t) return y # type: ignore[no-any-return] def _dydt(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: """ Generate the first time derivative of a unit amplitue beating signal. """ - main = np.cos(self._f_main * t + phase) - beat = np.sin(self._f_beat / 2.0 * t) - dmain = -self._f_main * np.sin(self._f_main * t + phase) - dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) - - dydt = dbeat * main + beat * dmain + dydt = BeatDOF(1.0, self._f_main, self._f_beat, phase=phase)._dydt(t) return dydt # type: ignore[no-any-return] def _d2ydt2(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: """ Generate the second time derivative of a unit amplitue beating signal. """ - main = np.cos(self._f_main * t + phase) - beat = np.sin(self._f_beat / 2.0 * t) - dmain = -self._f_main * np.sin(self._f_main * t + phase) - dbeat = self._f_beat / 2.0 * np.cos(self._f_beat / 2.0 * t) - d2main = -((self._f_main) ** 2) * np.cos(self._f_main * t + phase) - d2beat = -((self._f_beat / 2.0) ** 2) * np.sin(self._f_beat / 2.0 * t) - - d2ydt2 = dbeat * dmain + d2beat * main + beat * d2main + dbeat * dmain - + d2ydt2 = BeatDOF(1.0, self._f_main, self._f_beat, phase=phase)._d2ydt2(t) return d2ydt2 # type: ignore[no-any-return] From f8198e71b11ede7f3089dee586fc43be83ab490c Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:39:00 +0100 Subject: [PATCH 123/129] fix default freq_hz False chirp --- src/smsfusion/simulate/_simulate.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index b6e0021a..07f30914 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -315,7 +315,7 @@ def __init__( amp: float = 1.0, f_max: float = 0.25, f_os: float = 0.01, - freq_hz: bool = True, + freq_hz: bool = False, phase: float = 0.0, phase_degrees: bool = False, offset: float = 0.0, From 9b3de55f239ae818c2f29f54f2136d2565ba1328 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:45:07 +0100 Subject: [PATCH 124/129] fix chirp default values --- src/smsfusion/benchmark/_benchmark.py | 20 +++++--------------- src/smsfusion/simulate/_simulate.py | 10 +++++----- tests/test_simulate.py | 2 +- 3 files changed, 11 insertions(+), 21 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index 2b2a8d19..e33bef15 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -142,19 +142,14 @@ class ChirpSignal(_Signal): def __init__(self, f_max: float, f_os: float, freq_hz: bool = True) -> None: warn("`ChirpSignal` is deprecated, use `simulate.ChirpDOF` instead.") - self._f_max = f_max - self._f_os = f_os - - if freq_hz: - self._f_max *= 2.0 * np.pi - self._f_os *= 2.0 * np.pi + self._f_max = 2.0 * np.pi * f_max if freq_hz else f_max + self._f_os = 2.0 * np.pi * f_os if freq_hz else f_os def _y(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: """ Generate a unit amplitude chirp signal with oscillating frequency. """ - phi = 2.0 * self._f_max / self._f_os * np.sin(self._f_os / 2.0 * t) - y = np.sin(phi + phase) + y = ChirpDOF(1.0, self._f_max, self._f_os, phase=phase)._y(t) return y # type: ignore[no-any-return] def _dydt(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: @@ -162,9 +157,7 @@ def _dydt(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: Generate the time derivative of a unit amplitude chirp signal with oscillating frequency. """ - phi = 2.0 * self._f_max / self._f_os * np.sin(self._f_os / 2.0 * t) - dphi = self._f_max * np.cos(self._f_os / 2.0 * t) - dydt = dphi * np.cos(phi + phase) + dydt = ChirpDOF(1.0, self._f_max, self._f_os, phase=phase)._dydt(t) return dydt # type: ignore[no-any-return] def _d2ydt2(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: @@ -172,10 +165,7 @@ def _d2ydt2(self, t: NDArray[np.float64], phase: float) -> NDArray[np.float64]: Generate the second time derivative of a unit amplitude chirp signal with oscillating frequency. """ - phi = 2.0 * self._f_max / self._f_os * np.sin(self._f_os / 2.0 * t) - dphi = self._f_max * np.cos(self._f_os / 2.0 * t) - d2phi = -self._f_max * self._f_os / 2.0 * np.sin(self._f_os / 2.0 * t) - d2ydt2 = -(dphi**2) * np.sin(phi + phase) + d2phi * np.cos(phi + phase) + d2ydt2 = ChirpDOF(1.0, self._f_max, self._f_os, phase=phase)._d2ydt2(t) return d2ydt2 # type: ignore[no-any-return] diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 07f30914..1a330fae 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -296,10 +296,10 @@ class ChirpDOF(DOF): Parameters ---------- f_max : float - The maximum frequency of the chirp signal, y(t). + The maximum frequency of the chirp signal, y(t). Default is 0.25 Hz. f_os : float - The frequency oscillation rate. - freq_hz : bool, default True. + The frequency oscillation rate. Default is 0.01 Hz. + freq_hz : bool, default False. Whether the frequencies, ``f_max`` and ``f_os``, are in Hz or rad/s (default). phase : float, default 0.0 Phase offset of the chirp signal. Default is 0.0. @@ -313,8 +313,8 @@ class ChirpDOF(DOF): def __init__( self, amp: float = 1.0, - f_max: float = 0.25, - f_os: float = 0.01, + f_max: float = 0.5 * np.pi, + f_os: float = 0.02 * np.pi, freq_hz: bool = False, phase: float = 0.0, phase_degrees: bool = False, diff --git a/tests/test_simulate.py b/tests/test_simulate.py index 7beb27fc..4e25ab56 100644 --- a/tests/test_simulate.py +++ b/tests/test_simulate.py @@ -532,7 +532,7 @@ def test__init__default(self): assert isinstance(chirp_dof, DOF) assert chirp_dof._amp == 1.0 - assert chirp_dof._w_max == pytest.approx(2.0 * np.pi * 0.25) + assert chirp_dof._w_max == pytest.approx(np.pi / 2.0) assert chirp_dof._w_os == pytest.approx(2.0 * np.pi * 0.01) assert chirp_dof._phase == pytest.approx(0.0) assert chirp_dof._offset == 0.0 From a17c34656f539c9432454339adfb688f9fb7f2dc Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:46:18 +0100 Subject: [PATCH 125/129] delete obsolete imports --- src/smsfusion/benchmark/_benchmark.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/smsfusion/benchmark/_benchmark.py b/src/smsfusion/benchmark/_benchmark.py index e33bef15..d8dff047 100644 --- a/src/smsfusion/benchmark/_benchmark.py +++ b/src/smsfusion/benchmark/_benchmark.py @@ -6,8 +6,6 @@ import numpy as np from numpy.typing import ArrayLike, NDArray -from smsfusion._ins import gravity -from smsfusion._transforms import _inv_angular_matrix_from_euler, _rot_matrix_from_euler from smsfusion.simulate import BeatDOF, ChirpDOF, IMUSimulator From 4d99a3cae3b1d4b67875d6affcd3260128ef0cb4 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 09:56:06 +0100 Subject: [PATCH 126/129] delete commented out linear ramp --- src/smsfusion/simulate/_simulate.py | 51 ----------------------------- 1 file changed, 51 deletions(-) diff --git a/src/smsfusion/simulate/_simulate.py b/src/smsfusion/simulate/_simulate.py index 1a330fae..5908e786 100644 --- a/src/smsfusion/simulate/_simulate.py +++ b/src/smsfusion/simulate/_simulate.py @@ -363,57 +363,6 @@ def _d2ydt2(self, t: NDArray[np.float64]) -> NDArray[np.float64]: return d2ydt2 # type: ignore[no-any-return] -# class LinearRampUp(DOF): -# """ -# Linear ramp-up wrapper for DOF signals. - -# Parameters -# ---------- -# dof : _DOF -# The DOF signal to wrap with a linear ramp-up. -# t_start : float, default 0.0 -# The start time of the ramp-up in seconds. Default is 0.0, i.e., the ramp-up -# starts immediately. -# ramp_length : float, default 1.0 -# The duration of the ramp-up in seconds. Default is 1.0 second. -# """ - -# def __init__(self, dof: DOF, t_start=0.0, ramp_length=1.0): -# self._dof = dof -# self._t_start = t_start -# self._ramp_length = ramp_length -# self._t_end = t_start + ramp_length - -# def _y_ramp(self, t): -# ramp_up = np.clip((t - self._t_start) / self._ramp_length, 0.0, 1.0) -# return ramp_up - -# def _dydt_ramp(self, t): - -# # dydt_ramp = np.ones_like(t) -# # dydt_ramp = np.where(t < self._t_start, 0.0, dydt_ramp) -# # dydt_ramp = np.where(t > self._t_start + self._ramp_length, 0.0, dydt_ramp) - -# # dydt_ramp = np.where( -# # (t >= self._t_start) & (t <= self._t_start + self._ramp_length), -# # 1.0 / self._ramp_length, -# # 0.0, -# # ) -# # return dydt_ramp - -# def _y(self, t): -# ramp_up = self._ramp_up(t) -# return ramp_up * self._dof._y(t) - -# def _dydt(self, t): -# ramp_up = self._ramp_up(t) -# return ramp_up * self._dof._dydt(t) - -# def _d2ydt2(self, t): -# ramp_up = self._ramp_up(t) -# return ramp_up * self._dof._d2ydt2(t) - - class IMUSimulator: """ IMU simulator. From 96a2335aad7c7d5195f4b8cab95944a61a8e8ad9 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 10:31:00 +0100 Subject: [PATCH 127/129] update quickstart --- docs/user_guide/quickstart.rst | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 8e21368f..790f4041 100644 --- a/docs/user_guide/quickstart.rst +++ b/docs/user_guide/quickstart.rst @@ -26,17 +26,29 @@ data from an IMU sensor, and ideally position and heading data from other aiding sensors. If you do not have access to such data, you can generate synthetic measurements using the code provided here. -Using the ``benchmark`` module, you can generate synthetic 3D motion data with ``smsfusion``. -For example, you can generate beating signals representing position, velocity and -attitude (PVA) degrees of freedom using :func:`~smsfusion.benchmark.benchmark_full_pva_beat_202311A`: +Using the :class:`~smsfusion.simulate.IMUSimulator` class, you can generate synthetic +3D motion data and corresponding IMU accelerometer and gyroscope measurements.. +For example, you can simulate beating motion: + .. code-block:: python - from smsfusion.benchmark import benchmark_full_pva_beat_202311A + from smsfusion.simulate import BeatDOF, IMUSimulator fs = 10.24 # sampling rate in Hz - t, pos, vel, euler, acc, gyro = benchmark_full_pva_beat_202311A(fs) + n = 10_000 # number of samples + sim = IMUSimulator( + pos_x=BeatDOF(0.5, phase=0.0, phase_degrees=True), + pos_y=BeatDOF(0.5, phase=45.0, phase_degrees=True), + pos_z=BeatDOF(0.5, phase=90.0, phase_degrees=True), + alpha=BeatDOF(0.1, phase=135.0, phase_degrees=True), + beta=BeatDOF(0.1, phase=180.0, phase_degrees=True), + gamma=BeatDOF(0.1, phase=225.0, phase_degrees=True), + degrees=False, + ) + + t, pos, vel, euler, acc, gyro = sim(fs, 10_000, degrees=False) head = euler[:, 2] Note that the generated position signals are in meters (m), velocity signals are in meters From 9d6568242ffc5c0fbfee2e1d3ed1d1688a44b753 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 10:51:36 +0100 Subject: [PATCH 128/129] docs --- docs/user_guide/quickstart.rst | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 790f4041..e6ca1811 100644 --- a/docs/user_guide/quickstart.rst +++ b/docs/user_guide/quickstart.rst @@ -26,9 +26,9 @@ data from an IMU sensor, and ideally position and heading data from other aiding sensors. If you do not have access to such data, you can generate synthetic measurements using the code provided here. -Using the :class:`~smsfusion.simulate.IMUSimulator` class, you can generate synthetic -3D motion data and corresponding IMU accelerometer and gyroscope measurements.. -For example, you can simulate beating motion: +Using the :class:`~smsfusion.simulate.IMUSimulator` class in the ``simulate`` module, +you can generate synthetic 3D motion data and corresponding IMU accelerometer and +gyroscope measurements. For example, you can simulate beating motion: .. code-block:: python @@ -36,8 +36,6 @@ For example, you can simulate beating motion: from smsfusion.simulate import BeatDOF, IMUSimulator - fs = 10.24 # sampling rate in Hz - n = 10_000 # number of samples sim = IMUSimulator( pos_x=BeatDOF(0.5, phase=0.0, phase_degrees=True), pos_y=BeatDOF(0.5, phase=45.0, phase_degrees=True), @@ -48,6 +46,7 @@ For example, you can simulate beating motion: degrees=False, ) + fs = 10.24 # sampling rate in Hz t, pos, vel, euler, acc, gyro = sim(fs, 10_000, degrees=False) head = euler[:, 2] From 863a07d78e63532ced18948562d339b08b87111d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Mon, 22 Dec 2025 10:58:17 +0100 Subject: [PATCH 129/129] api ref --- docs/reference/index.rst | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/docs/reference/index.rst b/docs/reference/index.rst index 9a1cd7b5..4229bc52 100644 --- a/docs/reference/index.rst +++ b/docs/reference/index.rst @@ -29,3 +29,8 @@ API reference noise.gauss_markov noise.random_walk noise.white_noise + simulate.BeatDOF + simulate.ChirpDOF + simulate.ConstantDOF + simulate.IMUSimulator + simulate.SineDOF