Lda-4200d + adxl345 Kalman Filter

The base of the Two-wheeled self-balancing car is basically finished. A simple plastic box with two DC motors and tires is used, which is relatively simple, but can be used together.

 

 

 

 

 

 

Below the car is the two modules lda-4200d + adxl345, the acceleration module is not fixed, the board is too small, there is no place to punch, there is time to weld the two modules to the 10 thousand board should be easily fixed.

 

Acceleration module angle calculation:

  If the sensor X axis is down, the Y axis is facing forward   The formula for calculating the vertical radians is angle = atan2 (Y, Z) // The result is expressed in radians and is between-Pi and PI (excluding-Pi)

 If you want to convert it to a specific angle: angle = atan2 (Y, Z) * (180/3. 14)

 

Gyroscope angle calculation:

  In formula, angle (n) is the angle value from gyroscope sampling to n times;   Angle (n-1) is the angle value of the n-1 subsample of the gyroscope;   Gyron is the instantaneous angular rate value obtained from the nth sampling of the gyroscope;   DT indicates the time used to run the task again;

Angle_n + = gyro (n) * dt // Credit Calculation

 

Kalman Filter 

Kalman filter found on the Internet.CodeAs follows:
 

Static Const Float Dt = 0.02 ;

Static FloatP [2] [2] = {{1,0},{0,1}};

FloatAngle;
FloatQ_bias;
FloatRate;

Static Const FloatR_angle =0.5;
Static Const FloatQ_angle =0.001;
Static Const FloatQ_gyro =0.003;

FloatStateupdate (Const FloatGyro_m ){

Float Q;
Float Pdot [ 4 ];
Q = gyro_m-q_bias;
Pdot [ 0 ] = Q_angle-P [ 0 ] [ 1 ]-P [ 1 ] [ 0 ]; /* 0, 0 */
Pdot [ 1 ] =-P [ 1 ] [ 1 ]; /* 0, 1 */
Pdot [ 2 ] =-P [ 1 ] [ 1 ]; /* 1, 0 */
Pdot [ 3 ] = Q_gyro; /* 1, 1 */

Rate = Q;

Angle + = Q * DT;

P [0] [0] + = Pdot [0] * DT;
P [0] [1] + = Pdot [1] * DT;
P [1] [0] + = Pdot [2] * DT;
P [1] [1] + = Pdot [3] * DT;

ReturnAngle;
}

FloatKalmanupdate (Const FloatIncangle)
{

FloatAngle_m = incangle;
FloatAngle_err = angle_m-angle;

FloatH_0 =1;

Const FloatPht_0 = H_0 * P [0] [0];/*+ H_1 * P [0] [1] = 0*/ 
Const FloatPht_1 = H_0 * P [1] [0];/*+ H_1 * P [1] [1] = 0*/ 

FloatE = r_angle + (h_0 * pht_0 );

float K_0 = pht_0/E;
float k_1 = pht_1/E;

float y_0 = pht_0; /* H_0 * P [0] [0] */
float Y_1 = H_0 * P [ 0 ] [ 1 ];
P [ 0 ] [ 0 ]-= K_0 * y_0;
P [ 0 ] [ 1 ]-= K_0 * Y_1;
P [ 1 ] [ 0 ]-= k_1 * y_0;
P [ 1 ] [ 1 ]-= k_1 * Y_1;

Angle + = K_0 * angle_err;
Q_bias + = k_1 * angle_err;

ReturnAngle;

}

 

 

Waveform Display

Test description-single-chip microcomputer collects acceleration and gyroscope signals, and uses the Kalman Filter above to calculate the optimal inclination, and sends the signals to the PC through the serial port. The serial port oscilloscope running on the PC displays the relevant waveforms.

1. Blue is the angle after acceleration conversion. 2. Yellow indicates the angle after the gyroscope points directly. 3. Red indicates the angle after Kalman filtering.

 

 

When you tap the car with your fingers, you can see that the angle vibration obtained by the acceleration module is relatively severe, and the waveform after Kalman filtering is relatively gentle.

 

 

Partial enlargement. Let's see if the curve is still beautiful ..

 

Waveform Display with xf_z1988 garden open source waveform controls, his home page is: http://www.cnblogs.com/xf_z1988/