Mahony Filter¶
Implementation of Mahony filter block compatible with gym_jiminy reinforcement learning pipeline environment design.
- gym_jiminy.common.blocks.mahony_filter.mahony_filter(q, omega, cf, gyro, acc, bias_hat, kp, ki, dt)[source]¶
Attitude Estimation using Mahony filter.
Note
This method is fully vectorized, which means that multiple IMU signals can be processed at once. The first dimension corresponds to individual IMU data.
- Parameters:
q (ndarray) – Current orientation estimate as a quaternion to update in-place. The twist part of its twist-after-swing decomposition may have been removed.
omega (ndarray) – Pre-allocated memory that will be updated to store the estimate of the angular velocity in local frame (unbiased).
cf (ndarray) – Pre-allocated memory that will be updated to store the value of the complementary filter in local frame.
gyro (ndarray) – Sample of tri-axial Gyroscope in rad/s. It corresponds to the angular velocity in local frame.
acc (ndarray) – Sample of tri-axial Accelerometer in m/s^2. It corresponds to classical (as opposed to spatial) acceleration of the IMU in local frame minus gravity.
bias_hat (ndarray) – Current gyroscope bias estimate to update in-place.
kp (float) – Proportional gain used for gyro-accel sensor fusion.
ki (float) – Integral gain used for gyro bias estimate.
dt (float) – Time step, in seconds, between consecutive Quaternions.
- Return type:
None
- class gym_jiminy.common.blocks.mahony_filter.MahonyFilter(name, env, *, ignore_twist=False, exact_init=True, kp=1.0, ki=0.1, compute_rpy=False, update_ratio=1)[source]¶
Bases:
BaseObserverBlock[Dict[str,ndarray],Dict[str,ndarray],BaseObs,BaseAct]Mahony’s Nonlinear Complementary Filter on SO(3).
This observer estimate the 3D orientation of all the IMU of the robot from the Gyrometer and Accelerometer measurements, i.e. the angular velocity in local frame and the linear acceleration minus gravity in local frame.
See also
Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin “Nonlinear Complementary Filters on the Special Orthogonal Group” IEEE Transactions on Automatic Control, Institute of Electrical and Electronics Engineers, 2008, 53 (5), pp.1203-1217: https://hal.archives-ouvertes.fr/hal-00488376/document
Note
The feature space of this observer is a dictionary storing quaternion estimates under key ‘quat’, optionally, their corresponding Yaw-Pitch-Roll Euler angles representations under key ‘rpy’ if compute_rpy=True, and finally, the angular velocity in local frame estimates under key ‘omega’. Both leaves are 2D array of shape (N, M), where N is the number of components of the representation while M is the number of IMUs of the robot. Specifically, N=4 for quaternions (Qx, Qy, Qz, Qw), ‘N=3’ for both the Euler angles (Roll-Pitch-Yaw) and the angular velocity. The Yaw angle of the Yaw-Pitch-Roll Euler angles representations is systematically included in the feature space, even if its value is meaningless, i.e. ignore_twist=True.
Warning
This filter works best for ‘observe_dt’ smaller or equal to 5ms. Its performance drops rapidly beyond this point. Having ‘observe_dt’ equal to 10ms is generally acceptable but the yaw estimate is drifting anyway even for fairly slow motions and without sensor noise and bias.
- Parameters:
name (str) – Name of the block.
env (InterfaceJiminyEnv[BaseObs, BaseAct]) – Environment to connect with.
ignore_twist (bool) – Whether to ignore the twist of the IMU quaternion estimate.
exact_init (bool) – Whether to initialize orientation estimate using accelerometer measurements or ground truth. False is not recommended because the robot is often free-falling at init, which is not realistic anyway. Optional: True by default.
kp (ndarray | Sequence[float] | float) – Proportional gain used for gyro-accel sensor fusion. Set it to 0.0 to use only the gyroscope. In such a case, the orientation estimate would be exact if the sensor is bias- and noise-free, and the update period matches the simulation integrator timestep. Optional: 1.0 by default.
ki (ndarray | Sequence[float] | float) – Integral gain used for gyro bias estimate. Optional: 0.1 by default.
compute_rpy (bool) – Whether to compute the Yaw-Pitch-Roll Euler angles representation for the 3D orientation of the IMU, in addition to the quaternion representation. Optional: False by default.
update_ratio (int) – Ratio between the update period of the observer and the one of the subsequent observer. -1 to match the simulation timestep of the environment. Optional: 1 by default.
- property fieldnames: Dict[str, List[List[str]]]¶
Get mapping between each scalar element of the observation space of the observer block and the associated fieldname for logging.
It is expected to return an object with the same structure than the observation space, but having lists of string as leaves. Generic fieldnames are used by default.