Issue with imu_filter_madgwick in slope when using a magnetometer about imu_tools

[mintar]

Thanks for the super clear problem description. I've tried reproducing the problem. I'm currently still using ROS1 Noetic, so I've converted your ROS2 rosbag to ROS1 format using ternaris/rosbags and ran the current ROS1 version of imu_filter_madgwick. Here are two videos of the results:

video 1 (gain = 0.1)

imu_gain_0.1.mp4

video 2 (gain = 0.01)

imu_gain_0.01.mp4

Video 1 shows:

1. your Zed2i data (without magnetometer)
2. your imu_filter_madgwick data
3. my imu_filter_madgwick data (no magnetometer, gain = 0.1)
4. my imu_filter_madgwick data (with magnetometer, gain = 0.1)

Video 2 shows:

5. my imu_filter_madgwick data (no magnetometer, gain = 0.01)
6. my imu_filter_madgwick data (with magnetometer, gain = 0.01)

Some things I noticed:

1. Your data (2) is the noisiest of all. What parameters did you use? By reducing the gain from the default 0.1 to 0.01, I could reduce the noise a lot, at the cost of slower convergence (compare (3) vs (5) and (4) vs (6)).
2. The data frequency is extremely low (< 10 Hz). Usually, I work with IMUs that produce data around 250 Hz. That's just a side remark though, as long as the data is good this shouldn't be a problem.
3. I could reproduce the problem you were describing! In (4), the filter clearly converges to a different yaw after tilting, and then again to the previous one after tilting back. In (6), this effect is still there, but much less noticeable because of the lower gain. That doesn't mean the problem is fixed though: It will still converge to the wrong yaw, only longer.

I believe I found the cause of your problem! Your magnetometer axes doesn't match the IMU axes (as I suspected from the beginning). If you look at the videos, you can see that the device is rotated around the y axis. However, the magnetometer data is rotated around the x axes. You can also see that in the following plot:

The green line (magnetic_field/x) and the bright blue line (linear_acceleration/y) stay flat, indicating that these are the axes that are rotated around. This means that these axes are either parallel or antiparallel.

I suggest that you perform a couple more experiments like this with other axes to identify which axes correspond to which other axes (and what the signs are), like this:

x_acc = -y_mag
y_acc = x_mag
z_acc = z_mag

(This is just an example!)

Then you can easily write a converter node that adapts the magnetometer data.

[avanmalleghem]

Great, you solved my issue !

In fact, I misunderstood the IMU standard. That's why my axes was wrong... Configuring it like you suggested with PlotJuggler solved it.

For those reading this : to determine if axes are parallel or antiparallel, you should check the derivative sign of their slope. For example, here we see that y and x are antiparallel. (correct me if I'm wrong @mintar )

Thanks for your help !

[mintar]

The imu_filter_madgwick assumes that all input data is in the same frame; it doesn't do any TF transformations of the input data. If your magnetometer and your gyro/accelerometer are in different frames, you can probably get away with it as long as they are fixed relative to each other and have the same orientation. In other words, if the IMU is flat on the table with z pointing upwards, and the magnetometer points in the y direction, then if you rotate the whole thing counter-clockwise by 90°, then the magnetometer should point in the x direction. And so on for the other 2 axes.

You should probably plot the magnetometer readings, the rotational velocities and the linear accelerations using PlotJuggler while rotating by 90° around all 3 axes (and writing down what you did), and also visualize the IMU/magnetometer topics/TF in RViz.

[avanmalleghem]

Thanks for your answer ! Here are the the my frames :

Using PlotJuggler, I verified that the IMU follows this convention for angular velocity and linear acceleration (and it does).
I've also verified that my magnetometer is ENU based.

But still have this tilt issue.
Moreover, I don't understand your sentence "have the same orientation.". In my understanding, magnetometer gives absolute (ENU) values while IMU gives relative values. So I don't understand how I can set the mag_link frame and don't understand how it can affect the output. I've checked the madgwick code and observed that this frame id isn't use at all. Can you clarify it please ?

[mintar]

Moreover, I don't understand your sentence "have the same orientation.".

By "have the same orientation" I meant that there is no rotation between the IMU and magnetometer frames. But from your screenshot it looks all good.

In my understanding, magnetometer gives absolute (ENU) values while IMU gives relative values.

Yeah, you are right. One exception though: The accelerometer of the IMU measures gravity, so that's an absolute value as well.

Anyway, could you record a rosbag (all input topics of imu_filter_madgwick + TF) and a video that shows the problem that you have?

[avanmalleghem]

Thanks again for your help !

The experiment

The imu is in the camera while the magnetometer is on top of this setup. You can observe that the URDF (and consequently the TF) doesn't represent it exactly. That's because I tried to simplify the setup as much as possible to avoid noises. I don't think it can influence the result.

3 steps :

1. Initial position : eyes towards south (more or less). See image 1
2. Tilt. See image 2
3. Back to initial position (image 1)

Observation

The upper graph is the output of the madgwick filter while the other one is the orientation outputted from the ZED2i camera and SDK (I think it is computed based on an internal madgwick filter based on the IMU but I'm not sure. It is a relative value.).

You can see that when we go from step 1 to step 2 (red line on the graph) that the madgwick output yaw drifts a lot (from -2 to -1.3) while the yaw computed from the ZED2i camera stays constant (only a small noise because I'm not a robot). I expect a constant output from the madgwick aswell, that's the issue I try to solve.

Rosbag

Here is the rosbag to reproduce the experiment. I've included the inputs of the madgwick, TF and the output of the madgwick so that you can really see the issue.

rosbag2_2024_04_18-11_12_22.zip

Hope you will be able to guide me. Thanks.