/******************************三種濾波函數，每種都調試下，比較動態和靜態過程****************************************************/
/*******************下面濾波過程很重要，很重要，很重要，重要的事情說三遍*******************************************************/
/*******************下面濾波過程很重要，很重要，很重要，重要的事情說三遍*******************************************************/
#define dt 0.002
float Angle,Gyro_y;
//*************
float Q_angle=0.001;//0.001
//*************
float Q_gyro=0.003;//0.03
//*************
float R_angle=0.5;//0.5
//*************
//float dt=0.01;//0.1
//dt爲kalman濾波器採樣時間; //*************
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 } };
//*************
float w_kp=1;//0.945
//*************
/*矩陣卡爾曼函數*/
//*************
float Kalman_Filter(float Accel,float Gyro)
//*************
{
//*************
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; //後驗估計
//*************
Gyro_y = Gyro - Q_bias;
//輸出值(後驗估計)的微分=角速度 //*************
return
Angle; //*************
}
//*************
/**************************************************************************************
角度融合
**************************************************************************************/
//*************
float kalman_filter(float angle_m,float gyro_m)//非矩陣卡爾曼濾波
//*************
{
//*************
static float x=0;
//*************
static float P=0.000001;
//*************
static float Q=0.000001;
//*************
static float R=0.35;//0.35
//*************
static float k=0;
//*************
gyro_m=W_K*gyro_m;//角速度修正
//*************
x=x+gyro_m*dt;
//*************
P=P+Q;
//*************
k=P/(P+R);
//*************
x=x+k*(angle_m-x);
//*************
P=(1-k)*P;
//*************
return x;
//*************
}
//*************
//*************
float GYRO_Integration;
//*************
float Complement_Filter(float angle_m,float gyro_m)//互補融合濾波//*************
//*************
{
//*************
float Angle_Filter_Temp;//互補融合濾波中間量
//*************
float Angle_Difference_Value;//融合後的角度與加速度靜態角度差值，作爲反饋量加入積分迴路，讓靜差爲0
//*************
//*************
Angle_Filter_Temp=GYRO_Integration;
//*************
gyro_m=W_K*gyro_m;//角速度修正
//*************
Angle_Difference_Value=(angle_m-Angle_Filter_Temp)*1.0;//融合後的角度與加速度靜態角度差值
//*************
GYRO_Integration=GYRO_Integration+(gyro_m+Angle_Difference_Value )*dt;
//*************
return GYRO_Integration;
//*************
}
//*************
//*************
/************************上面濾波過程很重要，很重要，很重要，重要的事情說三遍**************************************************************/
/************************上面濾波過程很重要，很重要，很重要，重要的事情說三遍**************************************************************/