卡尔曼滤波代码整理（一维）

struct _1_ekf_filter
{
    float LastP;
    float   Now_P;
    float out;
    float Kg;
    float Q;
    float R;    
};

void kalman_1(struct _1_ekf_filter *ekf,float input)  
{
    ekf->Now_P = ekf->LastP + ekf->Q;
    ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
    ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
    ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
}


//使用范例
static struct _1_ekf_filter ekf = {0.02,0,0,0,0.001,0.543};//初始化参数
float pMpu;
kalman_1(&ekf,(float)pMpu);  
pMpu = (int16_t)ekf.out;