kerlman.cpp

卡尔曼滤波程序,关于数字信号处理中卡尔曼滤波的示范性程序

 #include<windows.h>
#include<stdio.h>
#include<time.h>
#include<stdlib.h>
#include<math.h>
#define N_gauss 256 //需要产生的高斯白噪声序列的点的个数

double *gauss(double ex,double dx,int n_point)//ex:均值；dx:方差；n_point:点数
{ 
  time_t t;
  int i;
  double *mem1;
  mem1=(double*)malloc(n_point*sizeof(double));
  srand((unsigned)time(&t));
  for(i=0;i<n_point;i++)
   mem1[i]=(sqrt(-2*log((double)rand()/32768))*cos((double)rand()/32768*2*3.1415926))*sqrt(dx)+ex;
  return(mem1);
}

LRESULT CALLBACK WndProc (HWND, UINT, WPARAM, LPARAM) ;

int WINAPI WinMain (HINSTANCE hInstance, HINSTANCE hPrevInstance,
                    PSTR szCmdLine, int iCmdShow)
{
     static TCHAR szAppName[] = TEXT ("LineDemo") ;
     HWND         hwnd ;
     MSG          msg ;
     WNDCLASS     wndclass ;
     
     wndclass.style         = CS_HREDRAW | CS_VREDRAW ;
     wndclass.lpfnWndProc   = WndProc ;
     wndclass.cbClsExtra    = 0 ;
     wndclass.cbWndExtra    = 0 ;
     wndclass.hInstance     = hInstance ;
     wndclass.hIcon         = LoadIcon (NULL, IDI_APPLICATION) ;
     wndclass.hCursor       = LoadCursor (NULL, IDC_ARROW) ;
     wndclass.hbrBackground = (HBRUSH) GetStockObject (WHITE_BRUSH) ;
     wndclass.lpszMenuName  = NULL ;
     wndclass.lpszClassName = szAppName ;
     
     if (!RegisterClass (&wndclass))
     {
          MessageBox (NULL, TEXT ("Program requires Windows NT!"), 
                      szAppName, MB_ICONERROR) ;
          return 0 ;
     }
     
     hwnd = CreateWindow (szAppName, TEXT ("卡尔曼滤波程序。使用VC++编写。姓名: 赵辉, 学号: 200311201"),
                          WS_OVERLAPPEDWINDOW,
                          CW_USEDEFAULT, CW_USEDEFAULT,
                          CW_USEDEFAULT, CW_USEDEFAULT,
                          NULL, NULL, hInstance, NULL) ;
     
     ShowWindow (hwnd, iCmdShow) ;
     UpdateWindow (hwnd) ;
     
     while (GetMessage (&msg, NULL, 0, 0))
     {
          TranslateMessage (&msg) ;
          DispatchMessage (&msg) ;
     }
     return msg.wParam ;
}

LRESULT CALLBACK WndProc (HWND hwnd, UINT message, WPARAM wParam, LPARAM lParam)
{
     static int  cxClient, cyClient;
     HDC         hdc;
     PAINTSTRUCT ps;
	 int         n;
     double      *w,*v;
	 double      s[N_gauss],x[N_gauss],s_out[N_gauss],k[N_gauss],p[N_gauss];

	 for(n=0;n<N_gauss;n++)
		s[n]=x[n]=s_out[n]=k[n]=p[n]=0;
	 
	 w=gauss(0,0.82,N_gauss); //产生激励源:w(n)
	 v=gauss(1,1,N_gauss);    //产生加性噪声:v(n)
		 
	 for(n=1;n<N_gauss;n++)   //状态方程
		 s[n]=s[n-1]+w[n-1];  
	 for(n=0;n<N_gauss;n++)   //观测方程
		 x[n]=s[n]+v[n];
	 
	 s_out[0]=0;
	 p[0]=(double)(0.82/0.64);
	 for(n=1;n<N_gauss;n++)
	 {
		 k[n]=(0.36*p[n-1]+0.82)/(0.36*p[n-1]+0.82+1);
		 p[n]=(1-k[n])*(0.36*p[n-1]+0.82);
		 s_out[n]=0.6*s_out[n-1]+k[n]*(x[n]-0.6*s_out[n-1]);
	 }

	switch (message)
	{
	 case WM_SIZE:
			  cxClient = LOWORD (lParam) ;
			  cyClient = HIWORD (lParam) ;
			  return 0 ;
     case WM_PAINT:
			  hdc = BeginPaint (hwnd, &ps) ;
			 for(n=0;n<N_gauss;n++)     
			  {
				  MoveToEx(hdc,n+30,cyClient/2,NULL);
				  LineTo(hdc,n+30,cyClient/2+(int)(s[n]*30));
			  }
			 /*for(n=0;n<N_gauss;n++)     
			  {
	              MoveToEx(hdc,n+380,cyClient/2,NULL);
				  LineTo(hdc,n+380,cyClient/2+(int)(x[n]*30));
			  }
	         for(n=0;n<N_gauss;n++)     
			  {
	              MoveToEx(hdc,n+740,cyClient/2,NULL);
				  LineTo(hdc,n+740,cyClient/2+(int)(s_out[n]*30));
			  }*/
	          EndPaint (hwnd, &ps) ;
			  return 0 ;
          
	 case WM_DESTROY:
			  PostQuitMessage (0) ;
			  return 0 ;
	}
    return DefWindowProc (hwnd, message, wParam, lParam) ;
}