ky9250(mpu9250)取得原始数据后通过简易卡尔曼滤波获取角度

我们通过ky9250(mpu9250)取得原始数据后（gx,gy,gz,ax,ay,az,mx,my,mz）后想通过原始数据解算角度姿态信息(想试验各种算法比如卡尔曼、mahony,Madgwick)，现将使用简易卡尔曼滤波获取姿态角度roll,pitch,yaw的方法介绍如下：

未完稍后继续....

1、首先对ax,ay,az,mx,my,mz取模

norm = sqrt(ax * ax + ay * ay + az * az);
  if (norm == 0.0f) return; // handle NaN
  norm = 1.0f/norm;
  ax *= norm;
  ay *= norm;
  az *= norm;

norm2 =  sqrt(mx * mx + my * my + mz * mz);
  if (norm2 != 0.0f)  
  norm2 = 1.0f / norm2;      
  mx *= norm2;
  my *= norm2;
  mz *= norm2;

2、然后通过加速度分别计算roll,pitch,yaw

float RAD_TO_DEG=57.29578f;

//算得Roll角。算法见文档。
float GetRoll(float ax,float ay,float az,float gx)
	{
 
	
    float roll1;		
	//roll1 = atan2(ay,az) * RAD_TO_DEG; //第一种算法
		
	roll1 =  atan2(ay , sqrt(ax * ax + az * az)) * RAD_TO_DEG;  //第二种算法(稍后来测试)
		
	return roll1;	
		
 }

//算得Pitch角。算法见文档。
float GetPitch(float ax ,float ay,float az,float gy) 
{
 		
  float pitch1;
  pitch1 = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;		 //第一种算法
	
  //pitch1 = atan2(-accX, accZ) * RAD_TO_DEG;	第二种算法(稍后来测试)	
	
	
	return pitch1;	
} 

//算得yaw角。算法见文档。
float GetYaw(float mx ,float my,float mz,float gz) 
{
 		
  float yaw1;
  yaw1 = atan2f(my, mx)* RAD_TO_DEG;		 //第一种算法
	
  //第二种算法
	
	
	return yaw1;	
}

3、把算好的roll,pitch,yaw代入卡尔曼滤波函数（函数见下面代码）

简易卡尔曼滤波

    angle2=roll1;
	Kalman_Filter(angle2, gx);   
	roll2=angle2;

    angle3=pitch1;
	Kalman_Filter(angle3, gy);   
	roll2=angle3;

    angle4=yaw1;
	Kalman_Filter(angle4, gz);   
	yaw2=angle4;

roll2,pitch2,yaw2 即为滤波后的角度姿态

4、简易卡尔曼滤波函数见下：

filter.h

extern float angle, angle_dot; 	
extern float angle2,angle3;

float Kalman_Filter(float Accel,float Gyro);


filter.c

float K1 =0.02; 
float angle, angle_dot; 	

float angle2,angle3;  

float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003;//0.03 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差 既测量偏差
float dt=0.005;//                 
char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回  值：无
**************************************************************************/
float Kalman_Filter(float Accel,float Gyro)		
{
	angle+=(Gyro - Q_bias) * dt; //先验估计
	Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

	Pdot[1]=-PP[1][1];
	Pdot[2]=-PP[1][1];
	Pdot[3]=Q_gyro;
	PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
	PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
	PP[1][0] += Pdot[2] * dt;
	PP[1][1] += Pdot[3] * dt;
		
	Angle_err = Accel - angle;	//zk-先验估计
	
	PCt_0 = C_0 * PP[0][0];
	PCt_1 = C_0 * PP[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * PP[0][1];

	PP[0][0] -= K_0 * t_0;		 //后验估计误差协方差
	PP[0][1] -= K_0 * t_1;
	PP[1][0] -= K_1 * t_0;
	PP[1][1] -= K_1 * t_1;
		
	angle	+= K_0 * Angle_err;	 //后验估计
	Q_bias	+= K_1 * Angle_err;	 //后验估计
	angle_dot   = Gyro - Q_bias;	 //输出值(后验估计)的微分=角速度
	
	return angle_dot;
}