通用型二阶卡尔曼滤波算法

由于c语言不支持矩阵运算，而卡尔曼滤波算法又涉及到矩阵的运算，因此我写了个通用性比较强的矩阵运算“库”，有了这个，就行轻松完成矩阵叉乘、点乘、求转置、二阶矩阵求逆等操作了。

函数列表：

void Matrix_Init(float *pr , int hang , int lie ); 

void Matrix_Init_One (float *matrix , int hang , int lie );

void Matrix_Init_eye (float *matrix , int hang , int lie );

void Matrix_Mul_Cross(float *pre  ,int hang_pre , int lie_pre , float *later,int hang_lat , int lie_lat ,float *res  );

void Matrix_Mul_dot(float *matrix , float mul ,int hang , int lie ,float *res  );

void Matrix_T(float *matrix ,int hang , int lie ,float *res  );

void Matrix_NI(float *matrix_a ,int scale,float *res );

void Matrix_Add(float *matrix_a ,float *matrix_b   ,int hang , int lie ,float *res  );

void Matrix_deduct(float *matrix_a ,float *matrix_b   ,int hang , int lie ,float *res  );

基于我自己写的矩阵运算库的二阶卡尔曼滤波源码：
公式如下：

/速度位置二阶系统
void Horizon_klm_estimate(float Pos , float Vel ,Second_order_sys * sys,float delta_T)
{
	sys->A_matrix[0][1] = delta_T;
	
	sys->y_oberve[0][0] = Pos ;
	sys->y_oberve[1][0] = Vel ;
	
	sys->x_state[0][0]= sys->x_state[0][0] + delta_T * sys->x_state[1][0] ;/step1 predict x

///step2 predict P
	float  P_temp[2][2] ,P_temp2[2][2]; 
	float  A_T_Matrix[2][2] ;
	Matrix_Mul_Cross((float *)(&sys->A_matrix) , 2,2 , (float *)&(sys->p_variance) , 2 , 2 ,(float *)&P_temp) ;
	Matrix_T((float *)&(sys->A_matrix),2,2,(float *)&A_T_Matrix);
	Matrix_Mul_Cross((float *)&P_temp , 2,2 , (float *)&A_T_Matrix , 2 , 2 ,(float *)&P_temp2) ;
	Matrix_Add((float *)&P_temp2 , (float *)&(sys->Q_variance) ,2,2, (float *)&(sys->p_variance)) ;
	
///step3 calculate K
	float matrix3_NI[2][2] ;
	float K_temp[2][2];
	Matrix_Add((float *)&(sys->p_variance) , (float *)&(sys->R_variance) ,2,2, (float *)&K_temp) ;
	Matrix_NI((float *)&K_temp,2,(float *)&matrix3_NI) ;
	Matrix_Mul_Cross((float *)&(sys->p_variance),2,2,(float *)&matrix3_NI ,2,2,(float *)&(sys->K_gain));
	
///step4 correct x
	float Z_delta[2][1] ;
	float statue_correct[2][1] ;
	
	Z_delta[0][0] = sys->x_state[0][0] - sys->y_oberve[0][0];
	Z_delta[1][0] = sys->x_state[1][0] - sys->y_oberve[1][0];
	
	Matrix_Mul_Cross((float *)&(sys->K_gain),2,2,(float *)&Z_delta ,2,1,(float *)&statue_correct);
	
	sys->x_state[0][0] =  sys->x_state[0][0] - statue_correct[0][0];
  sys->x_state[1][0] =  sys->x_state[1][0] - statue_correct[1][0];
	
///	step5 update P
	float P_update[2][2] ;
	
	Matrix_Init_eye((float *)&P_temp ,2,2);
	Matrix_deduct((float *)&P_temp, (float *)&(sys->K_gain) ,2,2,(float *)&P_temp);
	Matrix_Mul_Cross((float *)&P_temp,2,2,(float *)&(sys->p_variance) ,2,2,(float *)&P_update);
	Matrix_Mul_dot((float *)&P_update,1,2,2,(float *)&(sys->p_variance));
	
}

卡尔曼滤波结构体：

typedef struct
{
	float x_state[2][1];     //状态
	float y_oberve[2][1];		//观测
	float A_matrix[2][2];		//驱动
	float p_variance[2][2];	//协方差
	float K_gain[2][2]; 		//卡尔曼增益
	float R_variance[2][2]; //观测误差方差
	float Q_variance[2][2];	//过程误差方差
}Second_order_sys;;

这里用二维数组当做矩阵，左边是行数，右边是列数，[2][1]就是两行一列的矩阵。

函数调用：

int Horizon_counter =0;
void Horizon_POS_estimate()
{
	Horizon_PV.E_Vel = Hubu_filter(0.75,LC306_flow.flow_x,UAV_GPS_Home.E_speed);
	Horizon_PV.N_Vel = Hubu_filter(0.75,LC306_flow.flow_y,UAV_GPS_Home.N_speed);
	Horizon_PV.E_pos = UAV_GPS_Home.E_deviation;
	Horizon_PV.N_pos = UAV_GPS_Home.N_deviation;
	
	Horizon_counter++;
	if(Horizon_counter>100)
	{
		Horizon_counter=0;
	}
	
	if(Horizon_counter % 5 ==0)
	{
		Horizon_klm_estimate(UAV_GPS_Home.E_deviation , Horizon_PV.E_Vel ,&Horizon_E_klm ,0.04);
		Horizon_klm_estimate(UAV_GPS_Home.N_deviation , Horizon_PV.N_Vel ,&Horizon_N_klm ,0.04);
		
	}


}

代码量少，易懂，我自己觉得挺好用的。
如果想改成其他形式的系统，就只要改A矩阵以及H矩阵。
如果想用我这个矩阵运算库的，请看我上传的资源。