一:前言
滤波算法 用于过滤掉连续的数据中出现偏差较大的数据
二:卡尔曼滤波算法
<0>卡尔曼滤波的原理请自行百度
<1>以float类型为例的静态常量
static float acce_x_Q = 0.005; //Q为过程激励噪声方差
static float acce_x_R = 0.05; //R为测量噪声方差
static float acce_x_111 = 1; //x(0)任取
static float acce_x_P_111 = 1; //任意P(0)不为0均可以收敛，这里取1
static float acce_x_kkk,acce_x_P_kkk,acce_x_Kg,acce_xkkk,acce_x_Pkkk;
<2>函数引用
//卡尔曼滤波算法
- (float )Filter_X_Kalman:(float )num{
acce_x_kkk= acce_x_111;
acce_x_P_kkk= acce_x_P_111 + acce_x_Q;
acce_x_Kg = acce_x_P_kkk / (acce_x_P_kkk + acce_x_R);
acce_xkkk = acce_x_kkk + acce_x_Kg * (num - acce_x_kkk);
acce_x_Pkkk= (1-acce_x_Kg) * acce_x_P_kkk;
acce_x_111= acce_xkkk;
acce_x_P_111= acce_x_Pkkk;
return acce_xkkk;
}
<3>欢迎各位大牛指正
