kalman2view.cpp

VC++实现的kalman代码

// Kalman2View.cpp : implementation of the CKalman2View class
//

#include "stdafx.h"
#include "Kalman2.h"

#include "Kalman2Doc.h"
#include "Kalman2View.h"
#include "SetDlg.h"




#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CKalman2View

IMPLEMENT_DYNCREATE(CKalman2View, CView)

BEGIN_MESSAGE_MAP(CKalman2View, CView)
	//{{AFX_MSG_MAP(CKalman2View)
	ON_COMMAND(ID_DISPLAY, OnDisplay)
	ON_COMMAND(ID_DIS_ERROR, OnDisError)
	ON_COMMAND(ID_FILTER_LMS, OnFilterLms)
	ON_COMMAND(ID_LMS_ERROR, OnLmsError)
	ON_COMMAND(ID_PARA_SET, OnParaSet)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalman2View construction/destruction

CKalman2View::CKalman2View()
{
	// TODO: add construction code here
	alfa=1.0/60;
	M=12;
	m_Model=0;
	A=0.03;

}

CKalman2View::~CKalman2View()
{
}

BOOL CKalman2View::PreCreateWindow(CREATESTRUCT& cs)
{
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CKalman2View drawing

void CKalman2View::OnDraw(CDC* pDC)
{
	CKalman2Doc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here
}

/////////////////////////////////////////////////////////////////////////////
// CKalman2View diagnostics

#ifdef _DEBUG
void CKalman2View::AssertValid() const
{
	CView::AssertValid();
}

void CKalman2View::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}

CKalman2Doc* CKalman2View::GetDocument() // non-debug version is inline
{
	ASSERT(m_pDocument->IsKindOf(RUNTIME_CLASS(CKalman2Doc)));
	return (CKalman2Doc*)m_pDocument;
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CKalman2View message handlers



void CKalman2View::OnDisplay() 
{
	// TODO: Add your command handler code here

	int i;
	singer.alfa=alfa;
	if(m_Model==false)
		singer.Filter();
	else
		singer.Filter_2();
	RedrawWindow();

	CDC *pDC;
	pDC=GetDC();

	int x0,y0;
	double xscale,yscale;
	xscale=0.1;
	yscale=0.05;
	x0=200;
	y0=550;
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0-1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+1000);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-1000);

//	pDC->TextOut(x0+5,y0+5,"(0,0)");
	CString str1;

	for(i=-10;i<=10;i++)
	{
		pDC->MoveTo((int)(x0+i*1000*xscale),y0-2);
		pDC->LineTo(int(x0+i*1000*xscale),y0+2);
		

	
		
		pDC->MoveTo((int)(x0-2),(int)(y0+i*1000*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*1000*yscale));
		str1.Format("%d",(int)(i*1000));
    	str1=str1+"m";
		pDC->TextOut((int)(x0+i*1000*xscale)+10,y0+10,str1);
		pDC->TextOut(x0+10,(int)(y0-i*1000*yscale)+10,str1);


	}
		//i*1000*xscale
	
	
	pDC->TextOut(0,50,"黑色线__真实值");
	pDC->TextOut(0,100,"绿色线__观测值");
	pDC->TextOut(0,150,"红色线__滤波值");







	CPen pen1(PS_SOLID,1,RGB(255,0,0)),pen2(PS_SOLID,1,RGB(0,255,0)),pen3(PS_SOLID,1,RGB(0,0,255));

	pDC->MoveTo((int)(x0+singer.XY_Real[0][0]*xscale),(int)(y0-singer.XY_Real[0][1]*yscale));
	

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Real[i][0]*xscale),(int)(y0-singer.XY_Real[i][1]*yscale));
	}

	pDC->SelectObject(pen2);
	pDC->MoveTo((int)(x0+singer.XY_Obsv[0][0]*xscale),(int)(y0-singer.XY_Obsv[0][1]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Obsv[i][0]*xscale),(int)(y0-singer.XY_Obsv[i][1]*yscale));
	}

	pDC->SelectObject(pen1);
	pDC->MoveTo((int)(x0+singer.XY_Filt[0][0]*xscale),(int)(y0-singer.XY_Filt[0][1]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Filt[i][0]*xscale),(int)(y0-singer.XY_Filt[i][1]*yscale));
	}

}

void CKalman2View::OnDisError() 
{
	// TODO: Add your command handler code here
	
	singer.CalError(M,m_Model);
	RedrawWindow();
	int i;
	CDC *pDC;
	pDC=GetDC();

	int x0,y0;
	double xscale=2,yscale=1;
	x0=100;
	y0=200;

	CString str1;	
	str1.Format("%s%d","滤波次数",M);
	pDC->TextOut(30,30,str1);
	
	//画坐标轴

	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+120);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-120);



//画坐标点
	for(i=0;i<=12;i++)
	{
		pDC->MoveTo((int)(x0+i*30*xscale),y0-2);
    	pDC->LineTo(int(x0+i*30*xscale),y0+2);
		

	
		
		str1.Format("%d",(int)(i*30));
    	//str1=str1+"";
		pDC->TextOut((int)(x0+i*30*xscale)+10,y0+10,str1);
		//pDC->TextOut(x0+10,(int)(y0-i*30*yscale)+10,str1);


	}
	for(i=-6;i<7;i++)
	{
		pDC->MoveTo((int)(x0-2),(int)(y0+i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*20*yscale));
		pDC->MoveTo((int)(x0-2),(int)(y0-i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0-i*20*yscale));

		str1.Format("%d",(int)(-i*20));
		pDC->TextOut(x0-30 ,(int)(y0+i*20*yscale),str1);

	}
	

	pDC->TextOut(x0+300,y0+100,"X、Y滤波误差均值");
	CPen pen1(PS_SOLID,1,RGB(255,0,0)),pen2(PS_SOLID,1,RGB(0,255,0)),pen3(PS_SOLID,1,RGB(0,0,0));
	pDC->SelectObject(pen1);
//	pDC->MoveTo((int)(x0),(int)(y0));
//	pDC->LineTo((int)(x0+1000),(int)(y0));
	
	pDC->MoveTo((int)(x0),(int)(y0-singer.ex[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.ex[i]*yscale));
	}

	pDC->SelectObject(pen2);
	pDC->MoveTo((int)(x0),(int)(y0-singer.ey[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.ey[i]*yscale));
	}
	y0=y0+300;
	
	pDC->SelectObject(pen3);
		//画坐标轴
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+120);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-120);

 //画坐标点
	for(i=0;i<=12;i++)
	{
		pDC->MoveTo((int)(x0+i*30*xscale),y0-2);
    	pDC->LineTo(int(x0+i*30*xscale),y0+2);
		

	
		
		str1.Format("%d",(int)(i*30));
    	//str1=str1+"";
		pDC->TextOut((int)(x0+i*30*xscale)+10,y0+10,str1);
		//pDC->TextOut(x0+10,(int)(y0-i*30*yscale)+10,str1);


	}
	for(i=-6;i<7;i++)
	{
		pDC->MoveTo((int)(x0-2),(int)(y0+i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*20*yscale));
		pDC->MoveTo((int)(x0-2),(int)(y0-i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0-i*20*yscale));

		str1.Format("%d",(int)(-i*20));
		pDC->TextOut(x0-30 ,(int)(y0+i*20*yscale),str1);

	}

	pDC->TextOut(x0+300,y0+100,"X、Y滤波误差标准差");
	pDC->SelectObject(pen1);
//	pDC->MoveTo((int)(x0),(int)(y0));
//	pDC->LineTo((int)(x0+1000),(int)(y0));
	
	pDC->MoveTo((int)(x0),(int)(y0-singer.dx[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.dx[i]*yscale));

	}
	pDC->SelectObject(pen2);
		pDC->MoveTo((int)(x0),(int)(y0-singer.dy[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.dy[i]*yscale));

	}

}

void CKalman2View::OnFilterLms() 
{
	// TODO: Add your command handler code here
	
	int i;
	singer.alfa=alfa;

	singer.Filter_LMS();
	RedrawWindow();

	CDC *pDC;
	pDC=GetDC();

	int x0,y0;
	double xscale,yscale;
	xscale=0.1;
	yscale=0.05;
	x0=200;
	y0=550;
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0-1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+1000);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-1000);

//	pDC->TextOut(x0+5,y0+5,"(0,0)");
	CString str1;

	for(i=-10;i<=10;i++)
	{
		pDC->MoveTo((int)(x0+i*1000*xscale),y0-2);
		pDC->LineTo(int(x0+i*1000*xscale),y0+2);
		

	
		
		pDC->MoveTo((int)(x0-2),(int)(y0+i*1000*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*1000*yscale));
		str1.Format("%d",(int)(i*1000));
    	str1=str1+"m";
		pDC->TextOut((int)(x0+i*1000*xscale)+10,y0+10,str1);
		pDC->TextOut(x0+10,(int)(y0-i*1000*yscale)+10,str1);


	}
		//i*1000*xscale
	
	
	pDC->TextOut(0,50,"黑色线__真实值");
	pDC->TextOut(0,100,"绿色线__观测值");
	pDC->TextOut(0,150,"红色线__滤波值");







	CPen pen1(PS_SOLID,1,RGB(255,0,0)),pen2(PS_SOLID,1,RGB(0,255,0)),pen3(PS_SOLID,1,RGB(0,0,255));

	pDC->MoveTo((int)(x0+singer.XY_Real[0][0]*xscale),(int)(y0-singer.XY_Real[0][1]*yscale));
	

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Real[i][0]*xscale),(int)(y0-singer.XY_Real[i][1]*yscale));
	}

	pDC->SelectObject(pen2);
	pDC->MoveTo((int)(x0+singer.XY_Obsv[0][0]*xscale),(int)(y0-singer.XY_Obsv[0][1]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Obsv[i][0]*xscale),(int)(y0-singer.XY_Obsv[i][1]*yscale));
	}

	pDC->SelectObject(pen1);
	pDC->MoveTo((int)(x0+singer.XY_Filt[0][0]*xscale),(int)(y0-singer.XY_Filt[0][1]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+singer.XY_Filt[i][0]*xscale),(int)(y0-singer.XY_Filt[i][1]*yscale));
	}


	
}

void CKalman2View::OnLmsError() 
{
	// TODO: Add your command handler code here
	singer.CalErrorLms(M);
	RedrawWindow();
	int i;
	CDC *pDC;
	pDC=GetDC();

	int x0,y0;
	double xscale=2,yscale=1;
	x0=100;
	y0=200;

	CString str1;	
	str1.Format("%s%d","滤波次数",M);
	pDC->TextOut(30,30,str1);
	
	//画坐标轴

	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+120);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-120);



//画坐标点
	for(i=0;i<=12;i++)
	{
		pDC->MoveTo((int)(x0+i*30*xscale),y0-2);
    	pDC->LineTo(int(x0+i*30*xscale),y0+2);
		

	
		
		str1.Format("%d",(int)(i*30));
    	//str1=str1+"";
		pDC->TextOut((int)(x0+i*30*xscale)+10,y0+10,str1);
		//pDC->TextOut(x0+10,(int)(y0-i*30*yscale)+10,str1);


	}
	for(i=-6;i<7;i++)
	{
		pDC->MoveTo((int)(x0-2),(int)(y0+i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*20*yscale));
		pDC->MoveTo((int)(x0-2),(int)(y0-i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0-i*20*yscale));

		str1.Format("%d",(int)(-i*20));
		pDC->TextOut(x0-30 ,(int)(y0+i*20*yscale),str1);

	}
	

	pDC->TextOut(x0+300,y0+100,"X、Y滤波误差均值");
	CPen pen1(PS_SOLID,1,RGB(255,0,0)),pen2(PS_SOLID,1,RGB(0,255,0)),pen3(PS_SOLID,1,RGB(0,0,0));
	pDC->SelectObject(pen1);
//	pDC->MoveTo((int)(x0),(int)(y0));
//	pDC->LineTo((int)(x0+1000),(int)(y0));
	
	pDC->MoveTo((int)(x0),(int)(y0-singer.ex[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.ex[i]*yscale));
	}

	pDC->SelectObject(pen2);
	pDC->MoveTo((int)(x0),(int)(y0-singer.ey[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.ey[i]*yscale));
	}
	y0=y0+300;
	
	pDC->SelectObject(pen3);
		//画坐标轴
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+1000,y0);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0+120);
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0,y0-120);

 //画坐标点
	for(i=0;i<=12;i++)
	{
		pDC->MoveTo((int)(x0+i*30*xscale),y0-2);
    	pDC->LineTo(int(x0+i*30*xscale),y0+2);
		

	
		
		str1.Format("%d",(int)(i*30));
    	//str1=str1+"";
		pDC->TextOut((int)(x0+i*30*xscale)+10,y0+10,str1);
		//pDC->TextOut(x0+10,(int)(y0-i*30*yscale)+10,str1);


	}
	for(i=-6;i<7;i++)
	{
		pDC->MoveTo((int)(x0-2),(int)(y0+i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0+i*20*yscale));
		pDC->MoveTo((int)(x0-2),(int)(y0-i*20*yscale));
		pDC->LineTo(int(x0+2),(int)(y0-i*20*yscale));

		str1.Format("%d",(int)(-i*20));
		pDC->TextOut(x0-30 ,(int)(y0+i*20*yscale),str1);

	}

	pDC->TextOut(x0+300,y0+100,"X、Y滤波误差标准差");
	pDC->SelectObject(pen1);
//	pDC->MoveTo((int)(x0),(int)(y0));
//	pDC->LineTo((int)(x0+1000),(int)(y0));
	
	pDC->MoveTo((int)(x0),(int)(y0-singer.dx[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.dx[i]*yscale));

	}
	pDC->SelectObject(pen2);
		pDC->MoveTo((int)(x0),(int)(y0-singer.dy[0]*yscale));

	for(i=1;i<350;i++)
	{
		pDC->LineTo((int)(x0+i*xscale),(int)(y0-singer.dy[i]*yscale));

	}

	
}

void CKalman2View::OnParaSet() 
{
	// TODO: Add your command handler code here
		CSetDlg dlg;
	dlg.m_alfa=alfa;
	dlg.m_times=M;
	dlg.m_Model=m_Model;
	dlg.m_A=A;


	if(dlg.DoModal()==IDOK)
		
	{
		alfa=dlg.m_alfa;
		M=dlg.m_times;
		m_Model=dlg.m_Model;
		A=dlg.m_A;
	}
		
	
}