Two arrows together-Kalman filter algorithm
There are many algorithms on the Internet. My understanding is that Kalman filtering simply uses one variable to modify another variable.
*.h
#ifndef __FILTER_H
#define __FILTER_H
extern float angle, angle_dot;
float Kalman_Filter(float Accel,float Gyro);
void Yijielvbo(float angle_m, float gyro_m);
#endif
*.c
float K1 =0.02;
float angle, angle_dot;
float Q_angle=0.001;//The covariance of process noise
float Q_gyro=0.003;//0.03 The covariance of process noiseThe covariance of process noise is a matrix with one row and two columns
float R_angle=0.5 ;// The covariance of the measurement noise is the measurement deviation
float dt=0.005; //
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} };
/************************************************* *************************
Function function: Simple Kalman filter
Entry parameters: acceleration, angular velocity
Return value: None
******** ************************************************** ****************/
float Kalman_Filter(float Accel,float Gyro)
{ angle+=(Gyro-Q_bias) * dt; //priori estimate 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; //zk-prior estimation
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 += K_0 * Angle_err; //posterior estimate
Q_bias += K_1 * Angle_err; //posterior estimate
angle_dot = Gyro-Q_bias; //output value (posterior estimate) Differential = angular velocity
return angle_dot;
}
/************************************************* *************************
Function function: first-order complementary filtering
Entry parameters: acceleration, angular velocity
Return value: None
******** ************************************************** ****************/
void Yijielvbo(float angle_m, float gyro_m)
{ angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * 0.005); }