Python and C++ codes give different orientations.

[LarryDong]

Hi, thanks for sharing the codes. Great work!

I tested your codes, the python (Python/simple_example.py) and c++ version (Examples/Simple/main.cpp).
I saved two results into a text file and plot by matlab.
However, two curves seems similar but not totally the same.

See that the roll and pitch are slightly different at 71s.

---

I'm sure is not at calculation precision issue. I also tried this:
I generated an arbitrary angle as well as translation (the right one in following picture), and simulate the IMU's output without noise.
Then I used both python and c++ codes to estimate the orientation with the simulated IMU output.

You can see that the pitch (green line) from 3-5 seconds in the left picture (python) and middle picture (c++) are opposite, while the ground truth (right picture) is same as c++'s.
Also, the python's roll and pitch curves between 7-9s, at around 10s, are also different from c++'s and ground truth.

[xioTechnologies]

I would like to first re-describe the problem to ensure that I have understood. You have used a simulator to generate ideal IMU data alongside Euler angles. You then processed this IMU data separately through the Fusion C library and Python package. The Euler angles obtained from the C library match those of the simulator. The Euler angles obtained from the Python package do not, and differ significantly.

Please can you confirm that you are using Fusion to convert the quaternion to Euler angles in both cases. Not all libraries provide the same result for a quaternion to Euler angle conversion, as reported here.

[LarryDong]

Hi, thanks for your reply.

You have used a simulator to generate ideal IMU data alongside Euler angles. You then processed this IMU data separately through the Fusion C library and Python package. The Euler angles obtained from the C library match those of the simulator. The Euler angles obtained from the Python package do not,

Yes, you're right.
And I also pasted the first figure, which is the sensor_data.csv provided by Fusion, processed by both python and c++. You can still see the difference from 71s to 72s. Maybe you could test it.
(You may say that my "ground truth" euler conversion is not consistent with Fusion, but the Fusion's python and c++ should be the same I guess).

---

But the issue may be not in euler conversion!
I've checked the euler conversions. I was using Fusion python's quaternion.to_euler() and c++'s FusionQuaternionToEuler for conversion.
The quaterion from python and c++ are different.
Here I also plot the quaterion from: python, c++, and ground truth (for my simulation data).

The data from sensor_data.csv provided by Fusion, python v.s. c++.
Check the x, y at 71s.

The data from my simulation: python v.s. c++ v.s. ground truth.
Check the x,y from 4 to 8s.

[xioTechnologies]

I created the following main.c file to process sensor_data.csv and and write the quaternion output to file.

#include "Fusion/Fusion.h"
#include <stdio.h>

int main()
{
    FusionAhrs ahrs;
    FusionAhrsInitialise(&ahrs);

    FILE *const inputFile = fopen("sensor_data.csv", "r");
    FILE *const outputFile = fopen("main.csv", "w");

    char csvLine[256];
    while (fgets(csvLine, sizeof(csvLine), inputFile) != NULL)
    {
        float timestamp;
        FusionVector gyroscope;
        FusionVector accelerometer;
        if (sscanf(csvLine, "%f,%f,%f,%f,%f,%f,%f",
                   &timestamp,
                   &gyroscope.axis.x,
                   &gyroscope.axis.y,
                   &gyroscope.axis.z,
                   &accelerometer.axis.x,
                   &accelerometer.axis.y,
                   &accelerometer.axis.z) != 7)
        {
            continue;
        }

        FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer, 1.0f / 100.0f); // 100 Hz sample rate

        const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&ahrs);

        fprintf(outputFile, "%f,%f,%f,%f,%f\n",
                timestamp,
                quaternion.element.w,
                quaternion.element.x,
                quaternion.element.y,
                quaternion.element.z);
    }

    fclose(inputFile);
    fclose(outputFile);

    return 0;
}

I then created the following Python script to process the sensor_data.csv, and plot the result alongside the output of main.c.

import imufusion
import matplotlib.pyplot as pyplot
import numpy

# Calculate Python quaternion
data = numpy.genfromtxt("sensor_data.csv", delimiter=",", skip_header=1)

timestamp = data[:, 0]
gyroscope = data[:, 1:4]
accelerometer = data[:, 4:7]

ahrs = imufusion.Ahrs()
quaternion = numpy.empty((len(timestamp), 4))

for index in range(len(timestamp)):
    ahrs.update_no_magnetometer(gyroscope[index], accelerometer[index], 1 / 100)  # 100 Hz sample rate
    quaternion[index] = ahrs.quaternion.array

# Plot Python quaternion
_, axes = pyplot.subplots(nrows=3, sharex=True)

axes[0].plot(timestamp, quaternion[:, 0], "tab:gray", label="W")
axes[0].plot(timestamp, quaternion[:, 1], "tab:red", label="X")
axes[0].plot(timestamp, quaternion[:, 2], "tab:green", label="Y")
axes[0].plot(timestamp, quaternion[:, 3], "tab:blue", label="Z")
axes[0].set_title("Python quaternion")
axes[0].grid()
axes[0].legend()

# Plot C quaternion
data = numpy.genfromtxt("main.csv", delimiter=",")

c_timestamp = data[:, 0]
c_quaternion = data[:, 1:5]

axes[1].plot(c_timestamp, c_quaternion[:, 0], "tab:gray", label="W")
axes[1].plot(c_timestamp, c_quaternion[:, 1], "tab:red", label="X")
axes[1].plot(c_timestamp, c_quaternion[:, 2], "tab:green", label="Y")
axes[1].plot(c_timestamp, c_quaternion[:, 3], "tab:blue", label="Z")
axes[1].set_title("C quaternion")
axes[1].grid()
axes[1].legend()

# Plot residual
residual = quaternion - c_quaternion

axes[2].plot(timestamp, residual[:, 0], "tab:gray", label="W")
axes[2].plot(timestamp, residual[:, 1], "tab:red", label="X")
axes[2].plot(timestamp, residual[:, 2], "tab:green", label="Y")
axes[2].plot(timestamp, residual[:, 3], "tab:blue", label="Z")
axes[2].set_title("Residual")
axes[2].set_xlabel("Seconds")
axes[2].grid()
axes[2].legend()

pyplot.show()

This provides the following plot. The residual is the difference between the two quaternions. The difference is of the order 10^-6. This is within the expected precision. I was not able to replicate the significant differences you have reported. The issue seems to be specific to your system.

[LarryDong]

Hi, thanks for sharing the test codes. It works well here.
I would check my own codes. Many thanks for your quick and detailed reply.