c4dynamics.sensors.navigation.magnetometer.measure

magnetometer.measure(x_true)

Measure the body-frame geomagnetic field.

The method rotates the reference field mref into the body frame using the attitude entries of x_true (indices 6, 7, 8), then applies the soft-iron matrix, the hard-iron offset and a zero-mean Gaussian noise sample per axis.

Parameters:

x_true (array_like) – True state vector; only the attitude entries x_true[6:9] (roll, pitch, yaw) are used, [rad].

Returns:

numpy.ndarray – Measured body-frame field [mx, my, mz], in the units of field_intensity.

Errors Model

\[z = S_i \cdot [BI](\varphi, \theta, \psi) \cdot m_{ref} + b_i + std \cdot N(0, I_3)\]

The soft-iron matrix \(S_i\) and hard-iron offset \(b_i\) are constant for the magnetometer instance, while the noise is regenerated at every call.

Examples

>>> import c4dynamics as c4d
>>> import numpy as np
>>> np.random.seed(42)
>>> mag_sensor = c4d.sensors.magnetometer(isideal=True)
>>> x = np.zeros(12)
>>> x[8] = 0.5
>>> mag_sensor.measure(x)   
[0.439  -0.24  0.866]