From c2dcd889365edad0a7c8b5e26040beb86f10c1e6 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:15:57 +0200 Subject: [PATCH 1/8] add heading pseudo aiding to VRU --- src/smsfusion/_ins.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index 979e45df..b96f4497 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1297,6 +1297,7 @@ def update( degrees: bool = False, pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]), vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]), + head_var: float = 0.5 ) -> Self: """ Update/correct the VRU's state estimate with pseudo aiding measurements @@ -1321,6 +1322,9 @@ def update( vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] Variance of velocity measurement noise in (m/s)^2. Defaults to standard deviation of 10 m/s, while assuming zero velocity. + head_var : float, default 0.5 + Variance of heading measurement noise in (rad)^2. Defaults to + standard deviation of 0.5 rad, while assuming zero heading. Returns ------- @@ -1335,8 +1339,8 @@ def update( pos_var=pos_var, vel=np.array([0.0, 0.0, 0.0]), vel_var=vel_var, - head=None, - head_var=None, + head=0.0, + head_var=head_var, ) From 16f8d06913382ac4d79f5865019a18cdcc077795 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:18:35 +0200 Subject: [PATCH 2/8] update doc --- docs/user_guide/quickstart.rst | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 8e21368f..018b9ae9 100644 --- a/docs/user_guide/quickstart.rst +++ b/docs/user_guide/quickstart.rst @@ -196,9 +196,7 @@ aiding is sufficient: * Position: 0 m with 1000 m standard deviation * Velocity: 0 m/s with 10 m/s standard deviation - -Note that the heading is not corrected in VRU mode, and the yaw degree of freedom -will thus drift arbitrarily. +* Heading: 0 rad with 0.5 rad standard deviation If you have access to accelerometer and gyroscope data from an IMU sensor, you can estimate the roll and pitch degrees of freedom of a moving body using the From 1f259576ca38ba50e6ef1e85f5f8605b94b9ec07 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:20:41 +0200 Subject: [PATCH 3/8] head degrees false --- src/smsfusion/_ins.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index b96f4497..966120ee 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1341,6 +1341,7 @@ def update( vel_var=vel_var, head=0.0, head_var=head_var, + head_degrees=False, ) From 13389269d19ede5b2e2c96b79cd28a56c8614057 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:23:16 +0200 Subject: [PATCH 4/8] docstring --- src/smsfusion/_ins.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index 966120ee..93219c51 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1301,8 +1301,8 @@ def update( ) -> Self: """ Update/correct the VRU's state estimate with pseudo aiding measurements - (i.e., zero velocity and zero position with corresponding variances), and - project ahead using IMU measurements. + (i.e., zero velocity, zero position and zero heading with corresponding + variances), and project ahead using IMU measurements. Parameters ---------- From dcf3d68d101206cd0c31ad512d7b19539b9e7be0 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:25:52 +0200 Subject: [PATCH 5/8] doc --- docs/user_guide/quickstart.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 018b9ae9..59801e62 100644 --- a/docs/user_guide/quickstart.rst +++ b/docs/user_guide/quickstart.rst @@ -216,9 +216,9 @@ estimate the roll and pitch degrees of freedom of a moving body using the roll_pitch_est = [] for f_i, w_i in zip(acc_imu, gyro_imu): vru.update(f_i, w_i, degrees=False) - roll_pitch_est.append(vru.euler(degrees=False)[:2]) + euler_est.append(vru.euler(degrees=False)) - roll_pitch_est = np.array(roll_pitch_est) + euler_est = np.array(euler_est) Smoothing @@ -259,4 +259,4 @@ additional aiding parameters depending on the type of AINS instance used. smoother.update(f_i, w_i, degrees=False) # Get smoothed roll and pitch estimates - roll_pitch_est = smoother.euler(degrees=False)[:, :2] + euler_est = smoother.euler(degrees=False) From dc6161f3d04b217150c2a2db9050e77603e9ce3d Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:31:42 +0200 Subject: [PATCH 6/8] default aiding 1 rad --- docs/user_guide/quickstart.rst | 2 +- src/smsfusion/_ins.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 59801e62..73c45d8c 100644 --- a/docs/user_guide/quickstart.rst +++ b/docs/user_guide/quickstart.rst @@ -196,7 +196,7 @@ aiding is sufficient: * Position: 0 m with 1000 m standard deviation * Velocity: 0 m/s with 10 m/s standard deviation -* Heading: 0 rad with 0.5 rad standard deviation +* Heading: 0 rad with 1 rad standard deviation If you have access to accelerometer and gyroscope data from an IMU sensor, you can estimate the roll and pitch degrees of freedom of a moving body using the diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index 93219c51..16880453 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1297,7 +1297,7 @@ def update( degrees: bool = False, pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]), vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]), - head_var: float = 0.5 + head_var: float = 1.0, ) -> Self: """ Update/correct the VRU's state estimate with pseudo aiding measurements @@ -1322,9 +1322,9 @@ def update( vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] Variance of velocity measurement noise in (m/s)^2. Defaults to standard deviation of 10 m/s, while assuming zero velocity. - head_var : float, default 0.5 + head_var : float, default 1.0 Variance of heading measurement noise in (rad)^2. Defaults to - standard deviation of 0.5 rad, while assuming zero heading. + standard deviation of 1.0 rad, while assuming zero heading. Returns ------- From a3bbb1c2e72d11eac5f9a40bc5e2cb0f34de177f Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:57:33 +0200 Subject: [PATCH 7/8] docstring --- src/smsfusion/_ins.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index 16880453..cba8285e 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1317,14 +1317,17 @@ def update( degrees : bool, default False Specifies whether the unit of ``w_imu`` are in degrees or radians. pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6] - Variance of position measurement noise in m^2. Defaults to + Variance of position measurement noise in m^2. Defaults to a standard deviation of 1000 m, while assuming zero position. vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] - Variance of velocity measurement noise in (m/s)^2. Defaults to + Variance of velocity measurement noise in (m/s)^2. Defaults to a standard deviation of 10 m/s, while assuming zero velocity. head_var : float, default 1.0 - Variance of heading measurement noise in (rad)^2. Defaults to - standard deviation of 1.0 rad, while assuming zero heading. + Variance of heading measurement noise in (rad)^2. Defaults to a + standard deviation of 1.0 rad, while assuming zero heading. Lower + values can speed up convergence of the gyroscope biases; however, a + too small value may be detrimental to the roll and pitch estimates + if the sensor experiences significant yaw motion. Returns ------- From 27ad3eb0c4f284800190a61addd5176243c4de47 Mon Sep 17 00:00:00 2001 From: "Vegard R. Solum" Date: Fri, 22 Aug 2025 10:57:56 +0200 Subject: [PATCH 8/8] small docstring fix --- src/smsfusion/_ins.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index cba8285e..0b24cf74 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1485,10 +1485,10 @@ def update( Specifies whether the unit of ``head`` and ``head_var`` are in degrees and degrees^2, or radians and radians^2. Default is in radians and radians^2. pos_var : array-like, shape (3,), default [10**6, 10**6, 10**6] - Variance of position measurement noise in m^2. Defaults to + Variance of position measurement noise in m^2. Defaults to a standard deviation of 1000 m, while assuming zero position. vel_var : array-like, shape (3,), default [10**2, 10**2, 10**2] - Variance of velocity measurement noise in (m/s)^2. Defaults to + Variance of velocity measurement noise in (m/s)^2. Defaults to a standard deviation of 10 m/s, while assuming zero velocity. Returns