Six-axis sensor + Kalman filter + first-order low-pass filter

Tip: After the article is written, the table of contents can be automatically generated. How to generate it can refer to the help document on the right

---

Angle and angular velocity measurement

1. Acquisition of angle and angular velocity

  Upright control is carried out through angle and angular velocity feedback, so the measurement of angle and angular velocity is very important. This system uses MPU6050 as an attitude sensor, integrates an acceleration sensor and a gyroscope, and can output three-axis acceleration and angular velocity. The acquisition of the angular velocity can be directly read by the gyroscope, and the acquisition of the angle can be measured in two ways: one is calculated by the acceleration component of the accelerometer, and the other is obtained by integrating the angular velocity output by the gyroscope.
  The coordinate system definition of MPU6050 is shown in Figure 1-1.
　　　　　　　　　　　

Figure 1-1 MPU6050 coordinate system　
  When the sensor's positive Z-axis is pointing vertically at the sky, the accelerometer's Z-axis reading should be positive, and ideally g, due to the Earth's gravity. Note that reading the accelerometer at this time is not the acceleration of gravity, but the acceleration of the object's own motion. Because the acceleration of its own motion is equal to the acceleration of gravity and the direction is opposite, the chip can remain still.
  When the sensor is stationary, we only rotate a certain angle θ around the X-axis. At this time, the acceleration direction is always perpendicular to the X-axis, and the X-axis has no acceleration component. Ignore the X-axis and decompose the acceleration, as shown in Figure 2-2 , the angle of the sensor around the X-axis can be easily calculated.
　　　　　　　　　　　

Figure 1-2 Accelerometer around the X axis　

  The same principle applies when only rotating around the Y axis. When rotating around the Z axis, because the gravitational acceleration is fixed in the direction of the Z axis, there is no acceleration component on the X and Y axes, and the angle around the Z axis cannot be obtained only through the accelerometer. If you want to get the angle around the Z axis, you can only use angular velocity integration Obtained, but due to deviations, it will no longer be of reference significance after a period of time.
  The rotation of the object is the superposition of the rotation angles around the three axes. We read the data of the accelerometer and process it according to the formula to obtain the corresponding attitude angle. Therefore, the angle of the car around the X-axis and Y-axis can be calculated by the following formula:
　　　　　　　　

  It is true that the required angle can be obtained through the accelerometer when it is completely still. However, in practical applications, the car will generate acceleration due to the swing of the body, and it will not be able to accurately reflect the inclination angle of the car model when it is superimposed on the measurement signal. , as shown in Figure 1-3.
　　　　　　　　

Figure 1-3 Comparison of measured inclination and actual inclination　   The lower the height of the sensor installation, the more it can suppress the acceleration caused by motion, but it still cannot completely eliminate this effect.

  So how to get the angle by integrating the angular velocity of the gyroscope? If there is a small error or drift in the measured angular velocity, after the integral operation, an accumulated error will be formed. As time increases, the angle information will no longer be accurate, and this method is not feasible.
　　　　　　　　
  We can synthesize the angles of the accelerometer and gyroscope, perform filtering and smoothing to obtain an accurate angle. The program provides three algorithms for obtaining accurate angles: 1. DMP algorithm 2. Complementary filter algorithm 3. Kalman filter algorithm.
  The DMP algorithm is a filtering method that comes with the MPU6050. As long as some initialization is performed, the quaternion can be read by using the official library function, and the attitude angle can be calculated according to the formula.

2 Complementary filtering

  The angles acquired by the accelerometer and gyroscope have certain shortcomings. The angle acquired by the accelerometer is more accurate in the long run, but fluctuates greatly, which can be considered to be mixed with high-frequency noise; the angle acquired by the gyroscope is more accurate in a short time. However, there is an integral error, which can be considered to be doped with low-frequency noise. We can pass them through a low-pass filter and a high-pass filter respectively and then superimpose them together. This is the complementary filtering algorithm.

Figure 1-5 Complementary filtering process　

Figure 1-6 Block Diagram of Complementary Filtering System　

Figure 1-7 Simplified system block diagram

Corresponding to our code is:
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt); It
can be seen that complementary filtering is to calibrate the angle of gyroscope integration through the angle obtained by the accelerometer, so that the integral The angle is tracked step by step to the angle obtained by the accelerometer. K1 and 1-K1 take different weights for these two angles, which can represent our trust in different data.

3 Kalman filter

  The data measured by the sensor always has a lot of uncertainty, such as a lot of noise, and most of these noises conform to the Gaussian distribution. For our car, the input is angular velocity and the output is angle, which is a linear system. If a system is a linear system and these uncertainties conform to the Gaussian distribution, then we can use the Kalman filter algorithm for optimal estimation. The idea of ​​Kalman filtering is to use the state equation of the system to predict the current value, and use the observed value measured by the sensor to correct the predicted value. Like complementary filtering, different weights can be selected to achieve, but this weight is dynamically changed. We know that the state equation of a system and the measurement equation of the sensor are as follows:

4 Realize Carl's first-order low-pass filter

/****************************** BEFIN ********************************
**
**@Name       : Complementary_Filter_x
**@Brief      : 一阶互补滤波   
**@Param angle_m: 加速度算出的角度 
**		gyro_m: 陀螺仪的角速度
**@Return     : None
**@Author     : @mayuxin
**@Data	      : 2022-06-04
******************************** END *********************************/
float Complementary_Filter_x(float angle_m, float gyro_m)
{
    
    
	 static float angle;
	 float K1 =0.02; 
   angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * dt);
	 return angle;
}

5 Realize the Kalman filter algorithm

/****************************** BEFIN ********************************
**
**@Name       : Kalman_Filter_x
**@Brief      : 获取x轴角度简易卡尔曼滤波  
**@Param Accel: 加速度算出的角度
**		  Gyro: 陀螺仪的角速度
**@Return     : None
**@Author     : @mayuxin
**@Data	      : 2022-06-04
******************************** END *********************************/
float dt=0.005;		  //每5ms进行一次滤波    
float Kalman_Filter_x(float Accel,float Gyro)		
{
    
    
	static float angle_dot;
	static float angle;
	float Q_angle=0.001; // 过程噪声的协方差
	float Q_gyro=0.003;	//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
	float R_angle=0.5;		// 测量噪声的协方差 既测量偏差
	char  C_0 = 1;
	static float Q_bias, Angle_err;
	static float PCt_0, PCt_1, E;
	static float K_0, K_1, t_0, t_1;
	static float Pdot[4] ={
    
    0,0,0,0};
	static float PP[2][2] = {
    
     {
    
     1, 0 },{
    
     0, 1 } };
	angle+=(Gyro - Q_bias) * dt; //先验估计
	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1]=-PP[1][1];
	Pdot[2]=-PP[1][1];
	Pdot[3]=Q_gyro;
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[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 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	angle_dot   = Gyro - Q_bias;	 //输出值(后验估计)的微分=角速度
	return angle;
}