imuFilter
imuFilter copied to clipboard
RCmags
Using the algorithm of imuFilter with gyroscope(l3g20h) and accelerometer(lsm303) sensors
Hello @RCmags
I am rewriting the code to assemble it under the stm32f411ceu6 or BlackPill in the Arduino ecosystem.
I receive raw data, process it using the calibration function
calibrateSensor(gyroBias, accelBias);
I normalize the axes like this:
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G
and
gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
gx *= DEG_TO_RAD; // radians per second
After the filter, I multiply the orientation angles by 57.32 to get the angles(ROLL, PITCH, YAW) in degrees/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 the Kalman 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 coefficient
#define GAIN 1.0
The 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
/*
This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
*/
/* NOTE: The accelerometer MUST be calibrated for the fusion to work. Adjust the
AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest.
*/
#define AHRS_VERSION 13.1
#define PROCESSING
// #define DEBUG
#include <Wire.h>
// library Adafruit LSM303
#include <Adafruit_LSM303.h>
// library L3DG20
#include <L3G.h>
// #include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
#include <imuFilter.h> // https://github.com/RCmags/imuFilter/
/* // Gyro settings:
#define LP_FILTER 3 // Low pass filter. Value from 0 to 6
#define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3
#define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3
#define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW
// A0 -> 5v : ADDRESS_A0 = HIGH
// Accelerometer offset:
constexpr int AX_OFFSET = 0; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level.
constexpr int AY_OFFSET = 0; // These values are unlikely to be zero.
constexpr int AZ_OFFSET = 0;
//-- Set the template parameters:
basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0,
AX_OFFSET, AY_OFFSET, AZ_OFFSET
>imu; */
// =========== Settings ===========
imuFilter fusion;
#define G (9.80665)
#define DEG_TO_RAD (M_PI/180)
#define RAD_TO_DEG (180/M_PI)
#define GYRO_RANGE 2000.0f
// #define GYRO_SCALE 28571.4f
#define GYRO_SCALE 32768.0f
#define ACCEL_RANGE 8.0f
#define ACCEL_SCALE 32768.0f
#define STATUS_LED PC13
#define SERIAL_BAUD (115200)
#define GAIN 0.9 /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector.If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */
#define SD_ACCEL 0.1 /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed. Accelerations within this band are used to update the orientation. [Measured in g-force] */
#define FUSION true /* Enable sensor fusion. Setting to "true" enables gravity correction */
int16_t aix, aiy, aiz, gix, giy, giz;// raw
float ax, ay, az, gx, gy, gz, mx, my, mz;// G | deg/s | uGauss
float axX, ayY, azZ, gxX, gyY, gzZ, mxX, myY, mzZ;// m/s^2 | rad/s | microtesla (uT)
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
Adafruit_LSM303 lsm303;
// Adafruit_LSM303 lsm303(&Wire1);
L3G gyro;
void setup() {
Serial.begin(SERIAL_BAUD);
Wire.begin();
Serial.print("AHRS ver: "); Serial.println(AHRS_VERSION, 1);
pinMode(STATUS_LED, OUTPUT); // Status LED
digitalWrite(STATUS_LED, HIGH); // stm32f411 led OFF
// Calibrate imu
// imu.setup();
// imu.setBias();
// Adafruit LSM303
initAccelMag();
// L3DG20
initGyro();
// read accel + mag data
lsm303.read();
// read gyro data
gyro.read();
calibrateSensor(gyroBias, accelBias);
#ifdef DEBUG
Serial.print("gyroBias[0]: ");
Serial.println(gyroBias[0]);
Serial.print("gyroBias[1]: ");
Serial.println(gyroBias[1]);
Serial.print("gyroBias[2]: ");
Serial.println(gyroBias[2]);
Serial.print("accelBias[0]: ");
Serial.println(accelBias[0]);
Serial.print("accelBias[1]: ");
Serial.println(accelBias[1]);
Serial.print("accelBias[2]: ");
Serial.println(accelBias[2]);
delay(2000);
#endif
aix = lsm303.accelData.x - accelBias[0]; // RAW X
aiy = lsm303.accelData.y - accelBias[1]; // RAW Y
aiz = lsm303.accelData.z + accelBias[2]; // RAW Z
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // G
// ax *= G; // in 9.8 m/s/s
ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // G
// ay *= G; // in 9.8 m/s/s
az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // G
// az *= G; // in 9.8 m/s/s
#if FUSION
// Set quaternion with gravity vector
// fusion.setup( imu.ax(), imu.ay(), imu.az() );
fusion.setup( ax, -az, ay );
#else
// Just use gyro
fusion.setup();
#endif
}
void loop() {
// read accel + mag data
lsm303.read();
// read gyro data
gyro.read();
aix = lsm303.accelData.x - accelBias[0]; // RAW
aiy = lsm303.accelData.y - accelBias[1]; // RAW
aiz = lsm303.accelData.z + accelBias[2]; // RAW
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// ax *= G; // in 9.8 m/s/s
ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// ay *= G; // in 9.8 m/s/s
az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// az *= G; // in 9.8 m/s/s
// Serial.print("ax: ");
// Serial.println(ax);
// Serial.print("ay: ");
// Serial.println(ay);
// Serial.print("az: ");
// Serial.println(az);
gix = gyro.g.x - gyroBias[0];// RAW
giy = gyro.g.y - gyroBias[1];// RAW
giz = gyro.g.z - gyroBias[2];// RAW
gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
gx *= DEG_TO_RAD; // radians per second
gy = ((giy * GYRO_RANGE) / GYRO_SCALE); // DPS
gy *= DEG_TO_RAD; // radians per second
gz = ((giz * GYRO_RANGE) / GYRO_SCALE); // DPS
gz *= DEG_TO_RAD; // radians per second
#ifdef DEBUG
Serial.print("aix: ");
Serial.println(aix);
Serial.print("aiy: ");
Serial.println(aiy);
Serial.print("aiz: ");
Serial.println(aiz);
// Serial.print("ax: ");
// Serial.println(ax);
// Serial.print("ay: ");
// Serial.println(ay);
// Serial.print("az: ");
// Serial.println(az);
Serial.print("gix: ");
Serial.println(gix);
Serial.print("giy: ");
Serial.println(giy);
Serial.print("giz: ");
Serial.println(giz);
// Serial.print("gx: ");
// Serial.println(gx);
// Serial.print("gy: ")
// Serial.println(gy);
// Serial.print("gz: ");
// Serial.println(gz);
delay(200);
#endif
// Update filter:
#if FUSION
/* NOTE: GAIN and SD_ACCEL are optional parameters */
fusion.update( gx, -gz, -gy, ax, az, ay, GAIN, SD_ACCEL );
#else
// Only use gyroscope
fusion.update( imu.gx(), imu.gy(), imu.gz() );
#endif
/* // Display angles:
Serial.print( fusion.pitch() );
Serial.print( " " );
Serial.print( fusion.yaw() );
Serial.print( " " );
Serial.print( fusion.roll() );
// timestep
Serial.print( " " );
Serial.print( fusion.timeStep(), 6 );
Serial.println(); */
#ifdef PROCESSING
Serial.print("Orientation: ");
Serial.print(fusion.pitch() * RAD_TO_DEG); // roll
Serial.print(" ");
Serial.print(fusion.roll() * RAD_TO_DEG); // pitch
Serial.print(" ");
Serial.println(fusion.yaw() * RAD_TO_DEG); // yaw
#endif
}
bool initAccelMag()
// void initAccelMag()
{
bool successAccelMag = false;
if (!lsm303.begin())
{
successAccelMag = false;
#ifdef DEBUG
Serial.println("Failed to autodetect accel + mag LSM303!");
#endif
while (1);
} else {
successAccelMag = true;
#ifdef DEBUG
Serial.println("Accel + mag LSM303 FOUND!");
#endif
}
lsm303.enable_temperatureSensor();
// tempRaw = lsm303.get_temperatureData();
// lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_1_3);
lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_8_1);
// 0x08 = 0b00001000 2g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x08);
// 0x18 = 0b00011000 4g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x18);
// 0x28 = 0b00101000 8g
lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x28);
// 0x38 = 0b00111000 16g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x38);
// 0 Boot - 00 HP-Filter - 0 filtered data selection - 0 HP filter enabled for int2 source - 0 HP filter enabled for int1 source -
// 00 HP Filter Cut off Frequ
// 0b00000000
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x00);
// 0b10000100 - 0x84
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x84);
// 0b11111111 - 0xF
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0xFF);
lsm303.enable_temperatureSensor();
return successAccelMag;
}
bool initGyro()
// void initGyro()
{
bool successGyro = false;
if (!gyro.init())
{
successGyro = false;
#ifdef DEBUG
Serial.println("Failed to autodetect gyro L3GD20!");
#endif
while (1);
} else {
successGyro = true;
#ifdef DEBUG
Serial.println("Gyro L3DG20 FOUND!");
#endif
}
// enableDefault
// 0x5F = 0b01011111
// DR = 01 (190 Hz ODR); BW = 01 (25 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
gyro.enableDefault();
// gyro.writeReg(L3G::CTRL_REG4, 0x00); // 250 dps full scale 0b0000000
// gyro.writeReg(L3G::CTRL_REG4, 0x10); // 500 dps full scale 0b00100000
gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale 0b00110000
// gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, ODR 95Hz, Cut-Off 12.5 Hz - 0b00001111
// 0x6F = 0b01101111
// DR = 01 (190 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
// gyro.writeReg(L3G::CTRL_REG1, 0x6F);
// 0xAF = 0b10101111
// DR = 10 (380 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
// gyro.writeReg(L3G::CTRL_REG1, 0xAF);
// 0xBF = 0b10111111
// DR = 10 (380 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
gyro.writeReg(L3G::CTRL_REG1, 0xBF);
return successGyro;
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void calibrateSensor(float * dest1, float * dest2)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
// uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
// uint16_t accelsensitivity = 16384; // = 16384 LSB/g
uint16_t gyrosensitivity = GYRO_RANGE;
uint16_t accelsensitivity = ACCEL_RANGE;
fifo_count = 100;
packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
// Form signed 16-bit integer for each sample in FIFO
accel_temp[0] = (int16_t) lsm303.accelData.x;
accel_temp[1] = (int16_t) lsm303.accelData.y;
accel_temp[2] = (int16_t) lsm303.accelData.z;
gyro_temp[0] = (int16_t) gyro.g.x;
gyro_temp[1] = (int16_t) gyro.g.y;
gyro_temp[2] = (int16_t) gyro.g.z;
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[0] += (int32_t) accel_temp[0];
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
// Normalize sums to get average count biases
accel_bias[0] /= (int32_t) packet_count;
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
if(accel_bias[1] > 0L) {accel_bias[1] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[1] += (int32_t) accelsensitivity;}
// Push gyro biases to hardware registers
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
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
https://forum.flightgear.org/viewtopic.php?f=36&t=42308
I would be glad for a little help.
@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.
Good day @RCmags
I've switched to other programming a bit, I'm making a GUI for an aircraft instrument.
But I'm still interested in your filter implementation.
Do you still want to explain some points to me and help me?
Do you have an ESP32 or STM32 microcontroller available?
And also some kind of USB-TTL converter.
I could show you how to get raw orientation data from the FlightGear flight simulator and substitute it into your filter.
You would figure out how to set up the algorithm faster than I did.
I would be grateful if you were interested in this🙂
Sure I would be happy to help out. Just let me known what you want assistance with.
Unfortunately I don't have either of the boards you mentioned, but I can buy whichever one you're interested in focusing on. The only issue I see is getting the gyroscope you're using. I only have MPU6050's at hand.
In general, in order to close the development loop of HITL or SITL, at least one of the microcontrollers STM32, ESP8266 or ESP32, is needed.
You can also use some MEMS sensor, like BNO055/08X or ICM-20948 or something simpler, like MPU9250/6050 or LSM6DSM.
Although in general, gyroscope and accelerometer sensors are not needed, i.e. they will be obtained from the simulator, based on the jbsim engine.
Please tell me, by what method did you check and configure your algorithm?
The hardest part is to get stable attitude angles with noise and drift factors in flight.
I tried using Madgwick and RTMUlib algorithms in flight, the latter worked very well in flight, even with noise added to the simulated data.
Has your algorithm been tested in a highly dynamic environment?
For example, a car/train ride or in flight?🙂
Lets go with the STM32 since it seems to have better compatibility with Arduino library compared to the ESP32. Do you think we'll need a magnetometer of some kind? I could get an MPU9250 if that's the case.
Please tell me, by what method did you check and configure your algorithm?
The method I've used to configure the Filter has just been simple manual calibration.
For the accelerometer, I've just placed the IMU on a level table then read the X, Y, and Z axis values. If they're not very close to (0,0,1)g, that bias is subtracted from the acceleration fed to the filter algorithm.
For the gyroscope I've used my basicMPU6050 library since it automatically calibrates the gyroscopes on startup. It then counters drift if the gyroscope signal is within an given error band.
Has your algorithm been tested in a highly dynamic environment? For example, a car/train ride or in flight?
Yes! I've used it in the flight controller for a radio controlled helicopter and to stabilize the pitch axis of a weight-shift controlled airplane. In both cases, the filter has worked very well despite combined noise sources from unbalanced motors, propellers and low frequency pulses from a large rotor. If you'd like to see the devices in action, I can upload some videos on youtube.
Lets go with the STM32
Hello @RCmags
If you have time and microcontrollers available, then you can try to get data from the flight simulator FlightGear and feed this data into the algorithm for output to the artificial horizon indicator.
For a quick understanding of the process, I would like to offer you my code, which includes a parser of data received in the serial port from the simulator FlightGear, processing this data and output to the serial port for the GUI, which is also in the folder with the code, written in the processing language.
It seems to me that to understand how to modify your code, I would like to offer you to copy the library code RTIMULib2 and load it into stm32 and work with it and the flight simulator FlightGear.
https://github.com/user-attachments/assets/8926b46f-e503-4359-87a2-5143b0b8b2e3
After that, you will be able to integrate this functionality into your code and try to set up your Kalman filtering algorithm.
I don't understand your algorithm well, but you work with it and understand it well enough.
In the video, the data is obtained directly from the simulator, i.e. the orientation angles are the same as in the simulator.
This is set by a flag in the code:
#define SIM_ANGLES
Please let me know when you are ready to try the code and I will send it to you by email or here, via github. I will show you and tell you further how to install, configure and run everything.