iOS-卡尔曼滤波算法

一:前言

滤波算法 用于过滤掉连续的数据中出现偏差较大的数据

二:卡尔曼滤波算法

<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>欢迎各位大牛指正