Gyro and Accelerometer Fusion algorithm flow

Symbol Description:

T: The time between the measured value and the last measured value;

RESTX (n-1), Resty (n-1), Restz (n-1): The optimal value vector for the position of the n-1 time;

GRATEXZ (n), Grateyz (n): The value of the gyroscope, the value of the nth time after filtering alone.

GRx (n), GRy (n), GRz (n): position vector fused to gyroscope

RAx (n), RAy (n), RAz (n): The nth-time vector of the measured value of the accelerometer;

RESTX (n), Resty (n), Restz (n): the optimal value vector of the nth-time position;

1, get the last position of the best value and normalization: Restx (n-1), Resty (n-1), Restz (n-1).

Including: Restx (n-1) *restx (n-1) +resty (n-1) *resty (n-1) +restz (n-1) *restz (n-1) = 1*1

2, the calculation angle, according to RESTX (n-1), Resty (n-1), restz (n-1) calculated ∠axz, ∠ayz;

3, the value of the gyroscope normalized, get Gratexz (n), Grateyz (n);

4, data integration, the gyroscope data fusion into:

∠aestxz =∠axz + GRATEXZ (n) * T;

∠aestyz =∠ayz + grateyz (n) * T;

Note: Gratexz and Grateyz are best handled based on the measured value and the last measured value, such as averaging.

5, the inverse operation of step 2, get the GRX (n), GRy (n), GRz (n):

①grx (n) *grx (n) +gry (n) *gry (n) +grz (n) *grz (n) = 1*1

②SIN∠AESTXZ = GRx (n)/Sqrt[grx (n) *grx (n) + GRz (n) *grz (n)]

③sin∠aestyz =gry (N)/sqrt[gry (n) *gry (n) + GRz (n) *grz (n)]

Find the Grx (n), GRy (n), GRz (n);

6, data fusion Two, data filtering:

(1) Get the latest accelerometer measurement and normalization: RAx (n), RAy (n), RAz (n).

Where: RAx (n) *rax (n) +ray (n) *ray (n) +raz (n) *raz (n) = 1*1

(2) According to the GRX (n), GRy (n), GRz (n) and Rax (n), Ray (n), Raz (n), data processing, obtained: RESTX (n), Resty (n), Restz (n).

As to how to data processing, there is a simple weighted average, there is a complex Kalman filter, the author of Small white, only the weighted average filter, it is not caught dead.

7, repeat the 1~7, the next round of calculation.

Gyro and Accelerometer Fusion algorithm flow