30 minutes to understand Kalman filter_2 (the second article contains the original code compiled by arduino and stm32 Kalman filter)
Continue later...
//Kalman parameter
float Q_angle = 0.001;
float Q_gyro = 0.003;
float R_angle = 0.5;
float dt = 0.05; //dt is the Kalman filter sampling time;
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] = {0,0,0,0};
float PP[2][2] = {{1, 0 },{ 0, 1} };
void Kalman_Filter_X(float Accel,float Gyro) //Kalman function
{ Angle_X_Final += (Gyro-Q_bias) * dt; // priori estimation Pdot[0]=Q_angle-PP[0][1]-PP[1] [0]; // Pk- the differential of the prior estimation error covariance
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];
Pdot[3]= Q_gyro;
PP[0][0] += Pdot[0] * dt ; // Pk- the integral of the a priori estimation error covariance differential
PP[0][1] += Pdot[1] * dt; // = a priori estimation error covariance
PP[1][0] += Pdot[ 2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel-Angle_X_Final; //zk-priori estimate
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; //posterior estimation error covariance
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1 ][1] -= K_1 * t_1;
Angle_X_Final += K_0 * Angle_err; //posterior estimate
Q_bias += K_1 * Angle_err; //posterior estimate
Gyro_x = Gyro-Q_bias; //output value (posterior estimate) Differential = angular velocity
}
transfer
gx //The original gx value of the gyroscope
roll //roll angle before filtering
Angle_X_Final //roll angle after filtering
Angle_X_Final=Kalman_Filter_X(roll,gx); //Kalman filter to calculate X inclination
Finally, the filtered roll angle is obtained, and pitch and yaw are the same.