Open brightproject opened 1 month ago
@brightproject Hi bright, I'm happy to help with the conversion of the code. Unfortunately, I don't have the hardware to help you develop the converted code, so at best I can only provide suggestions that might be helpful.
Upon reading your explanation, It's not clear to me whether you've been able to obtain clean, almost drift free outputs for the gyroscope and accelerometer. Please note that for the fusion algorithm to work, the angular velocity must be measured in radians/second, and the acceleration must be measured in g-units (dimensionless unit). In the case of acceleration, it must read |g| = 1.0 (magnitude of gravity) when the sensor is held perfectly level.
Moreover, the fusion algorithm will not correct for an uncalibrated angular velocity and acceleration. These must be calibrated manually before being fed through the filter.
Also (and very importantly), sometimes the orientation the IMU when it is held level will not correspond to the acceleration vector (0,0,1) which the fusion algorithm expects. You may have to swap the acceleration components to a different coordinate system to orient the vector properly. For example: (Ax, Ay, Az) -> (-Ax, Az, Ay)
Another important thing is the GAIN variable. This value should ideally not be very large. Think of it like the alpha parameter of an exponential moving average. If GAIN=1 the fusion output will just be the instantaneous acceleration vector.
Moving forward: For now, lets just focus on the output of your IMU to see if its compatible with the form expected by imuFilter.
Collected data in the setup and loop section below
Accel + mag LSM303 FOUND!
Gyro L3DG20 FOUND!
ax: -64.00 ay: 4112.00 az: -432.00
gx: 108 gy: -35 gz: 62
gyroBias[0]: 0.05 gyroBias[1]: -0.02 gyroBias[2]: 0.03
accelBias[0]: -8.00 accelBias[1]: 513.00 accelBias[2]: -54.00
aix: -72 aiy: 3647 aiz: -454
ax: -0.02 ay: 0.89 az: -0.11
gix: 2 giy: 2 giz: -3
gx: 0.00 gy: 0.00 gz: -0.00
aix: -40 aiy: 3663 aiz: -454
ax: -0.01 ay: 0.89 az: -0.11
gix: -4 giy: -2 giz: 3
gx: -0.00 gy: -0.00 gz: 0.00
aix: -56 aiy: 3663 aiz: -406
ax: -0.01 ay: 0.89 az: -0.10
gix: -13 giy: 7 giz: 0
gx: -0.01 gy: 0.01 gz: 0.00
aix: -24 aiy: 3615 aiz: -470
ax: -0.01 ay: 0.88 az: -0.11
gix: 7 giy: -7 giz: 6
gx: 0.01 gy: -0.01 gz: 0.01
aix: -40 aiy: 3631 aiz: -470
ax: -0.01 ay: 0.89 az: -0.11
gix: -2 giy: 0 giz: 3
gx: -0.00 gy: 0.00 gz: 0.00
aix: -72 aiy: 3631 aiz: -438
ax: -0.02 ay: 0.89 az: -0.11
gix: 5 giy: -3 giz: -9
gx: 0.01 gy: -0.00 gz: -0.01
aix: -56 aiy: 3615 aiz: -422
ax: -0.01 ay: 0.88 az: -0.10
gix: -8 giy: -7 giz: -2
gx: -0.01 gy: -0.01 gz: -0.00
Acceleration values are both in raw values and in units of G
.
Angular velocities are also in raw values and in rad/sec
.
define GYRO_RANGE 2000.0f
define ACCEL_RANGE 8.0f
You wrote that gyroscopes should be set in degrees per second - is this probably a mistake?
Ouff! Sorry!!!! Here I am trying to help and I made a mistake. Yes, I meant radians/second!!!
That embarrassment aside, those accelerometer values seem to be miscalibrated. The magnitude of the vector ~(ax: -0.01 ay: 0.89 az: -0.11) is approximately ~0.9.
This filter requires the resting acceleration be exactly 1 in magnitude, as it uses any deviation from this value Error = Accel_Mag - 1 to determine whether to apply a correction or not. In this case, you should manually calibrate the accelerometer to achieve 1g in magnitude indifferent of how its rotated when the sensor is at rest.
The SD_ACCEL
parameter of fusion.update( gx, -gz, -gy, ax, az, ay, GAIN, SD_ACCEL );
determines whether the acceleration vector will update the heading, if Error < SD_ACCEL. Notice how if the acceleration is not calibrated and normalized, the error will be larger than the allowed tolerance.
Next step: After calibrating the accelerometer, how does the fusion algorithm react to the gyroscope and accelerometer values in isolation? try:
fusion.update( 0, 0, 0, ax, az, ay, GAIN, SD_ACCEL );
and
fusion.update( gx, -gz, -gy );
If the accelerometer and gyroscope axes are aligned properly, the acceleration should slowly converge towards the heading estimated by the gyroscope. If the acceleration by itself causes the heading output to diverge then swap the sign of one of the acceleration components.
Hello @RCmags I am rewriting the code to assemble it under the![source](https://github.com/RCmags/imuFilter/assets/1788098/ddbfd1aa-772d-4be0-8266-575137e1fb60)
stm32f411ceu6
or BlackPill in theArduino
ecosystem.I receive raw data, process it using the calibration function
I normalize the axes like this:
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G
andAfter the filter, I multiply the orientation angles by
57.32
to get the angles(ROLL, PITCH, YAW) indegrees/second
again. I output port data to the serial in a format that is understandable to the visualizer for processing. And most importantly, as a result, I do not see the “magic” of theKalman
filter. Orientation angles (yaw generally drifts probably half a degree per second, even the Majwick filter didn’t work that bad), roll also goes away quickly. Even with a coefficientThe algorithm doesn't want to calculate orientation angles based only on the accelerometer, it still uses a gyroscope, otherwise I can't explain the drift I see.
https://github.com/RCmags/imuFilter/assets/1788098/d650af09-ba42-42bd-a604-5146ab450850
and
https://github.com/RCmags/imuFilter/assets/1788098/072de865-9257-4229-832d-11f172a151d9
Code below
It would be great if you tried to run this code with similar sensors or others, for example icm20948 or bno055. Let me know if you need a graphic attitude indicator in the processing language, I will send it to you in an archive. I also plan to test the algorithm based on raw data from the gyroscope and accelerometer from the flight gear simulator, as I did here
I would be glad for a little help.