diff --git a/docs/user_guide/quickstart.rst b/docs/user_guide/quickstart.rst index 8e21368f..73c45d8c 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 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 @@ -218,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 @@ -261,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) diff --git a/src/smsfusion/_ins.py b/src/smsfusion/_ins.py index 979e45df..0b24cf74 100644 --- a/src/smsfusion/_ins.py +++ b/src/smsfusion/_ins.py @@ -1297,11 +1297,12 @@ def update( degrees: bool = False, pos_var: ArrayLike = np.array([1e6, 1e6, 1e6]), vel_var: ArrayLike = np.array([1e2, 1e2, 1e2]), + head_var: float = 1.0, ) -> 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 ---------- @@ -1316,11 +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 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 ------- @@ -1335,8 +1342,9 @@ 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, + head_degrees=False, ) @@ -1477,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