Please check my idea of how the 6050 dmp fusion works

[mark-hahn]

I have an idea about how the fusion algorithm works in the DMP. Please tell me if you agree or what you disagree about.

The algorithm keeps an estimate of the chip orientation (yaw, pitch roll) relative to gravity. Thie oriention estimate is kept as the rotation vector needed to rotate the reference frame vector (Z-axis) to the gravity vector. At start-up, when the DMP code is loaded into the chip, the rotation estimate vector is zero. In other words it estimates that the Z-axis matches the gravity vector.

Every time interval it uses a fusion of the acceleration and rotation measurements to update the estimate and improve its accuracy. Since the original estimate is usually wrong at start-up, it takes a while to change the estimate to the correct orientation with the Z-axis pointing to ground. I call this initial correction time the stabilization period.

The stabilization period is about 0.5 seconds when the initial chip orientation Z-axis is near the gravity vector. But when the chip is mounted in the body at an angle it takes longer to stabilize since the estimate has further to move. My bot has a big tilt and it takes 7 seconds to stabilize which is a problem.

After stabilization, when the chip is moving around, it only has to move the estimate a shorter distance each time interval so it keeps the estimate accurate with only a small lag time. This speed (lag) is the same no matter how the chip is mounted in the body.

This internal estimate is kept as a quaternion and every time interval it reports this estimate quaternion to the host mcu.

I have an idea on how to eliminate the extra stabilization time at start-up when the chip is tilted in the body. At startup the mcu could initialize the estimate quaternion to the known correct value. The quaternion could be overwritten to the DMP before starting it. This known quternion value would be a constant in the mcu code that is a function of the tilt vector of the chip in the body. This constant is is simply the quaternion reported to the mcu after start-up stabilization so it can be determined easily.

I'm going to try to write the constant quaternion into the DMP at start-up and hope that the DMP doesn't immediately overwrite it. Hopefully my stabilization time will go from 7 seconds down to 0.5 seconds. I'll report the results here.

Any comments would be greatly appreciated ...

[ZHomeSlice]

V6.12 DMP Features:

The DMP has many features which can be dynamically turned off and on at run-time. Individual feature can also be disabled while leaving others running. All DMP data is outputted to the FIFO except for pedometer. The DMP can also be programmed to generate an interrupt via gesture or if data ready. For details on flashing and enabling the DMP please read the porting guide.
 3 Axis Low Power Quaternions – gyro only quaternions. This feature when enabled will integrate the gyro data at 200Hz while outputting the sensor fusion data to the FIFO at the user requested rate. The 200Hz integration will allow for more accurate sensor fusion data. In MD6, if this feature is enabled, the driver will push the 3-axis quaternion into the MPL library and the MPL will handle the accel and compass integrations.
 6 Axis Low Power Quaternions – gyro and accel quaternions. Similar to the 3-axis LPQ, integrates the accel and gyro at 200Hz sampling rates will outputting to the FIFO at the user requested rates. The 3-axis LPQ and 6-axis LPQ are mutually exclusive and should not be run concurrently. If enabled the 6-axis quaternions can be pushed into the MPL library and the MPL will handle the compass integration for 9-axis.
 Orientation Gesture Recognition – Uses sensor data to detect is there is a change in device orientation from portrait, landscape, reverse portrait, and reverse orientation. Very much dependent on the orientation matrix.
 Tap Gesture Recognition - multi-directional tap detection on the device. This feature will let users know on which axis position or negative a tap is detected on. It can detect multi-taps up to 4. APIs exist to configure this feature’s threshold, dead time, and tap counts.
 Pedometer Gesture Recognition – simple pedometer providing step count and timestamp. This feature is automatically enabled but is not triggered until there is 5 seconds of continuous steps detected. After 5 seconds the count and timestamp will start and the data can be read out from the DMP memory.
 DMP Interrupt – interrupts can be configured to generate either when there is sensor data ready which is the FIFO output rate, or when a tap or orientation gesture is detected.

Also
Gyro Calibration (Fast No Motion) Run-time calibration routine. Once a no motion state is detected the gyro calibration will trigger. Calibration will complete within .5 seconds of no motion state detection.

...To get you started
I will attempt go over the details and answer your questions in a few I've got to go :)
Z

[ZHomeSlice]

I like your quaternion experiment. I look forward to your results
Z