Gyroscope + Accelerometer Integration

[shaileshadh]

Thank you for this nice work. I am using it to get data from the sensor to Raspberry pi. Is it also possible to read both Accelerometer and Gyroscope values at once?

[Henrik Blidh]

I have not actually tested that myself, but it should work just fine. The callback solution is threaded and should route values from each sensor to your designated functions:

import time
from pymetawear.client import MetaWearClient

c = MetaWearClient("XX-XX-XX-XX-XX-XX", 'pygatt')

acc_data = []
gyro_data = []

def acc_callback(data):
    acc_data.append(data)

def gyro_callback(data):
    gyro_data.append(data)

# Set the desired measurement ranges, etc.
c.accelerometer.set_settings(data_rate=100.0, data_range=4.0)
c.gyroscope.set_settings(data_rate=100.0, data_range=1000.0)

# Activate notifications
c.accelerometer.notifications(acc_callback)
c.gyroscope.notifications(gyro_callback)

time.sleep(20.0)

# Deactivate notifications
c.accelerometer.notifications(None)
c.gyroscope.notifications(None)

# Do what you want with the recorded data, e.g. plot it with Matplotlib
import matplotlib.pyplot as plt
f, (ax1, ax2) = plt.subplots(1, 2, sharex=True)
ax1.plot([d[0] for d in acc_data], [d[1:] for d in acc_data])
ax1.set_title('Accelerometer')
ax2.plot([d[0] for d in gyro_data], [d[1:] for d in gyro_data])
ax2.set_title('Gyroscope'')
plt.show()

Wrote this without testing, but I think it is close to correct at least. The sliced data in the plot commands might need to be transposed.

[shaileshadh]

Thank You! I will give it a try!

[shaileshadh]

Sorry for writing again, but wanted to ask if you have implemented some filters to integrate these values? I am trying to implement a Complementary filter to get the resulting pitch, roll and yaw values, but am not getting good results.
Any help or hints will be appreciated. Thank you!

[Henrik Blidh]

I have not done any such work for the MetaWear sensors yet, but a lot for other types of accelerometer and gyroscope sensors. Check out Mbientlabs blog where they do sensor fusion if you haven't done that yet. There you have some pseudo code solution to use as a basis for your experiments.

If you manage to get something working with PyMetaWear, feel free send it to me and I will include it as an example for others!

[shaileshadh]

The implementation of sensor fusion has not been clearly mentioned there.
If I found some solutions will definitely let you know.

[Henrik Blidh]

Something on the lines of this. N.B. I have not tested this yet, but go ahead and try it if you want to.

from math import atan2, pi

# Fill in the frequency of your data, in Hertz.
data_frequency = 50
# Desired fusion coefficient.
coeff = 0.98
# Timestep
timestep = 1.0 / data_frequency

def get_pitch_roll_from_accelerometer(acc):
    pitch = atan2(acc[1], acc[2]) * 180 / pi;
    roll = atan2(acc[0], acc[2]) * 180 / pi;
    return pitch, roll

def update_angle(old_pitch, old_roll, k, dt, acc, gyro):
    acc_pitch, acc_roll = get_pitch_roll_from_accelerometer(acc)
    new_pitch = k * (old_pitch + gyro[0] * dt) + (1 - k) * acc_pitch
    new_roll = k * (old_roll + gyro[1] * dt) + (1 - k) * acc_roll
    return new_pitch, new_roll

pitch, roll = get_pitch_roll_from_accelerometer(acc_data[0])
for a, g in zip(acc_data, gyro_data):
    pitch, roll = update_angle(pitch, roll, coeff, timestep, a, g)

Refences:

• Reading a IMU Without Kalman: The Complementary Filter
• INTRODUCTION TO SENSOR FUSION

[shaileshadh]

Thanks I will test this and will let you know the results!

[shaileshadh]

I could not figure out the way to integrate the code with previous one and work with both accelerometer and gyroscope values.

I used the same logic in my java program but the results are not not even close.
`double dt=0.02; //50 samples per sec
accPitch = Math.atan2(ay,az) * 180/Math.PI;
accRoll = Math.atan2(ax,az) * 180/Math.PI;

newPitch= (0.98 * (oldValue+gx * dt))+(0.02 * accPitch);
newRoll= (0.98 * (oldValue+gy * dt))+(0.02 * accRoll);
`

[Henrik Blidh]

Not sure if you are still interested, but I finished a complimentary filter example and added to the new 0.7.0 release today.