kalmanfilterdlg.cpp

VC.NET KalmanFilter.rar

// KalmanFilterDlg.cpp : implementation file
//

#include "stdafx.h"
#include "KalmanFilter.h"
#include "KalmanFilterDlg.h"
#include "Matrix.h"
#include "myStruct.h"
#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif



Matrix  measurement;

CKalmanFilter  myKalmanFilter(1,2,0);
/////////////////////////////////////////////////////////////////////////////
// CAboutDlg dialog used for App About

class CAboutDlg : public CDialog
{
public:
	CAboutDlg();

// Dialog Data
	//{{AFX_DATA(CAboutDlg)
	enum { IDD = IDD_ABOUTBOX };
	//}}AFX_DATA

	// ClassWizard generated virtual function overrides
	//{{AFX_VIRTUAL(CAboutDlg)
	protected:
	virtual void DoDataExchange(CDataExchange* pDX);    // DDX/DDV support
	//}}AFX_VIRTUAL

// Implementation
protected:
	//{{AFX_MSG(CAboutDlg)
	//}}AFX_MSG
	DECLARE_MESSAGE_MAP()
};

CAboutDlg::CAboutDlg() : CDialog(CAboutDlg::IDD)
{
	//{{AFX_DATA_INIT(CAboutDlg)
	//}}AFX_DATA_INIT
}

void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CAboutDlg)
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CAboutDlg, CDialog)
	//{{AFX_MSG_MAP(CAboutDlg)
		// No message handlers
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalmanFilterDlg dialog

CKalmanFilterDlg::CKalmanFilterDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CKalmanFilterDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CKalmanFilterDlg)
	//}}AFX_DATA_INIT
	// Note that LoadIcon does not require a subsequent DestroyIcon in Win32
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CKalmanFilterDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CKalmanFilterDlg)
	DDX_Control(pDX, IDC_EDIT_EST, m_editEst);
	DDX_Control(pDX, IDC_EDIT_PRE, m_editPre);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CKalmanFilterDlg, CDialog)
	//{{AFX_MSG_MAP(CKalmanFilterDlg)
	ON_WM_SYSCOMMAND()
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_BN_CLICKED(IDPREDICT, OnPredict)
	ON_BN_CLICKED(IDFILTER, OnFilter)
	ON_BN_CLICKED(IDC_INIT, OnInit)
	ON_BN_CLICKED(IDC_CLOSE, OnClose)
	ON_EN_CHANGE(IDC_EDIT_PRE, OnChangeEditPre)
	ON_EN_CHANGE(IDC_EDIT_EST, OnChangeEditEst)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalmanFilterDlg message handlers

BOOL CKalmanFilterDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// Add "About..." menu item to system menu.

	// IDM_ABOUTBOX must be in the system command range.
	ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
	ASSERT(IDM_ABOUTBOX < 0xF000);

	CMenu* pSysMenu = GetSystemMenu(FALSE);
	if (pSysMenu != NULL)
	{
		CString strAboutMenu;
		strAboutMenu.LoadString(IDS_ABOUTBOX);
		if (!strAboutMenu.IsEmpty())
		{
			pSysMenu->AppendMenu(MF_SEPARATOR);
			pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
		}
	}

	// Set the icon for this dialog.  The framework does this automatically
	//  when the application's main window is not a dialog
	SetIcon(m_hIcon, TRUE);			// Set big icon
	SetIcon(m_hIcon, FALSE);		// Set small icon
	
	// TODO: Add extra initialization here
	
	return TRUE;  // return TRUE  unless you set the focus to a control
}

void CKalmanFilterDlg::OnSysCommand(UINT nID, LPARAM lParam)
{
	if ((nID & 0xFFF0) == IDM_ABOUTBOX)
	{
		CAboutDlg dlgAbout;
		dlgAbout.DoModal();
	}
	else
	{
		CDialog::OnSysCommand(nID, lParam);
	}
}

// If you add a minimize button to your dialog, you will need the code below
//  to draw the icon.  For MFC applications using the document/view model,
//  this is automatically done for you by the framework.

void CKalmanFilterDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this); // device context for painting

		SendMessage(WM_ICONERASEBKGND, (WPARAM) dc.GetSafeHdc(), 0);

		// Center icon in client rectangle
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// Draw the icon
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

// The system calls this to obtain the cursor to display while the user drags
//  the minimized window.
HCURSOR CKalmanFilterDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}


void CKalmanFilterDlg::OnPredict() 
{
	// TODO: Add your control notification handler code here
	myKalmanFilter.kalman.state_pre=myKalmanFilter.KalmanPredict(myKalmanFilter.kalman);
// 	CEdit *pE=(CEdit*)GetDlgItem(IDC_EDIT_PRE,CKalmanFilterDlg);
	CString sTemp;
	sTemp.Format("%f,%f",myKalmanFilter.kalman.state_pre(0,0),myKalmanFilter.kalman.state_pre(1,0));
	TRACE("state_pre:%f,%f",myKalmanFilter.kalman.state_pre(0,0),myKalmanFilter.kalman.state_pre(1,0));
	m_editPre.SetWindowText(sTemp);
}

void CKalmanFilterDlg::OnFilter() 
{
	// TODO: Add your control notification handler code here
	myKalmanFilter.kalman.state_post=myKalmanFilter.KalmanUpdate(myKalmanFilter.kalman,measurement);
	CString sTemp;
	sTemp.Format("%f,%f",myKalmanFilter.kalman.state_post(0,0),myKalmanFilter.kalman.state_post(1,0));
	m_editEst.SetWindowText(sTemp);
}

void CKalmanFilterDlg::OnInit() 
{
	Matrix transMatrix;
	transMatrix.Assign(2,2,0.0);
	Matrix measureMatrix;
	measureMatrix.Assign(1,2,0.0);
	Matrix measurementNoise;
	measurementNoise.Assign(1,1,0.0);
	Matrix processNoise;
	processNoise.Assign(2,2,0.0);
	Matrix stateInit;
	stateInit.Assign(2,1,0.0);
	Matrix processInitNoise;
	processInitNoise.Assign(2,2,0.0);
	double timeStep=1.0;
// 	int i;
	int runTime=30;
	measurement.Assign(1,3,0.0);
	measurement(0,0)=1;
	measurement(0,1)=2;
	measurement(0,2)=3;
//	char* pFileName="measure.txt";
//	CFile file1;
///*	file1->Open("measure.txt",CFile::readOnly);*/
//	if (!file1.Open("measure.txt",CFile::readOnly))
//	{
//		AfxMessageBox("Can not open file!");
//		exit(0);
//	}
//	else
//	{
//		file1.Read(measurement,10000);
// 	}

	transMatrix(0,0)=1.0;
	transMatrix(0,1)=1*timeStep;
	transMatrix(1,0)=0.0;
	transMatrix(1,1)=1.0;
	measureMatrix(0,0)=1.0;
	measureMatrix(0,1)=0.0;
	measurementNoise(0,0)=3.0;
	processNoise(0,0)=6.0;
	processNoise(0,1)=0.0;
	processNoise(1,0)=0.0;
	processNoise(1,1)=12.0;
	TRACE("measureMatrix:%f,%f",measureMatrix(0,0),measureMatrix(0,1));
	TRACE("measurement:%f,%f",measurement(0,0),measurement(0,1));
	stateInit=myKalmanFilter.GetInitState(measurement,timeStep);
	TRACE("stateInit:%f,%f",stateInit(0,0),stateInit(1,0));
	processInitNoise=myKalmanFilter.GetInitCov(measurementNoise,timeStep);
	if (!myKalmanFilter.InitKalman(myKalmanFilter.kalman,stateInit,processInitNoise,transMatrix,measureMatrix,processNoise,measurementNoise))
	{
		AfxMessageBox("初始化错误！");
		exit(0);
	}

	// TODO: Add your control notification handler code here
// 	file1.Close();
	
}

void CKalmanFilterDlg::OnClose() 
{
	// TODO: Add your control notification handler code here
	exit(0);
}

void CKalmanFilterDlg::OnChangeEditPre() 
{
	// TODO: If this is a RICHEDIT control, the control will not
	// send this notification unless you override the CDialog::OnInitDialog()
	// function and call CRichEditCtrl().SetEventMask()
	// with the ENM_CHANGE flag ORed into the mask.
	
	// TODO: Add your control notification handler code here
	
}

void CKalmanFilterDlg::OnChangeEditEst() 
{
	// TODO: If this is a RICHEDIT control, the control will not
	// send this notification unless you override the CDialog::OnInitDialog()
	// function and call CRichEditCtrl().SetEventMask()
	// with the ENM_CHANGE flag ORed into the mask.
	
	// TODO: Add your control notification handler code here
	
}