Skip to content

Commit

Permalink
Bring imu code from upstream (#12)
Browse files Browse the repository at this point in the history 
* update __init__.py

* bring 'geo_msg_utils.py' and 'imu.py' files

Brings the files from "https://github.com/AuTURBO/StrideSim"
There are no modification with 'pegasus.simulator'.

* sensors: apply pylint rules

---------

Co-authored-by: jinwon kim <[email protected]>
  • Loading branch information
@harderthan @mqjinwon
harderthan and mqjinwon authored Jan 23, 2024
1 parent ecbff6d commit 806d876
Show file tree
Hide file tree
Showing 4 changed files with 335 additions and 1 deletion.
2 changes: 1 addition & 1 deletion exts/stride.simulator/stride/simulator/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -1 +1 @@
from .extension import *
from .extension import StrideSimulatorExtension
1 change: 1 addition & 0 deletions exts/stride.simulator/stride/simulator/vehicles/__init__.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
"""PlACEHOLDER"""
153 changes: 153 additions & 0 deletions exts/stride.simulator/stride/simulator/vehicles/sensors/geo_mag_utils.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
"""
| File: geo_mag_utils.py
| Description: Provides utilities for computing latitude, longitude, and magnetic strength
given the position of the vehicle in the simulated world. These computations and table constants are in agreement
with the PX4 stil_gazebo implementation (https://github.com/PX4/PX4-SITL_gazebo). Therefore, PX4 should behave similarly
to a gazebo-based simulation.
"""
import numpy as np

# Declare which functions are visible from this file
__all__ = ["get_mag_declination", "get_mag_inclination", "get_mag_strength", "reprojection", "GRAVITY_VECTOR"]

# --------------------------------------------------------------------
# Magnetic field data from WMM2018 (10^5xnanoTesla (N, E D) n-frame )
# --------------------------------------------------------------------

# Declination data in degrees
# pylint: disable=line-too-long, for the sake of readability.
DECLINATION_TABLE = [
[ 47,46,45,43,42,41,39,37,33,29,23,16,10,4,-1,-6,-10,-15,-20,-27,-34,-42,-49,-56,-62,-67,-72,-74,-75,-73,-61,-22,26,42,47,48,47 ],
[ 31,31,31,30,30,30,30,29,27,24,18,11,3,-4,-9,-13,-15,-18,-21,-27,-33,-40,-47,-52,-56,-57,-56,-52,-44,-30,-14,2,14,22,27,30,31 ],
[ 22,23,23,23,22,22,22,23,22,19,13,5,-4,-12,-17,-20,-22,-22,-23,-25,-30,-36,-41,-45,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,22 ],
[ 17,17,17,18,17,17,17,17,16,13,8,-1,-10,-18,-22,-25,-26,-25,-22,-20,-21,-25,-29,-32,-31,-28,-23,-16,-9,-3,0,4,7,11,14,16,17 ],
[ 13,13,14,14,14,13,13,12,11,9,3,-5,-14,-20,-24,-25,-24,-21,-17,-12,-9,-11,-14,-17,-18,-16,-12,-8,-3,-0,1,3,6,8,11,12,13 ],
[ 11,11,11,11,11,10,10,10,9,6,-0,-8,-15,-21,-23,-22,-19,-15,-10,-5,-2,-2,-4,-7,-9,-8,-7,-4,-1,1,1,2,4,7,9,10,11 ],
[ 10,9,9,9,9,9,9,8,7,3,-3,-10,-16,-20,-20,-18,-14,-9,-5,-2,1,2,0,-2,-4,-4,-3,-2,-0,0,0,1,3,5,7,9,10 ],
[ 9,9,9,9,9,9,9,8,6,1,-4,-11,-16,-18,-17,-14,-10,-5,-2,-0,2,3,2,0,-1,-2,-2,-1,-0,-1,-1,-1,1,3,6,8,9 ],
[ 8,9,9,10,10,10,10,8,5,0,-6,-12,-15,-16,-15,-11,-7,-4,-1,1,3,4,3,2,1,0,-0,-0,-1,-2,-3,-4,-2,0,3,6,8 ],
[ 7,9,10,11,12,12,12,9,5,-1,-7,-13,-15,-15,-13,-10,-6,-3,0,2,3,4,4,4,3,2,1,0,-1,-3,-5,-6,-6,-3,0,4,7 ],
[ 5,8,11,13,14,15,14,11,5,-2,-9,-15,-17,-16,-13,-10,-6,-3,0,3,4,5,6,6,6,5,4,2,-1,-5,-8,-9,-9,-6,-3,1,5 ],
[ 3,8,11,15,17,17,16,12,5,-4,-12,-18,-19,-18,-16,-12,-8,-4,-0,3,5,7,9,10,10,9,7,4,-1,-6,-10,-12,-12,-9,-5,-1,3 ],
[ 3,8,12,16,19,20,18,13,4,-8,-18,-24,-25,-23,-20,-16,-11,-6,-1,3,7,11,14,16,17,17,14,8,-0,-8,-13,-15,-14,-11,-7,-2,3 ]]
# pylint: disable=line-too-long, for the sake of readability.

# Inclination data in degrees
# pylint: disable=line-too-long, for the sake of readability.
INCLINATION_TABLE = [
[ -78,-76,-74,-72,-70,-68,-65,-63,-60,-57,-55,-54,-54,-55,-56,-57,-58,-59,-59,-59,-59,-60,-61,-63,-66,-69,-73,-76,-79,-83,-86,-87,-86,-84,-82,-80,-78 ],
[ -72,-70,-68,-66,-64,-62,-60,-57,-54,-51,-49,-48,-49,-51,-55,-58,-60,-61,-61,-61,-60,-60,-61,-63,-66,-69,-72,-76,-78,-80,-81,-80,-79,-77,-76,-74,-72 ],
[ -64,-62,-60,-59,-57,-55,-53,-50,-47,-44,-41,-41,-43,-47,-53,-58,-62,-65,-66,-65,-63,-62,-61,-63,-65,-68,-71,-73,-74,-74,-73,-72,-71,-70,-68,-66,-64 ],
[ -55,-53,-51,-49,-46,-44,-42,-40,-37,-33,-30,-30,-34,-41,-48,-55,-60,-65,-67,-68,-66,-63,-61,-61,-62,-64,-65,-66,-66,-65,-64,-63,-62,-61,-59,-57,-55 ],
[ -42,-40,-37,-35,-33,-30,-28,-25,-22,-18,-15,-16,-22,-31,-40,-48,-55,-59,-62,-63,-61,-58,-55,-53,-53,-54,-55,-55,-54,-53,-51,-51,-50,-49,-47,-45,-42 ],
[ -25,-22,-20,-17,-15,-12,-10,-7,-3,1,3,2,-5,-16,-27,-37,-44,-48,-50,-50,-48,-44,-41,-38,-38,-38,-39,-39,-38,-37,-36,-35,-35,-34,-31,-28,-25 ],
[ -5,-2,1,3,5,8,10,13,16,20,21,19,12,2,-10,-20,-27,-30,-30,-29,-27,-23,-19,-17,-17,-17,-18,-18,-17,-16,-16,-16,-16,-15,-12,-9,-5 ],
[ 15,18,21,22,24,26,29,31,34,36,37,34,28,20,10,2,-3,-5,-5,-4,-2,2,5,7,8,7,7,6,7,7,7,6,5,6,8,11,15 ],
[ 31,34,36,38,39,41,43,46,48,49,49,46,42,36,29,24,20,19,20,21,23,25,28,30,30,30,29,29,29,29,28,27,25,25,26,28,31 ],
[ 43,45,47,49,51,53,55,57,58,59,59,56,53,49,45,42,40,40,40,41,43,44,46,47,47,47,47,47,47,47,46,44,42,41,40,42,43 ],
[ 53,54,56,57,59,61,64,66,67,68,67,65,62,60,57,55,55,54,55,56,57,58,59,59,60,60,60,60,60,60,59,57,55,53,52,52,53 ],
[ 62,63,64,65,67,69,71,73,75,75,74,73,70,68,67,66,65,65,65,66,66,67,68,68,69,70,70,71,71,70,69,67,65,63,62,62,62 ],
[ 71,71,72,73,75,77,78,80,81,81,80,79,77,76,74,73,73,73,73,73,73,74,74,75,76,77,78,78,78,78,77,75,73,72,71,71,71 ]]
# pylint: disable=line-too-long, for the sake of readability.

# Strength data in centi-Tesla
STRENGTH_TABLE = [
[ 62,60,58,56,54,52,49,46,43,41,38,36,34,32,31,31,30,30,30,31,33,35,38,42,46,51,55,59,62,64,66,67,67,66,65,64,62 ],
[ 59,56,54,52,50,47,44,41,38,35,32,29,28,27,26,26,26,25,25,26,28,30,34,39,44,49,54,58,61,64,65,66,65,64,63,61,59 ],
[ 54,52,49,47,45,42,40,37,34,30,27,25,24,24,24,24,24,24,24,24,25,28,32,37,42,48,52,56,59,61,62,62,62,60,59,56,54 ],
[ 49,47,44,42,40,37,35,33,30,28,25,23,22,23,23,24,25,25,26,26,26,28,31,36,41,46,51,54,56,57,57,57,56,55,53,51,49 ],
[ 43,41,39,37,35,33,32,30,28,26,25,23,23,23,24,25,26,28,29,29,29,30,32,36,40,44,48,51,52,52,51,51,50,49,47,45,43 ],
[ 38,36,35,33,32,31,30,29,28,27,26,25,24,24,25,26,28,30,31,32,32,32,33,35,38,42,44,46,47,46,45,45,44,43,41,40,38 ],
[ 34,33,32,32,31,31,31,30,30,30,29,28,27,27,27,28,29,31,32,33,33,33,34,35,37,39,41,42,43,42,41,40,39,38,36,35,34 ],
[ 33,33,32,32,33,33,34,34,35,35,34,33,32,31,30,30,31,32,33,34,35,35,36,37,38,40,41,42,42,41,40,39,37,36,34,33,33 ],
[ 34,34,34,35,36,37,39,40,41,41,40,39,37,35,35,34,35,35,36,37,38,39,40,41,42,43,44,45,45,45,43,41,39,37,35,34,34 ],
[ 37,37,38,39,41,42,44,46,47,47,46,45,43,41,40,39,39,40,41,41,42,43,45,46,47,48,49,50,50,50,48,46,43,41,39,38,37 ],
[ 42,42,43,44,46,48,50,52,53,53,52,51,49,47,45,45,44,44,45,46,46,47,48,50,51,53,54,55,56,55,54,52,49,46,44,43,42 ],
[ 48,48,49,50,52,53,55,56,57,57,56,55,53,51,50,49,48,48,48,49,49,50,51,53,55,56,58,59,60,60,58,56,54,52,50,49,48 ],
[ 54,54,54,55,56,57,58,58,59,58,58,57,56,54,53,52,51,51,51,51,52,53,54,55,57,58,60,61,62,61,61,59,58,56,55,54,54 ]]

SAMPLING_RES = 10.0
SAMPLING_MIN_LAT = -60 # deg
SAMPLING_MAX_LAT = 60 # deg
SAMPLING_MIN_LON = -180 # deg
SAMPLING_MAX_LON = 180 # deg

EARTH_RADIUS = 6353000.0 # meters

# Gravity vector expressed in ENU
GRAVITY_VECTOR = np.array([0.0, 0.0, -9.80665]) # m/s^2


def get_lookup_table_index(val: int, min: int, max: int):

# for the rare case of hitting the bounds exactly
# the rounding logic wouldn't fit, so enforce it.
# limit to table bounds - required for maxima even when table spans full globe range
# limit to (table bounds - 1) because bilinear interpolation requires checking (index + 1)
val = np.clip(val, min, max - SAMPLING_RES)
return int((-min + val) / SAMPLING_RES)


def get_table_data(lat: float, lon: float, table):

# If the values exceed valid ranges, return zero as default
# as we have no way of knowing what the closest real value
# would be.
if lat < -90.0 or lat > 90.0 or lon < -180.0 or lon > 180.0:
return 0.0

# round down to nearest sampling resolution
min_lat = int(lat / SAMPLING_RES) * SAMPLING_RES
min_lon = int(lon / SAMPLING_RES) * SAMPLING_RES

# find index of nearest low sampling point
min_lat_index = get_lookup_table_index(min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT)
min_lon_index = get_lookup_table_index(min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON)

data_sw = table[min_lat_index][min_lon_index]
data_se = table[min_lat_index][min_lon_index + 1]
data_ne = table[min_lat_index + 1][min_lon_index + 1]
data_nw = table[min_lat_index + 1][min_lon_index]

# perform bilinear interpolation on the four grid corners
lat_scale = np.clip((lat - min_lat) / SAMPLING_RES, 0.0, 1.0)
lon_scale = np.clip((lon - min_lon) / SAMPLING_RES, 0.0, 1.0)

data_min = lon_scale * (data_se - data_sw) + data_sw
data_max = lon_scale * (data_ne - data_nw) + data_nw

return lat_scale * (data_max - data_min) + data_min


def get_mag_declination(latitude: float, longitude: float):
return get_table_data(latitude, longitude, DECLINATION_TABLE)


def get_mag_inclination(latitude: float, longitude: float):
return get_table_data(latitude, longitude, INCLINATION_TABLE)


def get_mag_strength(latitude: float, longitude: float):
return get_table_data(latitude, longitude, STRENGTH_TABLE)


def reprojection(position: np.ndarray, origin_lat=-999, origin_long=-999):
"""
Compute the latitude and longitude coordinates from a local position
"""

# reproject local position to gps coordinates
x_rad: float = position[1] / EARTH_RADIUS # north
y_rad: float = position[0] / EARTH_RADIUS # east
c: float = np.sqrt(x_rad * x_rad + y_rad * y_rad)
sin_c: float = np.sin(c)
cos_c: float = np.cos(c)

if c != 0.0:
latitude_rad = np.arcsin(cos_c * np.sin(origin_lat) + (x_rad * sin_c * np.cos(origin_lat)) / c)
longitude_rad = origin_long + np.arctan2(y_rad * sin_c, c * np.cos(origin_lat) * cos_c - x_rad * np.sin(origin_lat) * sin_c)
else:
latitude_rad = origin_lat
longitude_rad = origin_long

return latitude_rad, longitude_rad
180 changes: 180 additions & 0 deletions exts/stride.simulator/stride/simulator/vehicles/sensors/imu.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,180 @@
"""
| 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)
"""
__all__ = ["Imu"]

import numpy as np
from scipy.spatial.transform import Rotation

from pegasus.simulator.logic.state import State
from pegasus.simulator.logic.sensors import Sensor
from pegasus.simulator.logic.rotations import rot_FLU_to_FRD, rot_ENU_to_NED
from pegasus.simulator.logic.sensors.geo_mag_utils import GRAVITY_VECTOR

# TODO - test comment

class Imu(Sensor):
"""The class that implements the Imu sensor. This class inherits the base class Sensor.
"""
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.
Examples:
The dictionary default parameters are
>>> {"gyroscope": {
>>> "noise_density": 2.0 * 35.0 / 3600.0 / 180.0 * pi,
>>> "random_walk": 2.0 * 4.0 / 3600.0 / 180.0 * pi,
>>> "bias_correlation_time": 1.0e3,
>>> "turn_on_bias_sigma": 0.5 / 180.0 * pi},
>>> "accelerometer": {
>>> "noise_density": 2.0 * 2.0e-3,
>>> "random_walk": 2.0 * 3.0e-3,
>>> "bias_correlation_time": 300.0,
>>> "turn_on_bias_sigma": 20.0e-3 * 9.8
>>> },
>>> "update_rate": 1.0} # Hz
"""

# Initialize the Super class "object" attributes
super().__init__(sensor_type="Imu", update_rate=config.get("update_rate", 250.0))

# Orientation noise constant
self._orientation_noise: float = 0.0

# Gyroscope noise constants
self._gyroscope_bias: np.ndarray = np.zeros((3,))
gyroscope_config = config.get("gyroscope", {})
self._gyroscope_noise_density = gyroscope_config.get("noise_density", 0.0003393695767766752)
self._gyroscope_random_walk = gyroscope_config.get("random_walk", 3.878509448876288E-05)
self._gyroscope_bias_correlation_time = gyroscope_config.get("bias_correlation_time", 1.0E3)
self._gyroscope_turn_on_bias_sigma = gyroscope_config.get("turn_on_bias_sigma", 0.008726646259971648)

# Accelerometer noise constants
self._accelerometer_bias: np.ndarray = np.zeros((3,))
accelerometer_config = config.get("accelerometer", {})
self._accelerometer_noise_density = accelerometer_config.get("noise_density", 0.004)
self._accelerometer_random_walk = accelerometer_config.get("random_walk", 0.006)
self._accelerometer_bias_correlation_time = accelerometer_config.get("bias_correlation_time", 300.0)
self._accelerometer_turn_on_bias_sigma = accelerometer_config.get("turn_on_bias_sigma", 0.196)

# 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
self._state = {
"orientation": np.array([1.0, 0.0, 0.0, 0.0]),
"angular_velocity": np.array([0.0, 0.0, 0.0]),
"linear_acceleration": np.array([0.0, 0.0, 0.0]),
}

@property
def state(self):
"""
(dict) The 'state' of the sensor, i.e. the data produced by the sensor at any given point in time
"""
return self._state

@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.
Args:
state (State): The current state of the vehicle.
dt (float): The time elapsed between the previous and current function calls (s).
Returns:
(dict) A dictionary containing the current state of the sensor (the data produced by the sensor)
"""

# Gyroscopic terms
tau_g: float = self._accelerometer_bias_correlation_time

# Discrete-time standard deviation equivalent to an "integrating" sampler with integration time dt
sigma_g_d: float = 1 / np.sqrt(dt) * self._gyroscope_noise_density
sigma_b_g: float = self._gyroscope_random_walk

# Compute exact covariance of the process after dt [Maybeck 4-114]
sigma_b_g_d: float = np.sqrt(-sigma_b_g * sigma_b_g * tau_g / 2.0 * (np.exp(-2.0 * dt / tau_g) - 1.0))

# Compute state-transition
phi_g_d: float = np.exp(-1.0 / tau_g * dt)

# Simulate gyroscope noise processes and add them to the true angular rate.
angular_velocity: np.ndarray = np.zeros((3,))

for i in range(3):
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.
tau_a: float = self._accelerometer_bias_correlation_time

# 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

# Compute exact covariance of the process after dt [Maybeck 4-114].
sigma_b_a_d: float = np.sqrt(-sigma_b_a * sigma_b_a * tau_a / 2.0 * (np.exp(-2.0 * dt / tau_a) - 1.0))

# 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.
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.
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
for i in range(3):
self._accelerometer_bias[i] = phi_a_d * self._accelerometer_bias[i] + sigma_b_a_d * np.random.rand()
linear_acceleration[i] = (
linear_acceleration[i] + sigma_a_d * np.random.randn()
) #+ self._accelerometer_bias[i]

# TODO - Add small "noisy" to the attitude

# --------------------------------------------------------------------------------------------
# Apply rotations such that we express the Imu data according to the FRD body frame convention
# --------------------------------------------------------------------------------------------

# Convert the orientation to the FRD-NED standard
attitude_flu_enu = Rotation.from_quat(state.attitude)
attitude_frd_enu = attitude_flu_enu * rot_FLU_to_FRD
attitude_frd_ned = rot_ENU_to_NED * attitude_frd_enu

# Convert the angular velocity from FLU to FRD standard
angular_velocity_frd = rot_FLU_to_FRD.apply(angular_velocity)

# Convert the linear acceleration in the body frame from FLU to FRD standard
linear_acceleration_frd = rot_FLU_to_FRD.apply(linear_acceleration)

# Add the values to the dictionary and return it
self._state = {
"orientation": attitude_frd_ned.as_quat(),
"angular_velocity": angular_velocity_frd,
"linear_acceleration": linear_acceleration_frd,
}

return self._state

0 comments on commit 806d876

Please sign in to comment.