Madgwick: Attitude Value Drift and Yaw Alignment Issues

[bryangreener]

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).

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.

[jeffreymonaco]

@bryangreener

The Madgwick filter has a Dt parameter that corresponds to the sampling rate. Its default value in the current AHRS Magdwick class is 0.01 seconds. Your sample code does not set this parameter, so there is likely a mismatch between your data and what the algorithm expects given the description of your setup. This will factor into discrepancies between the actual quaternion and its propagation via integration.

The AHRS Madgwick class also expects your data to be provided with units of rad/s for the gyro measurements, m/s**2 for the accelerometer measurements, and mT for the magnetometer measurements. What are the units of your data from the device you are using?

NOTE I suspect that the accelerometer and magnetometer units do not actually matter in the implementation, b/c these are scaled by their 2-norm in the algorithm. However, the physical units of the gyroscope measurements are required to be in rad/s given the quaternion differential equation to propagate those.

As an aside, it is not clear what your initial quaternion represents physically. This is typically given as a unit quaternion. That said, this too probably does not matter given the protections in the AHRS Madgwick class to enforce a unit quaternion via normalization.