卡尔曼滤波

简单嵌入式的卡尔曼滤波

// 有4个传感器
float x_last[4] = {0};
float p_last[4] = {0.02,0.02,0.02,0.02}; 

float Q = 0.018;   
float R = 0.542; 

float kg;          
float x_mid;
float x_now;
float p_mid;
float p_now;


void Kalman_filtering_init(float*z_real,unsigned char num)
{
	*(x_last+num) = *z_real;
	*(p_last+num) =  0.02;
}
	

void Kalman_filtering(float*z_measure, unsigned char num)
{
		x_mid = *(x_last + num);
		p_mid = *(p_last+num) + Q;
		kg = p_mid / (p_mid + R);

		x_now = x_mid + kg * (*z_measure-x_mid);
		
		p_now = (1 - kg) * p_mid;

		*z_measure=x_now;
		
		*(p_last + num) = p_now;
		*(x_last + num) = x_now;
}