The application of Kalman filter attitude solution in Arduino Uno + mpu6050 Gyroscope

MPU6050 Six-axis gyroscope

In the electronic practice of four-axis UAV, balance car, robot and so on, it is used for attitude judgment, mastering the more interesting works that can play their own imagination.

This routine outputs the angle of XYZ, plus or minus 90 degrees.

Using the Kalman filter algorithm to solve the attitude, the feeling is relatively stable, but it seems a bit offset. Let's study for reference, and then improve it.

Output effect

First look at the output of this example XYZ axis:

(The embodiment of the time curve is: static posture → swing → restore the original static posture → Pat Table → static posture)

BOM List

Arduino Uno * *

mpu6050 Gyroscope Module * *

Jumper several

Wiring

The Arduino uno+mpu6050 wiring method is as follows

Program Implementation

To update the I²C library first

The I²c library found on GitHub

(Program Source: https://github.com/jrowberg/i2cdevlib)

Open, copy the i2cdev,mpu6050 folder in the Arduino folder to the Arduino IDE's library folder

(The default path is this C:\Program Files (x86) \arduino\libraries)

Kalman Filter Program found on GitHub (program Source: Https://github.com/wjjun/MPU6050_Kalman)

Upload the program to the board, open the serial monitor, you can see a heap of data

(I'll talk about how to sort this data later)

#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" MPU6050 accelgyro;unsigned long Now, lasttime = 0;float DT;             Differential time int16_t ax, Ay, AZ, GX, GY, GZ;    Accelerometer Gyroscope raw data float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;             Angle variable Long Axo = 0, Ayo = 0, azo = 0;             Accelerometer offset Long Gxo = 0, Gyo = 0, Gzo = 0;                  Gyro offset Float pi = 3.1415926;float acceratio = 16384.0;                    Accelerometer proportional coefficient float gyroratio = 131.0;                       Gyroscope proportional coefficient uint8_t n_sample = 8;         Accelerometer filter algorithm sample number float Aaxs[8] = {0}, Aays[8] = {0}, Aazs[8] = {0};                      x, Y axis sampling queue long aax_sum, aay_sum,aaz_sum; x, Y axis sampling and float a_x[10]={0}, a_y[10]={0},a_z[10]={0}, g_x[10]={0}, g_y[10]={0},g_z[10]={0};             Accelerometer covariance calculation queue float px=1, Rx, Kx, Sx, Vx, Qx;             X-axis Kalman variable float py=1, Ry, Ky, Sy, Vy, Qy;             Y-Axis Kalman variable float pz=1, Rz, Kz, Sz, Vz, Qz;    Z-axis kalman variable void setup () {wire.begin (); Serial.begin (115200);   Accelgyro.initialize ();             Initialize unsigned short times = 200; Number of samples for (int i=0;i<times;i++) {accelgyro.getmotion6 (&ax, &ay, &az, &AMP;GX, &gy, &g Z); Read the six-axis raw value AXO + = ax; Ayo + = ay;      Azo + = AZ; Sampling and Gxo + = GX; Gyo + = Gy;        Gzo + = GZ; } Axo/= times; Ayo/= times; Azo/= times; Calculates the accelerometer offset gxo/= times; Gyo/= times; Gzo/= times;             Calculate Gyro offset}void loop () {unsigned long now = Millis ();           Current time (ms) DT = (now-lasttime)/1000.0;                           Differential time (s) lasttime = now; Previous sampling time (ms) Accelgyro.getmotion6 (&ax, &ay, &az, &AMP;GX, &gy, &gz);              Read the six-axis original value float ACCX = ax/acceratio;              X-axis acceleration float accy = ay/acceratio;              Y-axis acceleration float ACCZ = az/acceratio;    Z-axis Acceleration aax = Atan (ACCY/ACCZ) * ( -180)/pi;       Y axis for z-axis Angle aay = Atan (ACCX/ACCZ) * 180/PI; X-axis angle for z-axis Aaz = aTan (accz/accy) * 180/PI;                              Z axis for y-axis angle aax_sum = 0;    A sliding weighted filtering algorithm for accelerometer raw data aay_sum = 0;      aaz_sum = 0;        for (int i=1;i<n_sample;i++) {aaxs[i-1] = aaxs[i];        Aax_sum + = aaxs[i] * i;        AAYS[I-1] = aays[i];        Aay_sum + = aays[i] * i;        AAZS[I-1] = aazs[i];        Aaz_sum + = aazs[i] * i;    } aaxs[n_sample-1] = Aax;    Aax_sum + = Aax * n_sample; Aax = (Aax_sum/(11*n_sample/2.0)) * 9/7.0;                        Angle amplitude modulation to 0-90°aays[n_sample-1] = Aay;                     This applies the experiment method to obtain the suitable coefficient aay_sum + = aay * n_sample;    The coefficient for this example is 9/7 aay = (Aay_sum/(11*n_sample/2.0)) * 9/7.0;     AAZS[N_SAMPLE-1] = Aaz;    Aaz_sum + = Aaz * n_sample;    Aaz = (Aaz_sum/(11*n_sample/2.0)) * 9/7.0; float Gyrox =-(GX-GXO)/gyroratio * DT; X-axis Angular velocity float Gyroy =-(Gy-gyo)/gyroratio * DT; Y-axis angular velocity float Gyroz =-(GZ-GZO)/gyroratio * DT;                             Z-axis Angular velocity agx + = Gyrox; X-axis Angular velocity integral    Agy + = Gyroy;        X-axis angular velocity integral agz + = Gyroz; /* Kalman Start */Sx = 0;    Rx = 0; Sy = 0;    Ry = 0; Sz = 0;        Rz = 0;                      for (int i=1;i<10;i++) {//Measured value averaging operation a_x[i-1] = a_x[i];        That is, the acceleration mean Sx + = a_x[i];        A_Y[I-1] = A_y[i];        Sy + = A_y[i];        A_Z[I-1] = A_z[i];        Sz + = A_z[i];    } a_x[9] = Aax;    Sx + = Aax;                                 Sx/= 10;    X-axis acceleration average a_y[9] = aay;    Sy + = Aay;                                 Sy/= 10;    Y-axis acceleration mean a_z[9] = Aaz;    Sz + = Aaz;    Sz/= 10;        for (int i=0;i<10;i++) {Rx + = sq (a_x[i]-Sx);        Ry + = sq (a_y[i]-Sy);        Rz + = sq (a_z[i]-Sz);                              } Rx = RX/9;                            Get variance Ry = RY/9;      Rz = RZ/9;                         px = px + 0.0025;    0.0025 below are the instructions ...                      Kx = px/(px + Rx); Calculate Kalman Gain AGX = AGX + Kx * (AAX-AGX);                       Gyro angle and accelerometer speed overlay px = (1-kx) * PX;    Update P-value py = py + 0.0025;    Ky = py/(py + Ry);     Agy = Agy + Ky * (aay-agy);      PY = (1-ky) * PY;    PZ = Pz + 0.0025;    Kz = PZ/(PZ + Rz);     Agz = Agz + Kz * (Aaz-agz);    PZ = (1-kz) * PZ; /* Kalman End */Serial.print (AGX);    Serial.print (","); Serial.print (Agy);    Serial.print (","); Serial.print (Agz);    Serial.println (); }

The application of Kalman filter attitude solution in Arduino Uno + mpu6050 Gyroscope