EEPW论坛

在EEPW看了很多也学了一些，一些网友总是对滤波有些疑问，对于卡尔曼滤波大家也觉得很难整定卡尔曼参数。下面想针对楼主自己调试kalman滤波器和互补滤波器的一些经验（主要是kalman滤波）：

1>工欲善其事必先利其器——串口调试工具（用以将数据传到电脑上进行对比），楼主这里用的是Serial Digtal V2（Serial_Digital_Scope V2.rar），这个软件还需要下位机对应的代码这里也分享一下（Debug Ways.rar）。

2>下位机软件准备（kalman），记得滤多个数据的话，得准备几个kalman滤波器，参数什么的都不一样，当然也可以整合为一个kalman滤波器下面分享一下代码。（悄悄的告诉大家这是我在烈火四轴里采用的参数什么，代码都是一样的），注释变成乱码了不好意思.........................代码分享（）Kalman filter.rar

/*-------------------------------------------------------------------------------------------------------------------------------------------*/

#include "kalman_filter.h" 

klm_result klm_all_res ;   /* 卡尔曼滤波器输出结果 */
   
//****************************************************************
//kalman滤波所需参数初始化

//****************************************************************
klm_prameter kalman_pra ;  /* 定义滤波参数结构体 */

void kalman_prameter_init(void)
{
	/* X轴滤波所需参数 */

	kalman_pra.x_Q_angle =500.0 ; 
	kalman_pra.x_Q_gyro =500.0;
	kalman_pra.x_R_angle = 3000.0 ;    
	/* Y轴滤波所需参数 * /

	kalman_pra.y_Q_angle = 500.0; 
	kalman_pra.y_Q_gyro = 500.0 ;
	kalman_pra.y_R_angle = 3000.0;  
	/* 由于没有指南所以不需要Z轴，Z轴为航向为陀螺仪直接积分 */

}

//*******************************************************************
//Kalman 滤波器

//*******************************************************************
const float dt=0.005; /* dt为采样时间 */ 
static float x_P[2][2] = {
    { 1, 0 },
    { 0, 1 }
};
static float y_P[2][2] = {
    { 1, 0 },
    { 0, 1 }
};


static float x_Pdot[4] ={0,0,0,0};
static float y_Pdot[4] ={0,0,0,0};


static const char C_0 = 1;

static float  x_PCt_0, x_PCt_1, x_E, x_K_0, x_K_1, x_t_0, x_t_1;
static float  y_PCt_0, y_PCt_1, y_E, y_K_0, y_K_1, y_t_0, y_t_1;

//-------------------------------------------------------
void Kalman_Filter(S_FLOAT_XYZ *angle_m,S_FLOAT_XYZ *gyro_m )      
  //gyro_m:gyro_measure
{		

    klm_all_res.x_angle +=(gyro_m->X - kalman_pra.x_q_bias) * dt;
    klm_all_res.y_angle +=(gyro_m->Y - kalman_pra.y_q_bias) * dt;
  
	
    x_Pdot[0]=kalman_pra.x_Q_angle - x_P[0][1] - x_P[1][0]; //Q_angle - 1 - 0 ;
    x_Pdot[1]= -x_P[1][1]; // Pdot[1] = -1
    x_Pdot[2]= -x_P[1][1]; // -1
    x_Pdot[3]=kalman_pra.x_Q_gyro;
	
    y_Pdot[0]=kalman_pra.y_Q_angle - y_P[0][1] - y_P[1][0]; //Q_angle - 1 - 0 ;
    y_Pdot[1]= -y_P[1][1]; // Pdot[1] = -1
    y_Pdot[2]= -y_P[1][1]; // -1
    y_Pdot[3]=kalman_pra.y_Q_gyro;
	
    x_P[0][0] += x_Pdot[0] * dt;
    x_P[0][1] += x_Pdot[1] * dt;
    x_P[1][0] += x_Pdot[2] * dt;
    x_P[1][1] += x_Pdot[3] * dt;
		
    y_P[0][0] += y_Pdot[0] * dt;
    y_P[0][1] += y_Pdot[1] * dt;
    y_P[1][0] += y_Pdot[2] * dt;
    y_P[1][1] += y_Pdot[3] * dt;

		
    kalman_pra.x_angle_err = angle_m->X - klm_all_res.x_angle;
    kalman_pra.y_angle_err = angle_m->Y - klm_all_res.y_angle;
    
    x_PCt_0 = C_0 * x_P[0][0];
    x_PCt_1 = C_0 * x_P[1][0];     

 
    y_PCt_0 = C_0 * y_P[0][0];
    y_PCt_1 = C_0 * y_P[1][0];    

      
    x_E = kalman_pra.x_R_angle + C_0 * x_PCt_0;
    y_E = kalman_pra.y_R_angle + C_0 * y_PCt_0;
   
    x_K_0 = x_PCt_0 / x_E;
    x_K_1 = x_PCt_1 / x_E;  
    
    y_K_0 = y_PCt_0 / y_E;
    y_K_1 = y_PCt_1 / y_E;  
    
    x_t_0 = x_PCt_0;
    x_t_1 = C_0 * x_P[0][1];
		
    y_t_0 = y_PCt_0;
    y_t_1 = C_0 * y_P[0][1];
				
    x_P[0][0] -= x_K_0 * x_t_0;
    x_P[0][1] -= x_K_0 * x_t_1;
    x_P[1][0] -= x_K_1 * x_t_0;
    x_P[1][1] -= x_K_1 * x_t_1;
		

    y_P[0][0] -= y_K_0 * y_t_0;
    y_P[0][1] -= y_K_0 * y_t_1;
    y_P[1][0] -= y_K_1 * y_t_0;
    y_P[1][1] -= y_K_1 * y_t_1;
	
    klm_all_res.x_angle += x_K_0 * kalman_pra.x_angle_err;
    kalman_pra.x_q_bias= kalman_pra.x_q_bias + x_K_1 * kalman_pra.x_angle_err;
    klm_all_res.x_angle_dot = gyro_m->X-kalman_pra.x_q_bias;  
		
    klm_all_res.y_angle += y_K_0 * kalman_pra.y_angle_err;
    kalman_pra.y_q_bias= kalman_pra.y_q_bias + y_K_1 * kalman_pra.y_angle_err;
    klm_all_res.y_angle_dot = gyro_m->Y-kalman_pra.y_q_bias;  
}

/*------------------------------------------------------------------------------ */

这里说一下注意事项：

1> Serial Digital Scop只能用9600波特率，其它波特率不支持

2> 话说2呢，嗯就没有2了，

/*-------------------------------------------------------------------------------*/

申明：

     楼主只讲调试经验，理论楼主不行，自行研究。完毕。

/*-------------------------------------------------------------------------------*/

如何调试kalman滤波？

kalman滤波一个轴只需要三个参数进行调试，你需要将ACC转化的角度和通过Kalman滤波器滤波的角度通过下位机程序发送到上位机上，通过观察波形来确定参数的调试。这里我们都认为ACC的角度时时都是比较准确的只是噪声比较大，没法直接用。

下面说一个几个参数的作用：

1>Q_angle控制的是kalman滤波的左右平移。（大概是这样的，至于为什么请大神探讨。）

2>Q_gyro这个参数表式你信任gyro有多少，数字越大你信任得越少。这里没有图片还没发说。我去，等等吧会有机会的。（这里等楼主再完善吧）

3>R_angle表示的是你信任ACC有多少，越大信任越小。这个主要是调试整体滤出波形跟随ACC转化的情况。

这个参数越大滤出的波形跟随ACC转化的波越慢。

/*-------------------------------------------------------------------------------*/

1>此贴未完，且听下回分解。（谢谢）

2>最后申明此贴是楼主私人观点，有何疑问清留言。有问题一起讨论不要骂楼主。