kalmanview.cpp

执行对一个 L 航迹进行Kalman滤波

// KalmanView.cpp : implementation of the CKalmanView class
//

#include "stdafx.h"
#include "Kalman.h"

#include "KalmanDoc.h"
#include "KalmanView.h"

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

/////////////////////////////////////////////////////////////////////////////
// CKalmanView

IMPLEMENT_DYNCREATE(CKalmanView, CView)

BEGIN_MESSAGE_MAP(CKalmanView, CView)
	//{{AFX_MSG_MAP(CKalmanView)
		// NOTE - the ClassWizard will add and remove mapping macros here.
		//    DO NOT EDIT what you see in these blocks of generated code!
	//}}AFX_MSG_MAP
	// Standard printing commands
	ON_COMMAND(ID_FILE_PRINT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_DIRECT, CView::OnFilePrint)
	ON_COMMAND(ID_FILE_PRINT_PREVIEW, CView::OnFilePrintPreview)
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalmanView construction/destruction

CKalmanView::CKalmanView()
{
	// TODO: add construction code here
    alfa=1.0/60;
	M=12;
	A=0.03;
	e_xx=0;
	e_yy=0;
	d_xx=0;
	d_yy=0;
}

CKalmanView::~CKalmanView()
{
}

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

	return CView::PreCreateWindow(cs);
}

/////////////////////////////////////////////////////////////////////////////
// CKalmanView drawing

void CKalmanView::OnDraw(CDC* pDC)
{
	CKalmanDoc* pDoc = GetDocument();
	ASSERT_VALID(pDoc);
	// TODO: add draw code for native data here
	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)),pen4(PS_SOLID,2,RGB(0,0,0));
	pDC->SelectObject(pen4);
	int i;
	int x0,y0;
	double xscale,yscale;
	xscale=0.1;
	yscale=0.05;
	x0=200;
	y0=550;
	pDC->MoveTo(x0,y0);
	pDC->LineTo(x0+800,y0);
	pDC->LineTo(x0+800,(int)(y0-10000*yscale));
	pDC->LineTo(x0,(int)(y0-10000*yscale));
	
	//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,(int)(y0-10000*yscale));

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

	for(i=0;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)-30,y0+10,str1);
		pDC->TextOut(x0-55,(int)(y0-i*1000*yscale)+10,str1);


	}
		//i*1000*xscale
	
	
	pDC->TextOut(25,50,"蓝色线__真实值");
	pDC->TextOut(25,100,"绿色线__观测值");
	pDC->TextOut(25,150,"红色线__滤波值");
    for(i=0;i<10;i++)
	{
	   singer.Filter();
	   singer.CalError();
	   

	
    
       e_xx+=singer.exx;
	   e_yy+=singer.eyy;
	   d_xx+=singer.dxx;
	   d_yy+=singer.dyy;
	}
	e_xx=e_xx/10.0;
	e_yy=e_yy/10.0;
	d_xx=d_xx/10.0;
	d_yy=d_yy/10.0;
	CString str5,str2,str3,str4;
	str5.Format("%f",e_xx);
	str2.Format("%f",e_yy);
	str3.Format("%f",d_xx);
	str4.Format("%f",d_yy);
	pDC->TextOut(25,240,"X轴误差均值为:");
	pDC->TextOut(40,260,str5);
	pDC->TextOut(25,280,"Y轴误差均值为:");
	pDC->TextOut(40,300,str2);
	pDC->TextOut(25,320,"X轴误差均方值为:");
	pDC->TextOut(40,340,str3);
	pDC->TextOut(25,360,"Y轴误差均方值为:");
	pDC->TextOut(40,380,str4);







	
    //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->SetPixel((int)(x0+singer.XY_Obsv[i][0]*xscale),(int)(y0-singer.XY_Obsv[i][1]*yscale)+30,RGB(0,255,0));
		pDC->SetPixel((int)(x0+singer.XY_Obsv[i][0]*xscale),(int)(y0-singer.XY_Obsv[i][1]*yscale+1+30),RGB(0,255,0));
		pDC->SetPixel((int)(x0+singer.XY_Obsv[i][0]*xscale+1),(int)(y0-singer.XY_Obsv[i][1]*yscale+30),RGB(0,255,0));
		pDC->SetPixel((int)(x0+singer.XY_Obsv[i][0]*xscale+1),(int)(y0-singer.XY_Obsv[i][1]*yscale+1+30),RGB(0,255,0));
	}

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

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

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

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

}

/////////////////////////////////////////////////////////////////////////////
// CKalmanView printing

BOOL CKalmanView::OnPreparePrinting(CPrintInfo* pInfo)
{
	// default preparation
	return DoPreparePrinting(pInfo);
}

void CKalmanView::OnBeginPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add extra initialization before printing
}

void CKalmanView::OnEndPrinting(CDC* /*pDC*/, CPrintInfo* /*pInfo*/)
{
	// TODO: add cleanup after printing
}

/////////////////////////////////////////////////////////////////////////////
// CKalmanView diagnostics

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

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

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

/////////////////////////////////////////////////////////////////////////////
// CKalmanView message handlers