如何使用磁力计为陀螺z轴添加辅助滤波器？

Question

I am using a complementary filter for my gyro X and gyro Y values:

float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x;

However, due to Gyro Z drift, I am using a magnetometer to try and eliminate it.

I have my local inclination angle and the z-axis magnetometer reading but I am not sure how to implement this into the complementary filter.

I've tried guessing (which I know doesn't help) to try this:

float angle_z = alpha*gyro_angle_z + ((1.0 -alpha)*accel_angle_y*InclinationAngle)/magZ;

However, it just increases gyro z value when moving and then brings it back to 0 when stationary.

Answer 1

Using the Madgwick filter will work best. The link can be found here . It is easy to implement (if using the correct library and the readings are pretty accurate. The only downside at the moment is that you will have to calibrate the magnetometer every time you want to use the device in a new direction).