超维空间S2无人机使用说明书——54、代码详解：递推+滤波算法——过滤无效值

引言： 在实际工程应用中，不会是仿真一样的理想情况，通常会存在各种干扰，为了降低干扰的影响，一般采用软件滤波的方式进行，条件允许的话，也可以直接采用硬件滤波的方式。需要指出的是，不同的干扰类型，一般采用不同的滤波算法，或者采用叠加的滤波算法进行抗干扰，以此提高系统的稳定性。

本次实验的背景是在进行图像识别的时候，较高的图像发布帧率会导致返回的目标位置数据出现丢失的情况。基于此，本文汉主要采取了递推滤波算法和均值滤波算法

原理：相当于“递推平均滤波法”。这种方法是,把连续N个采样值看成一个队列,队列的长度固定为N。每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据(先进先出原则)。把队列中的N个数据先去掉一个最大值和一个最小值,然后计算N-2个数据的平均值。这里根据实际情况进行简单的修改，不是去掉最大值和最小值，首先确保是否可信，在基础上再进行滤波

数据分析：本次采样的数据，是获取目标物的位置距离信息，偶尔会出现目标丢失的情况，此时返回的数值是0，可以基于此判断此时是丢失了目标。

步骤一、每次获取N个数据，根据目前观测到的实际目标丢失情况，代码每次采样五个数据为一组

（1）、 返回数据分别是三维位置信息，为此设置3个数组，分别接收保存，并进行计数。为了简便代码，也可以设置二维数组。此处设置3个数组

//设置全局变量
double positon_x_table[5]={};
double positon_y_table[5]={};
double distance_table[5] ={};
int count_positon_x = 0;
int count_positon_y = 0;
int count_distance  = 0;

(2)、每次回去最新数值，并且进行保存和计数，确保每次都是按照5个数据进行处理//此处将距离由单位米改称毫米，方便提高控制精度

	//获取五次X方向数据，其中temp_xxx是每次获取的最新值，计数器用于确保每次都有最新数值加入
	positon_x_table[count_positon_x] = temp_current_position_x;
    count_positon_x++;
    if(count_positon_x>=5)
    {
    	count_positon_x = 0;
    } 
    //获取五次Y方向数据
    positon_y_table[count_positon_y] = temp_current_position_y;
    count_positon_y++;
    if(count_positon_y>=5)
    {
    	count_positon_y = 0;
    }
    //获取五次距离数据
    distance_table[count_distance] = temp_current_distance;
    count_distance++;
    if(count_distance>=5)
    {
    	count_distance = 0;
    } 

(3)、遍历数组，判断是否存在无效数组，并进行计数

//设置临时数组变量，用于保存有效值，排除无效值
double temp_positon_x_table[5]={};
double temp_positon_y_table[5]={};
double temp_distance_table[5] ={};
//遍历数据查找是否有丢失目标的情况，每丢失一次，则计数器+1
int temp_i = 0;
for(int i=0;i<=4;i++)
{
    //所有数据为0，可以判定没有识别到目标，此处数据为无效值，不进行保存
    if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0)
    {
    	count_target_lost++;
    	ROS_INFO("count_target_lost = %d",count_target_lost); 
    }
    //将获取到的有效值存入临时数组变量，准备进行均值滤波处理
    else
    {
    	temp_positon_x_table[temp_i] =  positon_x_table[i];
    	temp_positon_y_table[temp_i] =  positon_y_table[i];
    	temp_distance_table[temp_i]  =  distance_table[i];
    	temp_i++;
    }
}
//如果5次里面丢失超过3次，直接判定为丢失目标，可能是其他干扰导致的误识别或者本身就是没有识别到目标
//直接将当前识别到的目标位置全部设置为0即可
if(count_target_lost>3)
{
    current_position_x = 0;
    current_position_y = 0;
    current_distance   = 0;
}
//如果认定数组里的数据有3个以上是有效的，直接进行均值滤波即可
//也可以采用中位均值滤波，除去最高与最低后采用均值滤波算法
//这里采样数据较少，直接均值滤波即可，可以改称采样10次进行中位均值滤波
else
{
    current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;
    current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;
    current_distance   = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;
}

通过以上算法可以实现简单的滤波了，从实际无人机测试视频效果看，无人机的识别跟随效果有了明显改善，根本原因是系统基本不再出现无效数据，消除了数据毛刺给无人机控制带来的抖动

整体代码如下：

void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{
	//ROS_INFO("current_position_x = %f");
	double temp_current_position_x;
	double temp_current_position_y;
	double temp_current_distance;
	int count_target_lost = 0;
	object_pos = *msg;
	current_frame_id   = object_pos.header.frame_id; 
	
	//获取最新的rpy值
	tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);	
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
	//printf("pitch = %f\r\n",pitch);
	
	//此处将距离由单位米改称毫米，方便提高控制精度
	temp_current_position_x = object_pos.point.x*(-1000);
    temp_current_position_y = object_pos.point.y*(-1000);
	temp_current_distance   = object_pos.point.z*(1000)/cos(pitch);

	//printf("temp_current_distance = %f\r\n",temp_current_distance);

	//获取五次X方向数据
	positon_x_table[count_positon_x] = temp_current_position_x;
    count_positon_x++;
    if(count_positon_x>=5)
    {
    	count_positon_x = 0;
    } 
    //获取五次Y方向数据
    positon_y_table[count_positon_y] = temp_current_position_y;
    count_positon_y++;
    if(count_positon_y>=5)
    {
    	count_positon_y = 0;
    }
    //获取五次距离数据
    distance_table[count_distance] = temp_current_distance;
    count_distance++;
    if(count_distance>=5)
    {
    	count_distance = 0;
    } 
    
    double temp_positon_x_table[5]={};
	double temp_positon_y_table[5]={};
	double temp_distance_table[5] ={};
    //遍历数据查找是否有丢失目标的情况，每丢失一次，则计数器+1
    int temp_i = 0;
    for(int i=0;i<=4;i++)
    {
    	//所有数据为0，可以判定没有识别到目标
    	if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0)
    	{
    		count_target_lost++;
    		ROS_INFO("count_target_lost = %d",count_target_lost); 
    	}
    	else
    	{
    		temp_positon_x_table[temp_i] =  positon_x_table[i];
    		temp_positon_y_table[temp_i] =  positon_y_table[i];
    		temp_distance_table[temp_i]  =  distance_table[i];
    		temp_i++;
    	}
    }
    //如果5次里面丢失超过3次，直接判定为识别到目标，可能是其他干扰导致的误识别或者本身就是没有识别到目标
    if(count_target_lost>3)
    {
    	current_position_x = 0;
    	current_position_y = 0;
    	current_distance   = 0;
    }
    //如果认定数组里的数据有3个以上是有效的，那么应该除去最高与最低后采用均值滤波算法
    else
    {
    	current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;
    	current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;
    	current_distance   = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;
    }
    
	if(tmp_flag_frame == 1)
	{
		pid_control();	 
	}
}