5% OFF Over \$19, Code: Elephant5; 10% OFF Over \$59, Code: Elephant10

# Self-balancing Car Kit For Arduino Robot Project 8: Calculating Inclined Angle And Angular Velocity Values

Posted by Fiona Su on

Description:
In the previous project, we have used accelerometer and gyroscope to get the car’s posture.
In this lesson, we use the data measured by MPU-6050 chip to directly get the posture of balance car.
You will learn how to calculate the tilt angle and angular velocity value detected by balance car. Show the test result on the serial monitor.

Source Code:
Note:Before test the source code, do remember to add the corresponding library.
You can refer to the method in the link: https://wiki.keyestudio.com/How_to_Install_Arduino_Library

```#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC communication library

MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu6050
int16_t ax, ay, az, gx, gy, gz;     //Define three-axis acceleration, three-axis gyroscope variables

float Angle;   //angle variable
int16_t Gyro_x;   //Angular velocity variable

void setup()
{
// Join the I2C bus
Wire.begin();                            //Join the I2C bus sequence
Serial.begin(9600);                       //open serial monitor and set the baud rate to 9600
delay(1500);
mpu6050.initialize();                       //initialize MPU6050
delay(2);
}

void loop()
{
mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis data  ax ay az gx gy gz
Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; the negative sign is direction processing
Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
Serial.print("Angle = ");
Serial.print(Angle);
Serial.print("   Gyro_x = ");
Serial.println(Gyro_x);
}
```

Test Result
Installed well the balance car, upload the source code and power on; turn the slide switch ON.
Then open the Arduino IDE, set the baud rate to 9600, the serial monitor will pop up the value.

How to use Kalman filtering to calculate the angel?

What’s Kalman filtering ?
Kalman filtering is an algorithm that uses the linear system state equation to estimate the state of the system through the input and output of the system.
Since the observed data includes the effects of noise and interference in the system, the optimal estimate can also be seen as a filtering process.

Accelerometer data cannot always be trusted 100% due to original data acquired by the MPU6050.
Why?
The accelerometer measures the inertial force, which may be caused by gravitational force (ideally only gravitational), but it may also be caused by the acceleration (motion) of the device.

Therefore, even if the accelerometer is in a relatively stable state, it is still very sensitive to vibration and mechanical noise.
That is, the individual number of inclination angles obtained by using the value of the 3-axis acceleration is erroneous, and the error is large. Then you need to use a gyroscope to eliminate any accelerometer errors.

But how is this done? Is there no noise in the gyroscope?
The gyroscope is not without noise, but because it measures rotation, it is less sensitive to linear mechanical motion (the type of noise the accelerometer is subjected to).
However, the gyroscope has other types of problems, such as drift (does not return to zero rate values when the rotation stops).
Nonetheless, by averaging the data from the accelerometer and the gyroscope, we can obtain a better estimate than the current device’s tilt angle obtained by using the accelerometer data alone.
Therefore, in order to obtain a smaller, more stable angle, we need to perform Kalman filtering on the inclination angle θ calculated by the acceleration and the measured angular velocity value θ' of the gyroscope.
In this project，we use Kalman filter to calculate the balance angle and angular velocity of the balance car, displaying the tested data on the serial monitor.

Source Code:
Note: Before test the source code, do remember to add the corresponding library.
You can refer to the method in the link: https://wiki.keyestudio.com/How_to_Install_Arduino_Library

```#include <MPU6050.h>      //MPU6050 library
#include <Wire.h>        //IIC communication library

MPU6050 mpu6050;     //Instantiate an MPU6050 object; name mpu60500
int16_t ax, ay, az, gx, gy, gz;     //Define three-axis acceleration, three-axis gyroscope variables
float Angle;
float Gyro_x,Gyro_y,Gyro_z;  //calculate angular velocity of each axis by gyroscope

///////////////////////Kalman_Filter////////////////////////////
float Q_angle = 0.001;  //Covariance of gyroscope noise
float Q_gyro = 0.003;    //Covariance of gyroscope drift noise
float R_angle = 0.5;    //Covariance of accelerometer
char C_0 = 1;
float dt = 0.005; //The value of dt is the filter sampling time.
float K1 = 0.05; // a function containing the Kalman gain is used to calculate the deviation of the optimal estimate.
float K_0,K_1,t_0,t_1;
float angle_err;
float q_bias;    //gyroscope drift

float accelz = 0;
float angle;
float angle_speed;

float Pdot[4] = { 0, 0, 0, 0};
float P[2][2] = {{ 1, 0 }, { 0, 1 }};
float  PCt_0, PCt_1, E;
//////////////////////Kalman_Filter/////////////////////////

void setup()
{
// Join the I2C bus
Wire.begin();                            //Join the I2C bus sequence
Serial.begin(9600);                       //open serial monitor and set the baud rate to 9600
delay(1500);
mpu6050.initialize();                       //initialize MPU6050
delay(2);
}

void loop()
{
Serial.print("Angle = ");
Serial.print(Angle);
Serial.print("  K_angle = ");
Serial.println(angle);
Serial.print("Gyro_x = ");
Serial.print(Gyro_x);
Serial.print("  K_Gyro_x = ");
Serial.println(angle_speed);

mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);     //IIC to get MPU6050 six-axis ax ay az gx gy gz
angle_calculate(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);      //obtain angle and KalmanFilter
}

/////////////////////////////angle calculate///////////////////////
void angle_calculate(int16_t ax,int16_t ay,int16_t az,int16_t gx,int16_t gy,int16_t gz,float dt,float Q_angle,float Q_gyro,float R_angle,float C_0,float K1)
{
Angle = -atan2(ay , az) * (180/ PI);           //Radial rotation angle calculation formula; negative sign is direction processing
Gyro_x = -gx / 131;              //The X-axis angular velocity calculated by the gyroscope; the negative sign is the direction processing
Kalman_Filter(Angle, Gyro_x);            //KalmanFilter
}
////////////////////////////////////////////////////////////////

///////////////////////////////KalmanFilter/////////////////////
void Kalman_Filter(double angle_m, double gyro_m)
{
angle += (gyro_m - q_bias) * dt;          //Prior estimate
angle_err = angle_m - angle;

Pdot[0] = Q_angle - P[0][1] - P[1][0];    //Differential of azimuth error covariance
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;

P[0][0] += Pdot[0] * dt;    //The integral of the covariance differential of the prior estimate error
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

//Intermediate variable of matrix multiplication
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
//Denominator
E = R_angle + C_0 * PCt_0;
//Gain value
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;  //Intermediate variable of matrix multiplication
t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;    //Posterior estimation error covariance
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

q_bias += K_1 * angle_err;    //Posterior estimation
angle_speed = gyro_m - q_bias;   //The differential value of the output value; work out the optimal angular velocity
angle += K_0 * angle_err; ////Posterior estimation; work out the optimal angle
}
```

Test Result
Installed well the balance car, upload the source code and power on; turn the slide switch ON.
Then open the Arduino IDE, set the baud rate to 9600, the serial monitor will pop up the value.

Buy Self-balancing Car Kit For Arduino Robot