Z axis points downwards if magnetometer does not publish during some seconds

[csimon-be]

Hi,

I am working with the imu_filter_madgwick package and have encountered some behavior that I do not understand.

Using the package with a magnetometer, we noticed that sometimes our robot's heading turns in the opposite direction when turning in place. After a certain delay, it returns to normal.

The issue seems to originate from the data published on /imu/data (output from the Madgwick filter), and it is observable in Rviz. To confirm this hypothesis, it is reproducible with another magnetometer we tested. Therefore, it confirms that the issue is not with the device itself but with our Madgwick filter configuration. I believe our IMU configuration is correct (Z is positive and points upwards).

For the record, here is our imu_filter_madgwick.yaml configuration:

imu_filter_madgwick:
  ros__parameters:
    gain: 0.1
    zeta: 0.0
    mag_bias_x: 0.0
    mag_bias_y: 0.0
    mag_bias_z: 0.0
    orientation_stddev: 0.1

    world_frame: enu
    use_mag: true
    use_magnetic_field_msg: true
    publish_tf: false
    constant_dt: 0.0
    publish_debug_topics: true
    stateless: false
    remove_gravity_vector: false

To reproduce the issue:

1. Launch the code.
2. Unplug the power supply of the magnetometer.
3. Wait for 5 seconds.
4. Plug it back in.

You'll notice that the axis rotates when you plug it back in (the z-axis points downwards instead of upwards). It slowly returns to normal after some time. The longer you wait, the more disoriented it gets.

I have attached a video demonstrating the steps described above. Once I plug the magnetometer back in, you can see the axes are completely disoriented. Increasing the gain helps to recover faster, but I would prefer to avoid this disorientation altogether. Can you help me, please?

Screencast.from.10-10-2024.12.11.51.PM.webm

Thanks in advance,
Kind regrads,

Christophe

[mintar]

Did you have a look at the input topics of imu_filter_madgwick? What happens during the time when the magnetometer is unplugged? Are no messages sent, are the values all 0/NaN/inf?

I could imagine something like this happening: magnetometer is unplugged -> no messages for 5 seconds -> dt becomes very large (5 s instead of ~0.01 s) -> last correction gets multiplied by this huge value -> orientation gets lost

Unfortunately the filter doesn't yet check for large time jumps forward. There is a function called checkTimeJump, but that only checks jumps backward. You could try modifying this function so that the filter is reset after a configurable threshold of 1 second or so when the time jumps forward. If you do and it solves you problem, please send a PR!

[csimon-be]

Hi @mintar ,

Thanks for your input, it helped me to figure out what could be the issue. Indeed, no messages are sent when the magnetometer is unplugged.

I will check the checkTimeJump method and try to modify it according to your advice.

[mintar]

There's already an open PR (#200) about the adding forward jump checks to the checkTimeJump method. It got stalled because the author and I didn't agree on what the correct behaviour would be. If you want, you could pick it up, simplify and test it, so we can merge it.

Alternatively, as a workaround, you can just set the constant_dt parameter appropriately. That way, a long pause in the incoming topics will not result in a large dt.

[csimon-be]

Hi @mintar,

I finally had the opportunity test your recommandation and heading seem to stay consistent. Forcing a constant_dt in madgwick configuration file (similar to the magnetometer's publisher frequency) seems to resolve the problem.

Thank you for your help!