micropython-imu / micropython-fusion Goto Github PK

Hello!
I have just included the library in a python project, in a RaspberryPi 0 2w, with python3.7 and an ICM20948 9DOF IMU as it follows:

`
from icm20948 import ICM20948
import time
import gc
from fusion import Fusion

def time_diff(start, end):
return (start - end)/1000000

imu = ICM20948()
fuse = Fusion(time_diff)

count = 0
while True:
x, y, z = imu.read_magnetometer_data()
ax, ay, az, gx, gy, gz = imu.read_accelerometer_gyro_data()
ts = time.time()
fuse.update([ax, ay, az], [gx, gy, gz], [x, y, z], ts) # Note blocking mag read
if count % 25 == 0:
print("{:7.3f} {:7.3f} {:7.3f} - {:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}|{:05.2f}".format(
fuse.heading, fuse.pitch, fuse.roll, ax, ay, az, gx, gy, gz, x, y, z))
count += 1
`

It seems to work errorless but the fused values (heading, pitch and roll) are either static or either the variation is negligible:

`
heading pitch roll. ax ay az. gx gy gz x y z

-0.002 0.005 0.005 - 00.07|00.00|01.04|00.05|00.21|00.27|-12.30|35.10|94.65
-0.002 0.005 0.005 - 00.07|00.00|01.04|00.10|00.42|00.13|-12.90|33.90|96.00
-0.002 0.005 0.005 - 00.07|00.00|01.04|-0.05|00.26|00.26|-12.90|35.70|96.00
-0.002 0.005 0.005 - 00.07|00.01|01.04|00.22|00.33|00.40|-11.85|35.70|94.95
-0.002 0.005 0.005 - 00.07|00.01|01.04|00.24|00.40|00.30|-13.35|36.15|96.00
-0.002 0.005 0.005 - 00.15|-0.25|01.01|-78.54|18.65|-5.38|-16.20|26.40|95.55
-0.002 0.005 0.005 - 00.09|-0.65|00.76|-3.21|03.21|-7.35|-13.35|09.75|87.00
-0.002 0.005 0.005 - 00.16|-0.77|00.64|-8.72|-9.74|-2.31|-18.15|02.55|83.85
-0.002 0.005 0.005 - -0.06|00.24|00.99|110.12|-7.94|23.41|-9.75|48.60|88.80
-0.002 0.005 0.005 - -0.17|00.35|00.98|-91.23|-17.25|-118.19|-4.50|49.65|84.60
-0.002 0.005 0.005 - 00.50|-0.52|00.94|-41.48|-78.02|-17.75|-44.70|-6.30|80.55
-0.002 0.005 0.005 - 00.54|-0.61|00.61|149.80|40.49|208.66|-37.35|-11.10|75.30
-0.002 0.005 0.005 - 00.19|00.33|00.92|01.63|-1.18|-124.72|-16.05|59.55|90.45
-0.002 0.005 0.005 - 00.11|00.16|00.94|-4.61|-2.31|09.70|-10.35|49.05|90.30
-0.002 0.005 0.005 - 00.09|00.09|01.05|-24.18|05.10|-32.29|-13.35|47.10|95.25
-0.002 0.005 0.005 - 00.06|-0.12|00.98|-0.69|-16.33|-36.27|-12.30|20.70|91.50
-0.002 0.005 0.005 - 00.31|-0.15|01.00|-4.06|-1.60|-28.98|-32.55|01.05|91.20
-0.002 0.005 0.005 - 00.12|-0.11|00.94|24.14|10.39|88.27|-15.45|21.15|93.60
-0.002 0.005 0.005 - -0.11|00.71|00.62|42.82|13.69|10.38|-9.75|66.15|72.75
-0.002 0.005 0.005 - -0.18|00.63|00.85|-108.66|-38.09|-25.57|-7.95|63.90|74.85
-0.002 0.005 0.005 - 00.21|-0.79|00.60|-49.85|-16.36|-5.11|-17.85|05.85|85.95
-0.002 0.005 0.005 - -0.48|-0.48|00.77|75.34|91.73|75.86|-1.50|18.45|71.55
-0.002 0.006 0.005 - -0.89|-0.06|00.61|-16.23|-110.88|-1.24|03.15|35.70|54.00
-0.002 0.006 0.005 - 00.34|00.13|00.95|20.72|-48.60|12.53|-21.15|45.75|97.20
-0.002 0.006 0.005 - 00.16|00.98|00.32|112.11|84.21|-46.33|-11.85|69.45|66.60
-0.002 0.006 0.005 - 00.02|-1.00|00.92|-23.82|-179.43|-74.77|-14.40|03.60|81.45
-0.002 0.006 0.005 - -0.54|00.90|00.02|-8.20|62.37|-83.08|-4.50|58.05|46.80
-0.002 0.006 0.005 - 00.21|-0.54|01.05|158.70|-11.55|25.06|-19.50|21.00|96.90
-0.002 0.006 0.005 - -0.13|-0.40|01.02|-95.34|-34.41|-24.75|-6.30|25.95|87.00
-0.002 0.006 0.005 - 00.12|00.09|01.00|11.50|-1.62|-9.82|-15.00|41.25|95.25
-0.002 0.006 0.005 - 00.09|00.14|01.04|12.60|05.79|12.21|-12.00|45.15|91.05
-0.002 0.006 0.005 - -0.02|00.36|00.97|29.73|14.14|-9.98|-10.95|42.15|93.75
-0.002 0.006 0.005 - 00.12|-0.02|00.96|02.65|10.74|-18.39|-12.45|42.15|94.50
-0.002 0.006 0.005 - 00.03|00.08|01.09|02.56|06.44|-2.18|-9.45|42.00|92.55
-0.002 0.006 0.005 - 00.05|00.05|01.03|-17.07|-4.51|02.40|-13.65|42.00|93.60
-0.002 0.006 0.005 - 00.11|00.00|01.01|-0.56|-1.44|-1.69|-12.90|40.95|94.95
-0.003 0.006 0.005 - -0.10|00.62|01.14|-9.95|-59.97|-107.58|-12.30|39.75|96.00
-0.003 0.006 0.005 - -0.87|-0.12|01.11|-19.57|-140.28|12.60|-13.65|39.00|95.25
-0.003 0.006 0.005 - -0.12|00.00|00.46|-17.89|62.86|-23.93|-14.10|41.40|93.00
-0.003 0.006 0.005 - 00.78|00.04|01.04|-82.00|93.40|-250.14|-15.00|35.10|96.00
-0.003 0.006 0.006 - 00.45|00.27|01.00|-8.88|35.37|71.79|-18.00|48.30|96.60
-0.003 0.006 0.006 - 00.09|00.05|01.06|04.44|25.15|-34.18|-14.10|42.15|96.60
-0.003 0.006 0.006 - -0.07|00.85|00.59|13.27|-0.63|-1.90|-10.05|66.90|71.40
-0.003 0.006 0.006 - 00.18|-0.84|00.68|65.41|03.37|13.23|-19.05|03.45|85.80
-0.003 0.006 0.006 - 00.09|00.04|01.03|-1.47|-14.40|-0.32|-10.80|39.00|94.65
-0.003 0.006 0.006 - 00.06|00.00|01.04|00.35|00.21|-0.03|-12.30|39.45|96.75
-0.003 0.006 0.006 - 00.06|00.01|01.04|00.15|-0.03|00.34|-12.90|39.00|94.65
-0.003 0.006 0.006 - 00.07|00.00|01.06|-0.11|00.13|00.38|-11.40|39.60|94.50
`

Can you help me?

Hi @peterhinch

when I try to run the fusion test.py for MPU9250, there is code for external switch. Could you explain what that is and how to define it? I am using ESP32 board.

thx

Dear Peter,

I am using your library on a wearable robot I am developing. I don't really need the heading but only the rotation on the sagittal plane ( or on what should be parallel to it), therefore i am using the update no mag. This, in your convention and according to how the sensor is placed, is the pitch angle. Together with it, i got a high level of noise on the roll axis, can you anyhow explain you me why, and how i can tackle it?

Moreover, do you know on which algorithm of data fusion it is based on? I would say a gradient descent one, but maybe you can give me some more reference.

Best,

Pietro

Hello, I don't understand why beta is set to be sqrt(3/4)*GyroMeasError
In the original study, beta is indeed set this way but there is no explanation.

I read that beta is similar to I in a PID control sense, I don't really understand why, could someone please explain?

Thanks in advance

Dear Peter,

I am working on an off-road robot that uses a gps module and the MPU9250 to navigate between waypoints. I am using your library for the MPU9250 together with this library to get readings for heading, and I'm trying to set up a PID controller to get to the waypoints. I have a couple of questions regarding this setup.

• Do I need to calibrate the MPU9250 separately, save the coefficients and then calibrate the Fuse object as well, or is it enough to only calibrate the Fuse?

• I have a separate thread for the imu which is continuously updated. The Fusion algorithm is run at 1.5 kHz to get better convergence, and my PID loop runs at 300 Hz. Is this setup sensible?

• Finally, I would like to ask if you have any guidance about which low pass filter I should use on the imu.

Best regards,
Ben

Hello,

Could anyone explain how to define the I2C pin and id when call the MPU6050 function? I am trying to use MPU6050 on ESP32 in MicroPython. my i2c pin are 4 and 5 and the i2c address is 0x68.

thx

So I see how we get heading, pitch, roll. How to get position updates?

Hello !

I took some data from my phone, and put it into a csv file sensor_data.csv. The sensor data looks like this :

I can already take the orientation but there's too much noise, so I'm hoping making the noise disappear with your code :

I made a code taking the data, applying the functions Fusion.calibrate(), and Fusion.update(), but the output isn't like my orientation data :

import csv
import matplotlib.pyplot as plt
import math
from fusion import Fusion
from math import radians

def timediff(t_new, t_old):
    dt = t_new - t_old
    return dt

class Getxyz:
    def __init__(self, L_comp_xyz):
        self.L_comp_xyz = L_comp_xyz
        self.i = -1

    def __call__(self):
        self.i += 1
        if self.i == len(self.L_comp_xyz):
            return False
        return self.L_comp_xyz[self.i]

class Stopfunc:
    def __init__(self, L):
        self.length = len(L)
        self.i = -1
    
    def __call__(self):
        self.i += 1
        if self.i == self.length - 1:
            return True
        else:
            return False


fuse = Fusion(timediff)

# Opening csv file, taking out header :
csv_name = "sensor_data.csv"
file = open(csv_name)
csvreader = csv.reader(file)
header = next(csvreader)
print(header)

L_time = []
L_orientation_azimuth = []
L_orientation_pitch = []
L_orientation_roll = []
L_accel_X = []
L_accel_Y = []
L_accel_Z = []
L_gyro_X = []
L_gyro_Y = []
L_gyro_Z = []
L_compass_X = []
L_compass_Y = []
L_compass_Z = []
L_comp_xyz = []

# Taking data :
rows = []
for row in csvreader:
    L_time.append(round(float(row[0]), 3))
    L_orientation_azimuth.append(round(float(row[1]), 3))
    L_orientation_pitch.append(round(float(row[2]), 3))
    L_orientation_roll.append(round(float(row[3]), 3))

    L_gyro_X.append(round(float(row[4]), 3))
    L_gyro_Y.append(round(float(row[5]), 3))
    L_gyro_Z.append(round(float(row[6]), 3))

    L_accel_X.append(round(float(row[7]), 3))
    L_accel_Y.append(round(float(row[8]), 3))
    L_accel_Z.append(round(float(row[9]), 3))

    L_compass_X.append(round(float(row[10]), 3))
    L_compass_Y.append(round(float(row[11]), 3))
    L_compass_Z.append(round(float(row[12]), 3))

    comp = (float(row[10]), float(row[11]), float(row[12]))
    L_comp_xyz.append(comp)

# Calibrating :
calibrate = False # Doesn't change the results ...
if calibrate:
    getxyz = Getxyz(L_comp_xyz)
    stopfunc = Stopfunc(L_comp_xyz)
    fuse.calibrate(getxyz, stopfunc)
    print("fuse.magbias : ", fuse.magbias)


# Applying the filter fuse.update() :
L_heading = []
L_pitch = []
L_roll = []

for i in range(len(L_time)):
    ts = L_time[i]
    acc = (L_accel_X[i], L_accel_Y[i], L_accel_Z[i])
    gyro = (L_gyro_X[i], L_gyro_Y[i], L_gyro_Z[i])
    comp = (L_compass_X[i], L_compass_Y[i], L_compass_Z[i])
    
    fuse.update(acc, gyro, comp, ts)

    L_heading.append(radians(fuse.heading))
    L_pitch.append(radians(fuse.pitch))
    L_roll.append(radians(fuse.roll))

# Compairing phone orientation with the filter's results on matplotlib :
display_results = True
if display_results:

    print("last time : ", L_time[-1])
    fig, axs = plt.subplots(3)
    axs[0].set_xlim(0, 100)
    axs[0].set_ylim(-math.pi, math.pi)

    axs[0].set_title('heading')
    line1 = axs[0].plot(L_time, L_orientation_azimuth, 'b-', label="phone orientation")
    line2 = axs[0].plot(L_time, L_heading, 'g-', label="fuse orientation")
    axs[0].set_ylabel("rad")

    axs[1].set_title('pitch')
    line1 = axs[1].plot(L_time, L_orientation_pitch, 'b-', label="phone orientation")
    line2 = axs[1].plot(L_time, L_pitch, 'g-', label="fuse orientation")
    axs[1].set_ylabel("rad")

    axs[2].set_title('roll')
    line1 = axs[2].plot(L_time, L_orientation_roll, 'b-', label="phone orientation")
    line2 = axs[2].plot(L_time, L_roll, 'g-', label="fuse orientation")
    axs[2].legend()
    axs[2].set_ylabel("rad")
    axs[2].set_xlabel("seconds")
    
    plt.show()

Results :

Do you have any idea why heading, pitch and roll of fuse aren't following the expected orientation ?

This use case can be added to your repository if you like it.

BR

Loic

I used kivy , plyer and socket to get this data.

I am having a problem importing the pub library where I can I find it. Tried using pip, and didn't have any luck.

I'm trying to get pitch and roll values that in some way correspond to the real pitch and roll of the device. However, whenever I do a motion, the values seem to drift back to roughly where they started even if the Wii Remote is now in a different orientation. This is the script that I'm currently using:

from datetime import datetime
from fusion import Fusion
import cwiid
import time

def timediff(time1, time2):
    return (time1-time2)


# This part is for connecting the Wii Remote.
wiimote = cwiid.Wiimote()
time.sleep(1)
wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS | cwiid.RPT_IR
wiimote.enable(cwiid.FLAG_MOTIONPLUS)
time.sleep(1)

fuse = Fusion(timediff)

count = 0
while True:
    # This ordering of values seems to correspond most closely to the order yaw, pitch, and then roll. Though, the "yaw" doesn't give intuitive values.
    # Tilting the Wiimote upward in pitch, the ['acc'][1] gets lower. Tilting it down, the ['acc'][1] gets higher.
    # Rolling it counterclockwise, the ['acc'][0] gets lower. Rolling clockwise, the ['acc'][0] value gets higher.
    accel = (wiimote.state['acc'][2], wiimote.state['acc'][1], wiimote.state['acc'][0])

    angle_rates = wiimote.state['motionplus']['angle_rate']

    # This ordering of values seems to correspond most closely to the order yaw, pitch, and then roll.
    # By tilting the Wiimote upward in pitch, the angle_rates[0] gets momentarily lower. Downward pitch gets it to go higher.
    # By rolling the Wiimote counterclockwise, the angle_rates[1] gets momentarily higher. Rolling it clockwise, it gets lower.
    # By swaying the Wiimote toward the left in yaw, the angle_rates[2] gets momentarily higher. Swaying it toward the right, it gets lower.

    # Explanation for the dividing by low_speed times something bit:
    # When low_speed is 1, then 20 represents turning at about 1 degree per second. So divide by 20 to get the degrees per second.
    # At high speed (Low speed bit = 0) 20 represents turning at about 5 degree per second. So divide by 4 to get the degrees per second.
    # Source: http://arduino-projects4u.com/wii-motion-plus/

    # The -7945, -8115 and -7964 calibrate the values so that for me the gyro values will be close to zero when the Wii Remote is resting on the table.
    gyro = (
        (angle_rates[2]-7945)/(4+wiimote.state['motionplus']['low_speed'][2]*16),
        (angle_rates[0]-8115)/(4+wiimote.state['motionplus']['low_speed'][0]*16),
        (angle_rates[1]-7964)/(4+wiimote.state['motionplus']['low_speed'][1]*16),
    )
    fuse.update_nomag(accel, gyro, time.time())
    if count % 5 == 0:
        print(accel, gyro)
        print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
    time.sleep(0.02)
    count += 1

What am I missing or doing wrong?

It would be useful for the update methods to accept a timestamp at which the sensor data was recorded, rather than use time.now which assumes a constant latency between sensor reading and calling the update function.

I have an ICM 20948 IMU (another Invensense) on a Raspberry Pi.

I can make the accelerometer and gyro fusion work well. The tilt and roll make sense when using the update_nomag, or when updating with the mag.

I have been able to calibrate the magnetometer for hard iron offsets - using scatter plot convergence similar to https://learn.adafruit.com/adafruit-sensorlab-magnetometer-calibration/calibration-with-raspberry-pi-using-blinka.

My problem is that heading reads a value of -32 for the heading when facing North, -81 facing East. -62 facing south and -85 facing west.

Have you got any ideas?

Hi @peterhinch

Thank you for your help on MPU6050. It works now, but the Heading is always 0. is it normal for 6050?

thx

Update time (uS): 1041
Heading, Pitch, Roll: 0.000 -0.011 -0.004
Heading, Pitch, Roll: 0.000 -5.174 -3.649
Heading, Pitch, Roll: 0.000 -5.389 -2.731
Heading, Pitch, Roll: 0.000 -29.774 -9.453
Heading, Pitch, Roll: 0.000 -5.891 -1.644