Tuesday, September 15, 2015

A Python Experiment in Gyro Calibration & Drift Cancelling

This article is to share my findings in gyro calibration. I tried to make a python script that uses data comes from mobile phone [S5 that has 6050 IMU]. It was a good chance to write simple code and visualize results much easier than doing so on Arduino with IMU 6050 attached. But still the concept is the same.
I used this mobile app, this is not the only one, so you can choose other if you want.

I used three approaches for Gyro calibration.
First Approach:
This is the normal approach where you read values from Gyro and then divide total by number of reads. This is the average value. Almost all quadcopter firmwares do this step before arming to make sure that the value that is read from gyro represents zero rad/s i.e. no rotations.
gyro from value will be:
     Gyro = (gyro_raw - gyro_avg ) * timediff
      where gyro_avg is the average measured by reading a sample values and divide over count.
There is nothing wrong in above approach, it is simple, and really can make your quad flies. 
Second Approach:
The second approach was almost the same, but in this trial, I took values from First averaging algorithm as a first guess,  then I ran an inner loop that tries to get error based on the original average, and tries to add correction to it.
gyro of value will be:
      Gyro = (gyro_raw - gyro_avg_dyn) * timediff
      where gyro_avg_dyn: is the best average with least error.
I find this is a equivalent of running the first approach method multiple time and try to get the best average with least error. It could be smarter than blindly get an average after n counts -first method- where you can test some values in between. This also may be more useful when using integers rather than decimal in 8-bit chips.
Third Approach:
This approach was a bit different. It assumes that original average will give zero error for the noise, however drift will still occur, and it is in one direction. So if you integrate values from gyro it will keep either increasing or decreasing in one direction -as an overall behaviour-. 
The idea was to try to read the rate of which drift occurs and store it in a value called Gyro_AvrDriftRate. The unit of this value will be rad/sec^2.
So the Value of Gyro will be:    

      Gyro = (gyro_raw - gyro_avg - (gyro_avgdriftrate * timediff) ) * timediff
       where gyro_avg is the average measured by reading a sample values and divide over count.
      and gyro_avgdriftrate is drift rate per second

Drift already affects the original average value, as gyro_raw is not random values with normal distribution around an offset value; as it has drift component inside. and so the original average has a skew from the zero offset. In the third approach the drift will appear clearly in the integration of readings and using time-difference one can estimate a rate for the drift.

Actual Results:

Script Logic:
     1- calculate average and drift using the three methods.

 2- while loop:
              a. read gyro_raw
              b. calculate gyro_value_method1,2,3

              c. integrate these values into three different variables gyro_x1_int, gyro_x3_int, gyro_x3_int

              d. print result
The best approach should have the least absolute value i.e. the nearest to zero.


I ran the code several times, the result is not the same every time. but in general the third method is promising, as in
many cases it gives less drift than the other methods.

the third method is the one has x_rad/s2 label.
Also the second method sometimes get stuck in local minimum, as it reads less values in the inner loop, and sometimes get worst that normal average. 
This is not always the case. Anyway I hope this could be a small start for others to make much more investigation.

Note: sometimes values sent from mobile jumps in crazy way without movements, so I just added a skip statement to skip those values, and I assume you run this script while leaving your mobile with no movement at all.

Latest Code version is available at this link

No comments:

Post a Comment