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, 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.
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
- gym_jiminy.common.blocks.mahony_filter.update_twist(q, twist, omega, time_constant_inv, dt)[source]¶
Update the twist estimate of the Twist-after-Swing decomposition of given orientations in quaternion representation using leaky integrator.
- Parameters:
q (ndarray) – Current swing estimate as a quaternion. It will be updated in-place to add the estimated twist.
twist (ndarray) – Current twist estimate to update in-place.
omega (ndarray) – Current angular velocity estimate in local frame.
time_constant_inv (float) – Inverse of the time constant of the leaky integrator used to update the twist estimate.
dt (float) – Time step, in seconds, between consecutive Quaternions.
- Return type:
None
- class gym_jiminy.common.blocks.mahony_filter.MahonyFilter(name, env, *, twist_time_constant=0.0, exact_init=True, kp=1.0, ki=0.1, update_ratio=1)[source]¶
Bases:
BaseObserverBlock[ndarray,ndarray,BaseObsT,BaseActT]Mahony’s Nonlinear Complementary Filter on SO(3).
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
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[BaseObsT, BaseActT]) – Environment to connect with.
twist_time_constant (float | None) – If specified, it corresponds to the time constant of the leaky integrator used to estimate the twist part of twist-after-swing decomposition of the estimated orientation in place of the Mahony Filter. If 0.0, then its is kept constant equal to zero. None to kept the original estimate provided by Mahony Filter. See remove_twist_from_quat and update_twist doc for details. Optional: 0.0 by default.
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.
mahony_kp – 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.
mahony_ki – Integral gain used for gyro bias estimate. Optional: 0.1 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.
kp (ndarray | float)
ki (ndarray | float)
- property fieldnames: 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.