DMP quaternions drifting, but absolute acceleration not drifting

[decamun]

Subject of the issue

It seems that the DMP quaternions are drifting, even though the absolute acceleration reading is not.

Your workbench

• Are you using a microSD card? Yes, 1GB, formatted out of the box from sparkfun
• At what frequency are you logging? 100Hz
• What version of firmware are you using? Artemis Openlog v1.11
• How is OpenLog Artemis wired to your sensor(s)? Board only
• How is everything being powered? USB-C
• Are there any additional details that may help us help you?

Steps to reproduce

Here is a test script I put together to visualize the quaternions in real time.:

#Script adapted from: https://toptechboy.com/9-axis-imu-lesson-19-vpython-visualization-of-pitch-and-yaw/
from vpython import *
from time import *
import numpy as np
from scipy.spatial.transform import Rotation
import math
import serial
ad=serial.Serial('com4',115200)
sleep(1)

def is_number(n):
    try:
        float(n)   # Type-casting the string to `float`.
                   # If string is not a valid `float`, 
                   # it'll raise `ValueError` exception
    except ValueError:
        return False
    return True

def check_packet_valid(packet):
    if len(packet) < 16:
        return False
    for n in packet:
        if not is_number(n):
            return False
    return True

def with_scalar_component(qs):
    qs_wr = np.asarray([*qs, np.sqrt(1.0 - np.sum(np.multiply(qs, qs)))]) # Real last
    qs_wr[np.isnan(qs_wr)] = 0
    return qs_wr

def rotated_vector(r, v):
    k=r.apply(v)
    return vector(k[0],k[1],k[2])

#setup VPython scene
scene.range=5
toRad=2*np.pi/360
toDeg=1/toRad
scene.forward=vector(1,-1,1)
scene.up=vector(0,0,-1)

scene.width=600
scene.height=600

frontArrowQuat=arrow(shaftwidth=.1,color=(color.red*0.5),axis=vector(1,0,0))
sideArrowQuat=arrow(shaftwidth=.1,color=(color.green*0.5),axis=vector(0,0,1))
upArrowQuat=arrow(shaftwidth=.1,color=(color.blue*0.5),axis=vector(0,1,0))

accArrow=arrow(shaftwidth=.1,color=(color.yellow*0.5),axis=vector(0,0,1))
magArrow=arrow(shaftwidth=.1,color=(color.purple*0.5),axis=vector(0,0,1))

#loop
while (True):
    while (ad.inWaiting()==0):
        pass
    dataPacket=str(ad.readline())
    splitPacket=dataPacket.split("'")[1].split(",")[:-1] #strip off unicode characters and split the string into a list

    #Log data to terminal
    print(str(splitPacket))

    if check_packet_valid(splitPacket):

        #Parse packet
        Q6_1=float(splitPacket[0])
        Q6_2=float(splitPacket[1])
        Q6_3=float(splitPacket[2])
        AX=float(splitPacket[7])
        AY=float(splitPacket[8])
        AZ=float(splitPacket[9])
        MX=float(splitPacket[13])
        MY=float(splitPacket[14])
        MZ=float(splitPacket[15])

        #Generate Quaternion
        Q6V = with_scalar_component([Q6_1, Q6_2, Q6_3])
        Q = Rotation.from_quat(Q6V)

        #Use Quaternion to rotate the unit vectors
        v1=rotated_vector(Q, np.array([1,0,0]))
        v2=rotated_vector(Q, np.array([0,1,0]))
        v3=rotated_vector(Q, np.array([0,0,1]))

        #Paint Quaternion unit vectors
        frontArrowQuat.axis=v1
        sideArrowQuat.axis=v2
        upArrowQuat.axis=v3

        #Paint abs acceleration and mag vector
        accArrow.axis=vector(AX,AY,AZ)
        accArrow.length = accArrow.length/10000
        magArrow.axis=vector(MX,MY,MZ)
        magArrow.length = magArrow.length/100

Here is my settings file:

sizeOfSettings=168
olaIdentifier=283
nextSerialLogNumber=4
nextDataLogNumber=136
usBetweenReadings=10000
logMaxRate=0
enableRTC=1
enableIMU=1
enableSD=1
enableTerminalOutput=1
logDate=0
logTime=0
logData=1
logSerial=1
logIMUAccel=1
logIMUGyro=1
logIMUMag=1
logIMUTemp=1
logRTC=1
logHertz=1
correctForDST=0
americanDateStyle=1
hour24Style=1
serialTerminalBaudRate=115200
serialLogBaudRate=9600
showHelperText=1
logA11=0
logA12=0
logA13=0
logA32=0
logAnalogVoltages=1
localUTCOffset=0
printDebugMessages=0
powerDownQwiicBusBetweenReads=0
qwiicBusMaxSpeed=100000
qwiicBusPowerUpDelayMs=250
printMeasurementCount=0
enablePwrLedDuringSleep=1
logVIN=0
openNewLogFilesAfter=0
vinCorrectionFactor=1.47
useGPIO32ForStopLogging=0
qwiicBusPullUps=1
outputSerial=0
zmodemStartDelay=20
enableLowBatteryDetection=0
lowBatteryThreshold=3.40
frequentFileAccessTimestamps=0
useGPIO11ForTrigger=0
fallingEdgeTrigger=1
imuAccDLPF=0
imuGyroDLPF=0
imuAccFSS=0
imuAccDLPFBW=7
imuGyroFSS=0
imuGyroDLPFBW=7
logMicroseconds=0
useTxRxPinsForTerminal=0
timestampSerial=0
timeStampToken=10
useGPIO11ForFastSlowLogging=0
slowLoggingWhenPin11Is=0
useRTCForFastSlowLogging=0
slowLoggingIntervalSeconds=300
slowLoggingStartMOD=1260
slowLoggingStopMOD=420
resetOnZeroDeviceCount=0
imuUseDMP=1
imuLogDMPQuat6=1
imuLogDMPQuat9=1
imuLogDMPAccel=1
imuLogDMPGyro=1
imuLogDMPCpass=1
minimumAwakeTimeMillis=0
identifyBioSensorHubs=0

Expected behavior

The absolute accel vector remains aligned with the absolute Z axis in the visualization in a robust way. No matter how I move the board in real space, it keeps track of which way is down.

I would expect that when I use this script, the 'yaw axis' of the transformed coordinate system in the visualization should be roughly the same as the absolute accel vector. In other words, when I spin the board in real space, the rotated coordinate system in the visualization should roughly spin about the absolute accel vector in the visualization.

Actual behavior

Manually spinning the board and watching the visualization, it is possible to see that initially the "yaw" axis of the board is aligned with the absolute acceleration vector as expected. Unfortunately over time it drifts such that spinning the board in real space causes it to rotate in other ways in the visualization.

As I mentioned, the absolute accel vector remains stable throughout this motion. (The absolute acceleration remains aligned with the absolute Z axis in the visualization, even if the board is rotated around in real space)

My conclusion is that the DMP does know which way is down, even when the quaternions have forgotten. In other words, it seems as if the Quat6 and Quat9 values are subject to drift over time that the DMP as a whole is not.

Another possibility is that my code is just broken.

Any thoughts?

[PaulZC]

Hi Devin,

Drifting quaternions is not unexpected. The IMU gyros especially drift over time.

I don't have a full answer for you. The DMP is still a mystery in many ways. There is background information here that may be useful. If you look through the open and closed issues in the 20948 Library repo, you'll find similar issues being discussed.

All I can suggest is: try the actual Invensense code (try one of the 'wrappers' mentioned in the DMP.md doc.). If you see the same thing, you know it's the DMP drifiting. If it is better behaved, then that may provide clues about something that is not quite right with the DMP configuration in our Library.

Good luck!
Paul

[PaulZC]

Hi Devin (@decamun),

I'm closing this as I really don't think there is any way I can (reasonably) correct this. It's either a 'feature' of the DMP itself, or we haven't quite got the DMP configured correctly in our library. Debugging the latter is not something I can take on anytime soon. Debugging the former is essentially impossible... Please re-open if you do manage to make any progress with this.

Best wishes,
Paul