基于stm32的超声波数据（卡尔曼滤波处理数据）

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "timer.h"
#include "beep.h"
#include "bmp.h"
#include "oled.h"

u8 table1[]={0X03,0XFC};//µ÷²¨ÉèÖÃ
u8 table2[]={0XFC,0X03};

extern u8  TIM5CH1_CAPTURE_STA;		//ÊäÈ벶»ñ״̬		    				
extern u32	TIM5CH1_CAPTURE_VAL;	//ÊäÈ벶»ñÖµ  
long long temp=0;//²â¾àʱ¼ä
long long S=0,s1=0,t=0,Sm=0,Sa=0;//¾àÀë²ÎÊý
int table[]={1,10,100,1000};
/*************½ÓÊÕÊý¾Ý******************
len ´®¿Ú½ÓÊÕµ½µÄÊý¾Ý³¤¶È
s1  ¾­¹ýASIICÂëת»»ºóµÃµ½µÄÊý×Ö£¨·§Öµ£©  
t   ´«µÝÖµ£¬½«s1¸³¸øt£¬È¥Ö´ÐÐÆäËû³ÌÐò
***************************************/
float recevie()
{
	u8 i;
	u8 len;	 
	if(USART_RX_STA&0x8000)//Èç¹û½ÓÊÕµ½Êý¾Ý
		{					   
			len=USART_RX_STA&0x3fff;//µÃµ½´Ë´Î½ÓÊÕµ½µÄÊý¾Ý³¤¶È
			for(i=0;i<len;i++)
			{
			   s1+=(USART_RX_BUF[len-1-i]-48)*table[i];
			}
			t=s1;//½«½ÓÊÕµ½µÄÊý¾Ý¸øµ½t
			s1=0;//s1³õʼ»¯
			USART_RX_STA=0;//״̬λÇåÁã
		}
		printf("\r\nÉèÖõÄ×îС¾àÀëΪ:%lld\r\n",t);
		return t;
}

/***************ÅжϺ¯Êý****************
S  ²âÁ¿µÃµ½µÄ¾àÀë
t  ͨ¹ý´®¿ÚÉèÖõķ§Öµ
***************************************/
float judge()
{ 
	  if(S<t)
		{
			 printf("\r\nÉèÖõÄ×îС¾àÀëΪ:%lld\r\n",t);
		   printf("\r\n error!!!¡\\n");
			 LED1=!LED1;//³¬¹ý·§Öµ£¬±¨¾¯
			 delay_ms(5);
		}
		else
		{   
			  printf("\r\nÉèÖõÄ×îС¾àÀëΪ:%lld\r\n",t);
		    printf("distance:%lld mm\r\n",Sm);//´òÓ¡¾àÀë
			  delay_ms(5);
		}
		return t;
}

/*******************¼ÆËã¾àÀë*****************
temp  ͨ¹ýÊäÈ벶»ñµÃµ½µÄ¸ßµçƽ³ÖÐøʱ¼ä
S     ¾àÀë
*********************************************/
float Count()
{
	 long long a=0;
   if(TIM5CH1_CAPTURE_STA&0X80)        //³É¹¦²¶»ñµ½ÁËÒ»´Î¸ßµçƽ
		{
			temp=TIM5CH1_CAPTURE_STA&0X3F; 
			temp*=0XFFFFFFFF;		 		         //Òç³öʱ¼ä×ܺÍ
			temp+=TIM5CH1_CAPTURE_VAL;		   //µÃµ½×ܵĸߵçƽʱ¼ä
			a=temp;
			S=0.17*a;
			Sa=S;
			TIM5CH1_CAPTURE_STA=0;	//״̬λÇåÁã
     }
		return S;
}
/************¿¨¶ûÂüÂ˲¨***************
z_measure  ËͽøÀ´´¦ÀíµÄÊý¾Ý
Sm   ´¦ÀíºóµÄÊý¾Ý
*************************************/
void kalmanfilter(long long  z_measure)
{
	static float x_mid,x_last=0,p_mid,p_last=0,p_now,Q=0.018,kg,R=0.0542;
	x_mid=x_last;
	p_mid=p_last+Q;
	kg=p_mid/(p_mid+R);
	//z_measure=z_real+rand()*0.03;
	Sm=(long)x_mid+kg*(z_measure-x_mid);
	p_now=(1-kg)*p_mid;
	p_last=p_now;
	x_last=Sm;
}
//µ÷²¨º¯Êý
void send_data(long long x,long long y)
{
   u8 i=0,j=0