IMU

Contents

IMU#

class c4dynamics.sensors.navigation.imu(gyro_std=0.01, acc_std=0.05, gyro_bias=None, acc_bias=None, g=9.81, isideal=False, dt=0.005)[source]#

Inertial measurement unit — gyroscope and accelerometer.

The imu class models a strapdown gyroscope and accelerometer pair. The gyroscope measures body rates [p, q, r]. The accelerometer measures the full body-frame specific force [ax, ay, az].

An imu instance is a state whose state vector is the last measured sample, X = [ax, ay, az, p, q, r]. This gives imu the store() and data() methods for free (measure uses store internally, see below).

Parameters:

  • gyro_std (float, optional) – Standard deviation of the gyroscope noise, [rad/s]. Defaults 0.01.

  • acc_std (float, optional) – Standard deviation of the accelerometer noise, [m/s²]. Defaults 0.05.

  • gyro_bias (array_like, optional) – Constant gyroscope bias [bp, bq, br], [rad/s]. Defaults [0, 0, 0].

  • acc_bias (array_like, optional) – Constant accelerometer bias [bax, bay, baz], [m/s²]. Defaults [0, 0, 0].

  • g (float, optional) – Gravitational acceleration, [m/s²]. Defaults 9.81.

  • isideal (bool, optional) – A flag indicating whether the errors model is off. Defaults False.

  • dt (float, optional) – Fallback timestep, [s], used for the accelerometer’s finite-difference inertial term when consecutive calls to measure don’t carry increasing t values. Defaults 0.005 (\(200Hz\)).

See also

ekf, gps, magnetometer

Functionality

At each sample the imu returns rate and acceleration measurements based on the true state of a rigid body. Let the true state be:

\[X = [x, y, z, v_x, v_y, v_z, \varphi, \theta, \psi, p, q, r]^T\]

Where:

  • \(x, y, z\) are the inertial position coordinates

  • \(v_x, v_y, v_z\) are the inertial velocity coordinates

  • \(\varphi, \theta, \psi\) are the Euler angles (roll, pitch, yaw)

  • \(p, q, r\) are the body rates about the roll, pitch, yaw axes

The gyroscope measurement is the last three states directly:

\[[p, q, r]_{meas} = [p, q, r] + bias_{gyro} + noise_{gyro}\]

The accelerometer measures the full body-frame specific force, i.e. gravity reaction plus the vehicle’s own coordinate acceleration:

\[[a_x, a_y, a_z]_{ideal} = [BI] \cdot \big(\dot{v} + [0,\ 0,\ g]^T\big)\]

where \([BI]\) is the body-from-inertial DCM and \(\dot{v}\) is the inertial-velocity derivative. Since measure is given only the current true state, \(\dot{v}\) is approximated by a finite difference against the previous call’s true state:

\[\dot{v} \approx {v(t) - v(t_{prev}) \over t - t_{prev}}\]

which is why measure keeps the previous sample as an internal attribute (t_prev, x_prev) rather than asking the caller for it: the first call to measure (no previous sample yet) therefore reports the gravity term alone.

Errors Model

  • Bias: a constant offset, set at construction through gyro_bias / acc_bias and unchanged between measurements.

  • Noise: a zero-mean Gaussian sample, drawn independently at every call to measure, with standard deviation gyro_std / acc_std.

The errors model can be disabled by applying isideal = True at the imu construction stage: the gyroscope and accelerometer standard deviations and biases are then muted (forced to zero), regardless of the gyro_std / acc_std / gyro_bias / acc_bias arguments, and measure returns the noise-free, bias-free truth.

Construction

An imu instance is created by making a direct call to the constructor:

>>> imu_sensor = c4d.sensors.imu()

Initialization does not require any mandatory arguments.

Examples

Import required packages:

>>> import c4dynamics as c4d
>>> import numpy as np

Ideal imu

An ideal imu can be created by muting the errors model:

>>> imu_ideal = c4d.sensors.imu(isideal = True)
>>> rb = c4d.rigidbody(theta = -0.1, p = 0.2, q = -0.1, r = 0.05)
>>> ax, ay, az, p, q, r = imu_ideal.measure(rb)
>>> np.array([p, q, r]) 
[0.2  -0.1  0.05]
>>> ax 
-0.979...
>>> az 
-9.760...

Non-ideal imu

>>> np.random.seed(100)
>>> imu_sensor = c4d.sensors.imu(gyro_std = 0.01, acc_std = 0.05)
>>> rb = c4d.rigidbody()
>>> imu_sensor.measure(rb) 
(-0.012..., 0.049..., -9.784..., -0.017..., 0.003..., 0.011...)

Store

Passing store = True stores the sampled [ax, ay, az, p, q, r] along with the given timestamp; the histories are then available through data():

>>> np.random.seed(200)
>>> imu_sensor = c4d.sensors.imu(isideal = True)
>>> rb = c4d.rigidbody()
>>> for t in np.arange(0, 0.02, 0.005):
...     rb.p = 0.1 * t
...     imu_sensor.measure(rb, t = t, store = True) 
>>> imu_sensor.data('p')
(array([0.   , 0.005, 0.01 , 0.015]), array([0.    , 0.0005, 0.001 , 0.0015]))

Demo

The built-in demo method provides a compact demonstration of the imu errors model and plots the true and measured rates and accelerations:

>>> fig = c4d.sensors.imu.demo(show = True)
../_images/imu_demo.png

The same demonstration can be run without displaying the figure by using show = False.

Methods

imu.measure(rb[, t, store])

Measures body rates and specific acceleration of a rigid body.
Contents