Compensation for centrifugal accelerations

[brightproject]

Hello @reefwing, thanks for you work! I'm trying the AHRS library for experiments. Before that I used Madgwick filtering. Everything works fine on the ground. After testing in flight, centrifugal acceleration changes the level of the horizon and makes it parallel to the airplane wing. Compiled the code from the example. Acceleration and magnetic field data are provided by lsm303, and angular velocity data are provided by l3gd20. I work with the development board. Due to the design features, I had to rotate the board axes. As a result, my roll axis is Z, the pitch axis is X, and the yaw axis is Y.

// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise

The positive direction of rotation of the aircraft axes does not coincide with the positive rotation of the board axes. But the “minus” was indicated only for the Z axis, inside the library Reefwing-AHRS code there was some kind of perception of the “sign” of the axis rotation. I submit data from the following units to the input of the ahrs. Accelerations in G. Angular velocities in degrees/sec. Magnetic field strength in uGauss. I tell the filter like this ahrs.setFusionAlgorithm(SensorFusion::MADGWICK);

In the code, I swapped the Z and Y axes only for the accelerometer and gyroscope, I did not change the magnetometer axis, because when I change the magnetometer, the pitch angle starts to twitch like crazy.

gyro.read();
lsm303.read();  

data.ax = lsm303.accelData.x;// G
data.ay = -lsm303.accelData.z;// G
data.az = lsm303.accelData.y;// G

data.gx = gyro.g.x;// deg/s
data.gy = -gyro.g.z;// deg/s
data.gz = gyro.g.y;// deg/s

data.mx = lsm303.magData.x;// uGauss
data.my = lsm303.magData.y;// uGauss
data.mz = lsm303.magData.z;// uGauss

ahrs.setData(data);
ahrs.update();

roll = ahrs.angles.pitch;
pitch = ahrs.angles.roll;
heading = ahrs.angles.heading;

At the output I get the roll, pitch and heading angles.

        Roll: 0.71      Pitch: 87.70    Yaw: 43.96      Heading: 43.96  Loop Frequency: 1 Hz
                Accel X: -18.00         Y: 16568.00     Z: 1088.00 Raw
                Accel X: -0.00  Y: 1.01         Z: 0.07 G
                Accel X: -0.01  Y: 9.92         Z: 0.65 m/s^2
                Gyro X: 33.00 Y: 19.00 Z: -20.00 raw
                Gyro X: 0.50 Y: 0.29 Z:  deg/s
                Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
                Mag X: 65354.00         Y: 64935.00     Z: 7.00 uGauss
                Mag X: 653.54   Y: 649.35       Z: 0.07 microtesla (uT)
        Roll: -2.16     Pitch: 80.85    Yaw: 45.96      Heading: 45.96  Loop Frequency: 1 Hz
                Accel X: 238.00         Y: 16248.00     Z: 1024.00 Raw
                Accel X: 0.01   Y: 0.99         Z: 0.06 G
                Accel X: 0.14   Y: 9.73         Z: 0.61 m/s^2
                Gyro X: 43.00 Y: 28.00 Z: -33.00 raw
                Gyro X: 0.66 Y: 0.43 Z:  deg/s
                Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
                Mag X: 65354.00         Y: 64935.00     Z: 137.00 uGauss
                Mag X: 653.54   Y: 649.35       Z: 1.37 microtesla (uT)
        Roll: -0.20     Pitch: 89.22    Yaw: 44.82      Heading: 44.82  Loop Frequency: 1 Hz
                Accel X: -18.00         Y: 16440.00     Z: 1024.00 Raw
                Accel X: -0.00  Y: 1.00         Z: 0.06 G
                Accel X: -0.01  Y: 9.84         Z: 0.61 m/s^2
                Gyro X: 29.00 Y: 32.00 Z: -31.00 raw
                Gyro X: 0.44 Y: 0.49 Z:  deg/s
                Gyro X: 0.01 Y: 0.01 Z: -0.01 rad/s
                Mag X: 65355.00         Y: 64934.00     Z: 65532.00 uGauss
                Mag X: 653.55   Y: 649.34       Z: 655.32 microtesla (uT)

If I move the sensors from side to side, for example along the X axis, then the roll angle also changes, which indicates that the ahrs does not compensate for centrifugal forces. Have you tried this code on a flight?

[reefwing]

Hi @brightproject - this is an interesting issue that you have raised.

My library uses the standard open source Madgwick algorithm, so you should get a similar result to the Madgwick Arduino library. I am not doing any additional compensation for centrifugal forces.

Have you tried the Fusion library from xioTechnologies? This is an updated version of the algorithm written by Seb Madgwick. He owns xioTechnologies and sells an IMU to use with it. If anyone can address your issue it will be him. Let me know how you go, yours looks like an interesting project.

[brightproject]

Have you tried the Fusion library from xioTechnologies? This is an updated version of the algorithm written by Seb Madgwick. He owns xioTechnologies and sells an IMU to use with it. If anyone can address your issue it will be him. Let me know how you go, yours looks like an interesting project.

Nope, but I'll put this code on the waiting shelf. The authors of the library, in one of the issues, indicate that the code should compensate for the accelerometer taking into account gravity. If your code uses the Majwick model, how can you compensate for centrifugal forces or at least reduce the effect of acceleration on pitch and roll? I also want to check the code of a fairly old project for Razor board. Your AHRS code, for example can assign different “weights” to the accelerometer and gyroscope. As I understand the physics of flight, in a straight, steady flight the force of gravity is directed downwards (gravity + - is equal to the lifting force and the centrifugal and centripetal forces). When we start a turn, for example a left bank, the gravity vector from vertical down turns slightly to the side, to the right, and becomes perpendicular to the wing of the aircraft. The accelerometer, of course, measures this acceleration and shows, accordingly, a horizon line parallel to the line of the wing. While the real horizon outside the airplane window has a completely different plane. Somehow it would be possible, at the moment of starting turns, to reduce the strength and influence of the accelerometer on the output result of the filter.

Enter some scaling factors and change the polling time. For example, poll the gyroscope 200 times per second, and the accelerometer 20 times per second, and the magnetometer generally 5 times per second. Have you used Reefwing-AHRS in a quadcopter or airplane flight?

[reefwing]

Yes I understand what you are saying and no my library doesn't compensate for centripetal or centrifugal force (depending on your frame of reference).

I've used the library with my quadcopter flight controller and many other drone controllers use these same sensor fusion algorithms. I think there are three reasons why it is less of an issue for drones.

The first reason is that it is just one more error in the accuracy of the MEMS IMUs. Other errors include gyro drift, mag hard & soft iron distortion, bias offset, vibration noise, and scale factor. Some of this you try to calibrate out, but you can't remove all the errors.

The theory behind sensor fusion is to combine the outputs of different sensors in attempt to compensate for the remaining errors. My library gives you the option of choosing from the following algorithms:

• Classic Complementary Filter
• Quaternion Complementary Filter
• Madgwick Filter
• Mahony Explicit Complementary Filter (ECF)
• Kalman Filter (6 DOF)
• No sensor fusion

I include the no sensor fusion option to illustrate why you need sensor fusion. The IMU values without it are useless!

When you speak about applying different "weights" to the gyro and accelerometer, this is only relevant to the complementary filter. The free parameter that effects this is called gain or alpha. You can adjust it in the library. Typically, for the complementary filter 98% of the output is from the gyro and 2% is from the accelerometer. I go into a lot more detail about this in my article - How to Write your own Flight Controller Software — Part 7.

The second reason that I think that it is less of an issue in drone flight controllers is because the IMU is used to generate a feedback error into a PID loop which is trying to maintain a setpoint. This PID loop is also getting input from the person flying the drone who is also compensating for errors.

Finally, there are a number of standard flight control modes for drones, including rate and stabilise. In rate, gyro or acro mode the flight controller only uses the gyroscope for flight feedback. Stabilise mode attempts to self level the drone using the accelerometer - this is most effective when the drone is not moving.

Regarding different polling rates for the gyro and accel, as you are using two separate devices you can already do that. The Output Data Rate (ODR) for the LSM303 has 4 options (10, 20, 50 or 100 Hz) - Table 93 of the data sheet. The L3GD20 gyro has options of 95, 190, 380, or 760 Hz (Table 4 of the data sheet). You can keep track of the time and update the AHRS model whenever you want with whatever values you want.

[brightproject]

I tried the reefwing code on my flight, there is a roll deviation when turning. I decided to implement physics as it is. Calculate roll and pitch using separate accelerometers and gyroscopes. The main idea is to take data from the gyroscope during rotation, and when the device is stationary (has some small degree of rotation) take readings from the accelerometer.

#define RAD_TO_DEG M_PI*180;

float rollRawA, pitchRawA, rollNewA, pitchNewA, rollNewG, pitchNewG;
float rollOldA=0;
float pitchOldA=0;
float rollRawG=0;
float pitchRawG=0;

millisOld=millis();

// ax, ay, az in G, i.e. 1G - equivalent 9.81 m/s^2

rollRawA=-atan2(ax,ay)/RAD_TO_DEG;// roll by accels(no filtered)
pitchRawA=-atan2(az,ay)/RAD_TO_DEG;// pitch by accels(no filtered)
rollNewA=.95*rollOldA+.05*rollRawA;// roll by accels(filtered)
pitchNewA=.95*pitchOldA+.05*pitchRawA;// pitch by accels(filtered)

dt = (millis() - millisOld) / 1000.0;
millisOld = millis();

// gx, gz in deg/sec

rollRawG = rollRawG - gz * dt; // roll by gyro(no compensated drift)
pitchRawG = pitchRawG - gx * dt; // pitch by gyro(no compensated drift)

// Check condition for OFF gyro and ON accel

if (gz >= -0.1 && gz <= 0.1) {
// Apply first formula if angular velocity is in range [-0.1, 0.1]
rollNewG= 0.05 * rollRawG+ 0.95 * rollRawA; // roll
rollRawG= rollNewG;// for compensate gyro drift
} else {
// Apply second formula otherwise
rollNewG= 0.95 * rollRawG+ 0.05 * rollRawA; // roll
}

if (gx >= -0.1 && gx <= 0.1) {
// Apply first formula if angular velocity is in range [-0.1, 0.1]
pitchNewG= 0.05 * pitchRawG+ 0.95 * pitchRawA; // pitch
pitchRawG= phiGnew;// for compensate gyro drift
} else {
// Apply second formula otherwise
pitchNewG= 0.95 * pitchRawG+ 0.05 * pitchRawA; // pitch
}

I checked this code in a plotter, when there is a roll and some angular velocity, the roll angle is not influenced by accelerations. Also along the pitch axis. The orange graph shows roll(fluctuated) without acceleration compensation, the green graph shows roll(-9 degrees) with acceleration compensation. I’m thinking about how to combine this method with some kind of Madgwick filter so that there is no influence of the pitch axis on the roll axis. To operate not with Euler angles, but with quaternions. Of course, the code has not yet been perfected and there are some rough edges in the work. I don’t have enough experience in the topic of rotating bodies and measuring them with MEMS sensors.

[reefwing]

Nice. Have a look at the Kalman filter as well. You can use this with just an accel and gyro. Madgwick needs the three sensors, including magnetometer. I actually mostly use the Mahony algo.

[titanproger]

Hi all, just note that Madgwick has implementation without magnetormer. But it good to have , as gyro can drift by vertical axis (track angle) and compensate gyro drift on that axis done with magnetometer output.

If not constantly adjusting gyro offset on z axis, we will have more errors on roll and pitch as long we are not in perfect horizontal.

[reefwing]

Thanks @titanproger - can you point me towards the no mag madgwick algorithm? I wouldn't mind including it if is is open source.

[titanproger]

@reefwing just first founded example (not read it carefully so if it is bad example, sorry): https://github.com/arduino-libraries/MadgwickAHRS/blob/master/src/MadgwickAHRS.cpp#L153

magnetometer is optional

[reefwing]

I'll check it out @titanproger - thanks again!