I’ve been using the GY-521 IMU breakout board containing Invensense’s MPU-6050 IMU to compute orientation in my self-balancing scooter (the “Halfway”). I’d like to improve the scooter’s performance on hills and uneven surfaces. I thought I’d revisit the fusion algorithm which combines gyroscope and accelerometer data to compute the scooter’s tilt angle. The initial code for the Halfway used a complementary filter algorithm, explained in an earlier blog post. Accelerometer data is noisy on short time scales, and gyroscope data drifts on longer timescales, so the complementary filter combines both for greater accuracy. However, the MPU-6050 contains a digital motion processor (DMP) which can perform the data fusion on the IMU chip iteslf. Though the exact details of how the DMP does is calculations are not published, the DMP is still useful because presumably Invensense has a pretty good fusion algorithm and because the DMP calculations occur on the MPU-6050 chip, freeing up processor power. I wrote about using the i2cdevlib library to obtain orientation data from the MPU-6050 DMP in this post. I’d made earlier attempts to compare the DMP and complementary filter data, but I kept running into problems retrieving data from the FIFO buffer on the MPU-6050. The FIFO buffer would overflow while the Arduino program was performing the complementary filter calculations. I found the fix for that in the i2cdevlib forums here. The solution is to reduce the rate at which the DMP writes to the FIFO buffer. In the i2cdevlib library, the FIFO rate is controlled in line 305 of the file “MPU6050_6axis_motionapps.h” (the exact line number depends on which version of the file you’re using)
0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) // It is important to make sure the host processor can keep up with reading and processing // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
Initially, the FIFO write rate was 100 Hz, which overflowed the buffer easily. Reducing the rate to 20 Hz allowed enough time to complete the complementary filter calculations before retrieving the DMP data. I took Jeff Rowberg’s Arudino sketch which extracts the DMP data and modified it to perform the complementary filter calculations as well. The Arduino sketch then sent the data over the serial port to a Processing sketch to display it graphically. Please note that Rowberg’s code can calculate orientation angles in terms of either yaw, pitch, roll angles or Euler angles. I’m not clear on the difference between these two sets of angles (If someone can explain it to me, please let me know!) In i2cdevlib the computation of the yaw, pitch, roll angles incorporates the gravity vector and seems to best match the data from the complementary filter, so that is what I chose for the comparison. Below is a screenshot from the Processing sketch:
Below is a video comparison between the orientation angles from the MPU-6050 as calculated by the DMP and the complementary filter algorithm. I don’t have any quantitative data showing which algorithm is better. Qualitatively, however, I can say that for pitch (rotation about the X-axis) and roll (rotation about the Y-axis), the calculations are fairly close, but the complementary filter seems to consistently lag the DMP. The DMP algorithm is able to calculate yaw, which the complementary filter cannot. I suspect that when the algorithms differ, the DMP is more accurate, however it is possible that a better implementation of the complementary filter might be able to reduce some of the lag.
Here is the Arduino code that generates the data. It requires i2cdevlib to work, and as discussed above, the DMP FIFO rate should be set to about 20 Hz: