-
Notifications
You must be signed in to change notification settings - Fork 8
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
bb17602
commit 58d8cde
Showing
2 changed files
with
30 additions
and
21 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,9 +2,10 @@ | |
| | File: imu.py | ||
| | Author: Marcelo Jacinto ([email protected]) | ||
| | License: BSD-3-Clause. Copyright (c) 2023, Marcelo Jacinto. All rights reserved. | ||
| | Description: Simulates an imu. Based on the implementation provided in PX4 stil_gazebo (https://github.com/PX4/PX4-SITL_gazebo) | ||
| | Description: Simulates an imu. Based on the implementation provided in PX4 stil_gazebo | ||
| (https://github.com/PX4/PX4-SITL_gazebo) | ||
| """ | ||
| __all__ = ["IMU"] | ||
| __all__ = ["Imu"] | ||
|
|
||
| import numpy as np | ||
| from scipy.spatial.transform import Rotation | ||
|
|
@@ -15,14 +16,15 @@ | |
| from pegasus.simulator.logic.sensors.geo_mag_utils import GRAVITY_VECTOR | ||
|
|
||
|
|
||
| class IMU(Sensor): | ||
| """The class that implements the IMU sensor. This class inherits the base class Sensor. | ||
| class Imu(Sensor): | ||
| """The class that implements the Imu sensor. This class inherits the base class Sensor. | ||
| """ | ||
| def __init__(self, config={}): | ||
| """Initialize the IMU class | ||
| def __init__(self, config=None): | ||
| """Initialize the Imu class | ||
| Args: | ||
| config (dict): A Dictionary that contains all teh parameters for configuring the IMU - it can be empty or only have some of the parameters used by the IMU. | ||
| config (dict): A Dictionary that contains all teh parameters for configuring the Imu - it can be empty or | ||
| only have some of the parameters used by the Imu. | ||
| Examples: | ||
| The dictionary default parameters are | ||
|
|
@@ -42,7 +44,7 @@ def __init__(self, config={}): | |
| """ | ||
|
|
||
| # Initialize the Super class "object" attributes | ||
| super().__init__(sensor_type="IMU", update_rate=config.get("update_rate", 250.0)) | ||
| super().__init__(sensor_type="Imu", update_rate=config.get("update_rate", 250.0)) | ||
|
|
||
| # Orientation noise constant | ||
| self._orientation_noise: float = 0.0 | ||
|
|
@@ -66,7 +68,7 @@ def __init__(self, config={}): | |
| # Auxiliar variable used to compute the linear acceleration of the vehicle | ||
| self._prev_linear_velocity = np.zeros((3,)) | ||
|
|
||
| # Save the current state measured by the IMU | ||
| # Save the current state measured by the Imu | ||
| self._state = { | ||
| "orientation": np.array([1.0, 0.0, 0.0, 0.0]), | ||
| "angular_velocity": np.array([0.0, 0.0, 0.0]), | ||
|
|
@@ -82,12 +84,13 @@ def state(self): | |
|
|
||
| @Sensor.update_at_rate | ||
| def update(self, state: State, dt: float): | ||
| """Method that implements the logic of an IMU. In this method we start by generating the random walk of the | ||
| gyroscope. This value is then added to the real angular velocity of the vehicle (FLU relative to ENU inertial frame | ||
| expressed in FLU body frame). The same logic is followed for the accelerometer and the accelerations. After this step, | ||
| the angular velocity is rotated such that it expressed a FRD body frame, relative to a NED inertial frame, expressed | ||
| in the FRD body frame. Additionally, the acceleration is also rotated, such that it becomes expressed in the body | ||
| FRD frame of the vehicle. This sensor outputs data that follows the PX4 adopted standard. | ||
| """Method that implements the logic of an Imu. In this method we start by generating the random walk of the | ||
| gyroscope. This value is then added to the real angular velocity of the vehicle (FLU relative to ENU inertial | ||
| frame expressed in FLU body frame). The same logic is followed for the accelerometer and the accelerations. | ||
| After this step, the angular velocity is rotated such that it expressed a FRD body frame, relative to a NED | ||
| inertial frame, expressed in the FRD body frame. Additionally, the acceleration is also rotated, such that it | ||
| becomes expressed in the body FRD frame of the vehicle. This sensor outputs data that follows the PX4 adopted | ||
| standard. | ||
| Args: | ||
| state (State): The current state of the vehicle. | ||
|
|
@@ -117,10 +120,10 @@ def update(self, state: State, dt: float): | |
| self._gyroscope_bias[i] = phi_g_d * self._gyroscope_bias[i] + sigma_b_g_d * np.random.randn() | ||
| angular_velocity[i] = state.angular_velocity[i] + sigma_g_d * np.random.randn() + self._gyroscope_bias[i] | ||
|
|
||
| # Accelerometer terms | ||
| # Accelerometer terms. | ||
| tau_a: float = self._accelerometer_bias_correlation_time | ||
|
|
||
| # Discrete-time standard deviation equivalent to an "integrating" sampler with integration time dt | ||
| # Discrete-time standard deviation equivalent to an "integrating" sampler with integration time dt. | ||
| sigma_a_d: float = 1.0 / np.sqrt(dt) * self._accelerometer_noise_density | ||
| sigma_b_a: float = self._accelerometer_random_walk | ||
|
|
||
|
|
@@ -130,14 +133,16 @@ def update(self, state: State, dt: float): | |
| # Compute state-transition. | ||
| phi_a_d: float = np.exp(-1.0 / tau_a * dt) | ||
|
|
||
| # Compute the linear acceleration from diferentiating the velocity of the vehicle expressed in the inertial frame | ||
| # Compute the linear acceleration from diferentiating the velocity of the vehicle expressed in the inertial | ||
| # frame. | ||
| linear_acceleration_inertial = (state.linear_velocity - self._prev_linear_velocity) / dt | ||
| linear_acceleration_inertial = linear_acceleration_inertial - GRAVITY_VECTOR | ||
|
|
||
| # Update the previous linear velocity for the next computation | ||
| self._prev_linear_velocity = state.linear_velocity | ||
|
|
||
| # Compute the linear acceleration of the body frame, with respect to the inertial frame, expressed in the body frame | ||
| # Compute the linear acceleration of the body frame, with respect to the inertial frame, expressed in the body | ||
| # frame. | ||
| linear_acceleration = np.array(Rotation.from_quat(state.attitude).inv().apply(linear_acceleration_inertial)) | ||
|
|
||
| # Simulate the accelerometer noise processes and add them to the true linear aceleration values | ||
|
|
@@ -150,7 +155,7 @@ def update(self, state: State, dt: float): | |
| # TODO - Add small "noisy" to the attitude | ||
|
|
||
| # -------------------------------------------------------------------------------------------- | ||
| # Apply rotations such that we express the IMU data according to the FRD body frame convention | ||
| # Apply rotations such that we express the Imu data according to the FRD body frame convention | ||
| # -------------------------------------------------------------------------------------------- | ||
|
|
||
| # Convert the orientation to the FRD-NED standard | ||
|
|
||