Implementation of Elkapod orientation system with Kalman Filter on STM32

[VisteK528]

Since we don't have now the sensor that will be mounted on the Hardware Controller Board (Bosh's BMI088) I used the ICM20948 6-axis IMU as a temporary replacement.

IMU setup:

• accelerometer -> +- 2G
• gyroscope -> +- 250 deg/s

[VisteK528]

I've acquired needed data (accelerations, angular velocities, etc.) from my setup using ICM20948 IMU and my custom universal board with STM32L476RG onboard.

Later, I modeled a Kalman Filter for the job of estimating 2 of 3 euler angles (roll and pitch). Currently, the result is quite satisfying, but more parameter tuning can be made.

[VisteK528]

I decided to better calibrate the IMU sensor so that the raw data before filtering will be of greater quality. For the purpose of the calibration I mounted the PCB flat on the leveled table and measured acceleration, gyro and euler data for about 60 sec which resulted in more than 20000 samples.

To check if the calibration was successful I measured the MSE and MEAN of the euler angles before and after the calibration.

Pre calibration

$MSE{\text{roll}} = 0.0891$ $MSE{\text{pitch}} = 0.5673$

$MEAN{\text{roll}} = -0.23544 \deg$ $MEAN{\text{pitch}} = -0.7325806 \deg$

Post calibration

$MSE{\text{roll}} = 0.0316$ $MSE{\text{pitch}} = 0.0297$

$MEAN{\text{roll}} = 0.003762 \deg$ $MEAN{\text{pitch}} = -0.001179 \deg$

I think the results are quite satisfying. Here you can see the euler angles before and after the calibration on the plot.

Before

After

Accelerometer offsets

$ax = a{x\text{raw}} + b_x$ $ay = a{y\text{raw}} + b_y$ $az = a{z\text{raw}} + b_z$

where

$b_x = -0.04008$ $b_y = 0.01267$ $b_z = -0.0189003$

[VisteK528]

Here you can see the results of almost calibrated Kalman Filter working on data from previously calibrated IMU. I also uploaded the data I was working on.

accel_data_2.csv gyro_data_2.csv orientation_data_2.csv

[VisteK528]

I was thinking if more optimization is needed for the orientation estimation using Kalman Filter such as getting rid of the Eigen library in favour of replacing matrix multiplications by simple multiplications on raw arrays, but after some measurements I think the performance of the code using Eigen library, especialy when compiled with -Ofast optimization is furiously fast -> loop is executed in $72 \mu s$.