#include "kalman.h"



int kalman_init(kalman_struct *k)
{
	k->x_last=0;
	k->p_last=0.02;
	k->Q=0.018;
	k->R=0.542;
	
	return 0;
}


float kalman_filter(kalman_struct *k,float now)
{
	k->x_mid=k->x_last;                                     	//x_last=x(k-1|k-1),x_mid=x(k|k-1)
	k->p_mid=k->p_last+k->Q;                                  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q= 噪声
	k->kg=k->p_mid/(k->p_mid+k->R);                           //kg 为kalman filter ，R为噪声
//		k->z_measure=z_real+frand()*0.03;                      // 测量值
	k->x_now=k->x_mid+k->kg*(now-k->x_mid);                  // 估计出的最优值
	k->p_now=(1-k->kg)*k->p_mid;                             // 最优值对应的covariance

	k->p_last = k->p_now;                                    // 更新covariance 值
	k->x_last = k->x_now;                                    // 更新系统状态值
	
	return k->x_now;
}