# include "I2Cdev. H"
# include "MPU6050_6Axis_MotionApps20. H"

MPU6050 mpu.//instantiate a MPU6050 object, the object name for mpu
Int16_t ax, ay, az, gx, gy, gz.

//* * * * * * * * * * * * * * * * * * * * Angle data * * * * * * * * * * * * * * * * * * * * *//
Float Gyro_y;//Y axis gyroscope data staging
Float Gyro_x;
Float Gyro_z;
Float angleAx;
Float angle6;
Float K1=0.05;//the weight value of accelerometer
Float Angle;//calculate the first-order complementary filter Angle
Float accelz=0;

//* * * * * * * * * * * * * * * * * * * * Angle data * * * * * * * * * * * * * * * * * * * * *//

//* * * * * * * * * * * * * * * Kalman_Filter * * * * * * * * * * * * * * * * * * * * *//
Float P [2], [2]={{1, 0},
{0, 1}
};
Float Pdot [4]={0, 0, 0, 0};
Float Q_angle=0.001, Q_gyro=0.005; Data confidence//Angle, angular velocity data confidence
Float R_angle=0.5, C_0=1;
Float q_bias angle_err, PCt_0 PCt_1, E, K_0, K_1, t_0 and t_1;
Float timeChange=5;//filter sampling interval millisecond
Float dt=timeChange * 0.001;//note: dt values for the filter sampling time
//* * * * * * * * * * * * * * * Kalman_Filter * * * * * * * * * * * * * * * * * * * * *//

Void Angletest ()
{
//balance parameter
Angle=atan2 (ay, az) * 180/PI;//Angle formula
Gyro_x=(gx - 128.1)/131;//Angle conversion
Kalman_Filter (Angle, Gyro_x);//kalman filtering
//Z axis rotation Angle parameter
If (gz & gt; Gz - 32768)=65536;//casting 2 g 1 g
Gyro_z=- gz/131;//Z axis parameter conversion
Accelz=az/16.4;

AngleAx=atan2 (ax, az) * 180/PI;//computing and x axis Angle
Gyro_y=- gy/131.00;//calculate the angular velocity
//first order complementary filter
Angle6=K1 + (1 - K1) * * angleAx (angle6 + Gyro_y * dt);
}

////////////////////////kalman/////////////////////////
Float Angle, angle_dot;//balance Angle values
Void Kalman_Filter (double angle_m, double gyro_m)
{
Angle +=(gyro_m - q_bias) * dt;
Angle_err=angle_m - Angle;
Pdot [0]=Q_angle - P [0] P [1] [1] - [0].
Pdot [1]=-p [1] [1].
Pdot [2]=-p [1] [1].
Pdot [3]=Q_gyro;
P [0] [0] +=Pdot [0] * dt.
P [0] [1] +=Pdot [1] * dt.
P [1] [0] +=Pdot [2] * dt.
P [1] [1] +=Pdot [3] * dt.
PCt_0 C_0 * P=[0] [0];
PCt_1=C_0 * P [1] [0].
E=R_angle + C_0 * PCt_0;
K_0=PCt_0/E;
K_1=PCt_1/E;
T_0=PCt_0;
T_1 C_0 * P=[0] [1].
P [0] [0] -=K_0 * t_0;
P [0] [1] -=K_0 * t_1;
P [1] [0] -=K_1 * t_0;
P [1] [1] -=K_1 * t_1;
Angle +=K_0 * angle_err;//Angle
Q_bias +=K_1 * angle_err;
Angle_dot=gyro_m - q_bias;//angular velocity
}


Void setup () {
Wire.begin();//to join the I2C bus sequence
Serial. The begin (9600);//open the serial port, set the baud rate
delay(1000);
Mpu. The initialize ();//initialize MPU6050
}

Void loop () {
Mpu. GetMotion6 (& amp; Ax, & amp; Ay, & amp; Az, & amp; Gx, & amp; Gy, & amp; Gz);//IIC obtain MPU6050 six axis data ax ay az gx gy gz

Angletest ();//get Angle Angle and kalman filtering

Serial. Print (ax); Serial. Print (", ");
Serial. Print (ay); Serial. Print (", ");
Serial. Print (az); Serial. Print (" - ");
Serial. Print (Angle); Serial. Print (", ");
Serial. Print (angle_dot); Serial. Print (", ");
Serial. Println (angle6);

Delay (50);
}