卡尔曼滤波（五）——C语言代码演示

一、模板代码演示

下面代码中，一个卡尔曼实体化的滤波器只处理特定的传感器数据

这里会提供接口供用户设置矩阵和系统输入

typedef struct Kalman_Data
{
  	float Post_This; //这次后验
    float Post_Last; //上次后验
    float Pri_This;  //这次先验
    float Pri_Last;  //上次先验
 
    float Post_Err_This;  //这次后验误差
    float Post_Err_Last;  //上次后验误差
    float Pri_Err_This;  //这次先验误差

	float Sys_Input;		//系统输入

	float Meas_Data;     //测量的值

    float Kalman_Gain; //卡尔曼增益
     
    float A;     //矩阵
	float B;
    float Q;
    float R;
    float H;

}Kalman_Data_t;


typedef struct Kalman_Filter
{
	Kalman_Data_t Data_Pack;


//函数指针
	void  		   (*creat)(Kalman_Data_t *p, 
						    float A,
						    float B,
    						float Q,
    						float R,
    						float H);

	void          (*clear)(Kalman_Data_t *p);
	
	float         (*start)(Kalman_Data_t *p, 
							float Sys_Input,
							float Meas_Data);
}Kalman_Filter_t;


void Kalman_Filter_Creat(Kalman_Data_t *p, 
						 float A,
						 float B,
    				     float Q,
    					 float R,
    					 float H)
{
	p->A = A;
    p->B = B;
	p->Q = Q;
	p->R = R;
	p->H = H;

}

void Kalman_Filter_Clear(Kalman_Data_t *p)
{
	meset(p, 0, sizeof(Kalman_Data_t));

}


float Kalman_Filter_Start(Kalman_Data_t *p, 
						  float Sys_Input,
						  float Meas_Data)
{
	//保存这一次的系统输入和测量值
	p->Sys_Input = Sys_Input;
	p->Meas_Data = Meas_Data;


	//计算先验估计值  			 Pri(这次) = A * Post(上次) + B * U(系统输入)
	p->Pri_This = p->A * p->Post_Last + p->B * p->Sys_Input;

	//计算先验误差的协方差        Pri_Err(这次) =  A * Post_Err(上次) * A的转置 + Q
	p->Pri_Err_This = p->A * p->Post_Err_Last * p->A + p->Q; 

	//计算卡尔曼增益 K =【H * Pri_Err(这次) * H的转置 + R】的逆 * (Pri_Err(这次) * H的转置)
	p->Kalman_Gain = (p->H * p->Pri_Err_This * p->H + p->R) * (p->Pri_Err_This + p->H)

	//计算后验估计值				Post(这次) = Pri(这次) + K *【Meas(这次) - Pri(这次)】
	p->Post_This = p->Pri_This + p->Kalman_Gain * 【p->Meas_Data - p->Pri_This】;

	//计算后验误差的协方差		Post_Err(这次) = (I - K * H) * Pri_Err(这次)
	p->Post_Err_This = (1 - p->Kalman_Gain * p->H) * p->Pri_Err_This;

	//更新迭代数据
	p->Post_Last = p->Post_This;
	p->Pri_Last  = p->Pri_This;
	p->Post_Err_Last = p->Post_Err_This;
	
	return p->Post_This;

}

---

二、例子演示

#define SAMPLE_dT          1     //采样周期
#define PITCH_Q_ELEMENT    0.1f	 //Q矩阵取值
#define  PITCH_R_ELEMENT   0.1f	 //R矩阵取值

Kalman_Filter_t     Kal_Filter_aim_at_pitch = 
{
	.creat = Kalman_Filter_Creat,

	.clear = Kalman_Filter_Clear,

	.start = Kalman_Filter_Start,
};





int main()
{

	//创建针对于pitch角度的卡尔曼滤波器
	Kal_Filter_aim_at_pitch.creat( &Kal_Filter_aim_at_pitch.Data_Pack, 
									1,				//A矩阵
									SAMPLE_dT,		//B矩阵
									PITCH_Q_ELEMENT,//Q矩阵
									PITCH_R_ELEMENT,//R矩阵
									1 );		    //H矩阵

	while(1)
  {
	//获取pitch角度和pitch角速度
	pitch_meas = sensor1();

	rate_pitch_meas = sensor2();


	//开始滤波，返回最优估计值
	pitch = Kal_Filter_aim_at_pitch.start( &Kal_Filter_aim_at_pitch.Data_Pack, 
											rate_pitch_meas,      //系统输入
											pitch_meas );		  //测量值

  }

}