Hello,
I am having a few issues while using the Madgwick() class in this library and was wondering if maybe someone here might be able to assist me.
Setup
I am using an Arduino Nano BLE 33 Sense which uses an LSM9DS1 9-axis iNEMO IMU.
Raw Accelerometer, Gyroscope, and Magnetometer values are read on the arduino, then corrected using calibration values. These values (x, y, z for each sensor) are then sent over USB serial to my host machine at a baud rate of 115,200. The sample rates are below:
- Accelerometer: 116.79Hz
- Gyroscope: 116.79
- Magnetometer: 38.94 Hz
I'm unable to set all three of these sample rates to the same values due to their hardware specifications.
Then in python I am opening a serial connection to the arduino and reading this data in to be processed using the AHRS library.
Each line of output from the arduino is of the form ax,ay,az,gx,gy,gz,mx,my,mz
import serial
import numpy as np
import ahrs
q0 = [1.0, 154.0, -1.0, -0.5] #resting position of device
q_prev = q0
madgewick = ahrs.filters.Madgewick(q0=q0)
while 1:
ser = serial.Serial('COM12', 115200)
line = ser.readline().decode('utf8').strip()
line = [float(x) for x in line.split(',')]
acc = np.array(line[0:3])
gyr = np.array(line[3:6])
mag = np.array(line[6:9])
attitude = madgwick.updateMARG(q_prev, acc=acc, gyr=gyr, mag=mag)
q_prev = attitude
I then convert the attitude to yaw/pitch/roll format and use pygame to display a cube which has its orientation set to these values.
I also do some simple value clipping to de-noise the values coming in from the arduino sensor since they hover around +/- 0.25 when stationary.
Problem
When I rotate my arduino, the cube that I am displaying will rotate properly for the most part, but when I stop rotating the arduino, the cube will slowly dift back to what appears to be my q0 value that I set during instantiation of the Madgwick class.
This issue does not occur when I re-instantiate the Madgwick class each time I get new data as shown below:
import serial
import numpy as np
import ahrs
while 1:
ser = serial.Serial('COM12', 115200)
line = ser.readline().decode('utf8').strip()
line = [float(x) for x in line.split(',')]
acc = np.array(line[0:3])
gyr = np.array(line[3:6])
mag = np.array(line[6:9])
attitude = ahrs.filters.Madgwick(acc=acc, gyr=gyr, mag=mag).Q[-1]
The other problem I have is that the yaw is being heavily influenced by the pitch/roll values. For example, let's say that I rotate my arduino about a central fixed point on the board, similar to an euler's disk (see gif below).
![unnamed](https://user-images.githubusercontent.com/16657209/128086360-e5c7ad26-cdb7-44ba-bc7b-ac5d089d37f8.gif)
When I do this, the yaw will rotate 360 degrees and continue rotating as I continue to pivot the board. I expect a bit of motion along this axis but it should move one direction then reverse and return to the origin, not continue rotating without reversing. Again, refer to the gif above and pay attention to the yellow band on the ring, notice how the yellow part of the ring travels around in a circle.
So my question is this: Am I misunderstanding the fundamentals of the Madgwick functions or am I doing something else completely wrong in this process?
Thank you in advance for your time.