mainfrm.cpp

基于VC环境下的组合导航卡尔曼滤波仿真器设计

// MainFrm.cpp : implementation of the CMainFrame class
//

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

#include "MainFrm.h"

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

/////////////////////////////////////////////////////////////////////////////
// CMainFrame

IMPLEMENT_DYNCREATE(CMainFrame, CFrameWnd)

BEGIN_MESSAGE_MAP(CMainFrame, CFrameWnd)
	//{{AFX_MSG_MAP(CMainFrame)
	ON_WM_CREATE()
	ON_COMMAND(IDM_NOMAL, OnNomal)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

static UINT indicators[] =
{
	ID_SEPARATOR,           // status line indicator
	ID_INDICATOR_CAPS,
	ID_INDICATOR_NUM,
	ID_INDICATOR_SCRL,
};

/////////////////////////////////////////////////////////////////////////////
// CMainFrame construction/destruction

CMainFrame::CMainFrame()
{
	// TODO: add member initialization code here
	
}

CMainFrame::~CMainFrame()
{
}

int CMainFrame::OnCreate(LPCREATESTRUCT lpCreateStruct)
{
	if (CFrameWnd::OnCreate(lpCreateStruct) == -1)
		return -1;
	
	if (!m_wndToolBar.CreateEx(this, TBSTYLE_FLAT, WS_CHILD | WS_VISIBLE | CBRS_TOP
		| CBRS_GRIPPER | CBRS_TOOLTIPS | CBRS_FLYBY | CBRS_SIZE_DYNAMIC) ||
		!m_wndToolBar.LoadToolBar(IDR_MAINFRAME))
	{
		TRACE0("Failed to create toolbar\n");
		return -1;      // fail to create
	}

	if (!m_wndStatusBar.Create(this) ||
		!m_wndStatusBar.SetIndicators(indicators,
		  sizeof(indicators)/sizeof(UINT)))
	{
		TRACE0("Failed to create status bar\n");
		return -1;      // fail to create
	}

	// TODO: Delete these three lines if you don't want the toolbar to
	//  be dockable
	m_wndToolBar.EnableDocking(CBRS_ALIGN_ANY);
	EnableDocking(CBRS_ALIGN_ANY);
	DockControlBar(&m_wndToolBar);

	return 0;
}

BOOL CMainFrame::PreCreateWindow(CREATESTRUCT& cs)
{
	if( !CFrameWnd::PreCreateWindow(cs) )
		return FALSE;
	// TODO: Modify the Window class or styles here by modifying
	//  the CREATESTRUCT cs

	return TRUE;
}

/////////////////////////////////////////////////////////////////////////////
// CMainFrame diagnostics

#ifdef _DEBUG
void CMainFrame::AssertValid() const
{
	CFrameWnd::AssertValid();
}

void CMainFrame::Dump(CDumpContext& dc) const
{
	CFrameWnd::Dump(dc);
}

#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CMainFrame message handlers


void CMainFrame::OnNomal() 
{
	// TODO: Add your command handler code here
	//	float rk[4][4]={{225.0f,0,0,0},
//		            {0,256.0f,0,0},
//					{0,0,2.5e-5f,0},
//					{0,0,0,0.49f}};
//	float qk[6][6]={{-0.34924f,0.01218f,0.0116f,0,0,0},
//		            {0.01218f,0.03026f,0.03596f,0,0,0},
//					{0.0116f,0.03596f,0.07782f,0,0,0},
//					{0,0,0,-0.34924f,0.01218f,0.0116f},
//					{0,0,0,0.01218f,0.03026f,0.03596f},
//					{0,0,0,0.0116f,0.03596f,0.07782f}};
//	float ztzy[6][6]={{1,1,0.5f,0,0,0},
//		              {0,1,1,0,0,0},
//					  {0,0,1,0,0,0},
//					  {0,0,0,1,1,0.5f},
//					  {0,0,0,0,1,1},
//					  {0,0,0,0,0,1}};
//	float I[6][6]={{1,0,0,0,0,0},
//		           {0,1,0,0,0,0},
//				   {0,0,1,0,0,0},
//				   {0,0,0,1,0,0},
//				   {0,0,0,0,1,0},
//				   {0,0,0,0,0,1}};
//	int i,j,t;
//	//给系统观测噪声方差阵Rk赋值
//	for(i=0;i<4;i++)
//		for(j=0;j<4;j++)
//			Rk[i][j]=rk[i][j];
//	//给系统过程噪声方差阵Qk赋值
//	for(i=0;i<6;i++)
//		for(j=0;j<6;j++)
//			Qk[i][j]=qk[i][j];
//	//给X0赋值
//	Xkgj[0][0]=Xkgj[3][0]=0;Xkgj[1][0]=Xkgj[4][0]=10;Xkgj[2][0]=Xkgj[5][0]=0;
//	//给P0赋值
//	for(i=0;i<6;i++)//先将对角阵PK中的值全部赋零
//		for(j=0;j<6;j++)
//			Pk[i][j]=0;
//	Pk[0][0]=Pk[3][3]=100;Pk[1][1]=Pk[4][4]=1;Pk[2][2]=Pk[5][5]=0.04f;
//	//给状态转移矩阵赋值
//	for(i=0;i<6;i++)//将ztzy[i][j]的值全部赋给ZTZY[i][j]
//		for(j=0;j<6;j++)
//			ZTZY[i][j]=ztzy[i][j];
//	float OP[6][6];
//	float TZTZY[6][6];
//	ReserMat(66,&ZTZY[0][0],&TZTZY[0][0]);//求出状态转移矩阵的逆阵
//	float THk[6][4];
//	float PHT[6][4];
//	float HPHR[4][4];
//	float KH[6][6];
//	//进行Kalman滤波
//	for(t=0;t<301;t++)
//	{
//		//先将矩阵I置为单位矩阵
//		for(i=0;i<6;i++)
//			for(j=0;j<6;j++)
//			{
//				if(i==j)
//					I[i][j]=1;
//				else
//					I[i][j]=0;
//			}
//		//第一步：求出Pk/k-1
//		MultiMat(66,&ZTZY[0][0],66,&Pk[0][0],&OP[6][6]);
//		MultiMat(66,&OP[0][0],66,&TZTZY[0][0],&Pkyc[0][0]);
//		PlusMat(66,&Pkyc[0][0],66,&Qk[0][0]);//由此求得Pk/k-1
//		//第二步：求出Kk
//		MultiMat(66,&ZTZY[0][0],61,&Xkgj[0][0],&Xkyc[0][0]);//求得Xkyc
//		for(i=0;i<4;i++)//求出Hk
//			for(j=0;j<6;j++)
//				Hk[i][j]=0;
//		Hk[2][1]=(Xkyc[5][0]*Xkyc[1][0]-2*Xkyc[1][0]*Xkyc[4][0]*Xkyc[2][0]
//			      -Xkyc[5][0]*Xkyc[4][0]*Xkyc[4][0])/((Xkyc[4][0]*Xkyc[4][0]
//				  +Xkyc[1][0]*Xkyc[1][0])*(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]));//h1
//		Hk[2][2]=Xkyc[4][0]/(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]);//h2
//		Hk[2][4]=(Xkyc[2][0]*Xkyc[1][0]+2*Xkyc[1][0]*Xkyc[4][0]*Xkyc[5][0]
//			     -Xkyc[2][0]*Xkyc[4][0]*Xkyc[4][0])/((Xkyc[4][0]*Xkyc[4][0]
//				  +Xkyc[1][0]*Xkyc[1][0])*(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]));//h3
//		Hk[2][5]=-Xkyc[1][0]/(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]);//h4
//		Hk[3][1]=Xkyc[1][0]/float(sqrt(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]));//h5
//		Hk[3][4]=Xkyc[4][0]/float(sqrt(Xkyc[4][0]*Xkyc[4][0]+Xkyc[1][0]*Xkyc[1][0]));//h6
//		ReserMat(46,&Hk[0][0],&THk[0][0]);//求得Hk的转置矩阵THk
//		MultiMat(66,&Pkyc[0][0],64,&THk[0][0],&PHT[0][0]);//相乘求得PHt
//		MultiMat(46,&Hk[0][0],64,&PHT[0][0],&HPHR[0][0]);
//		PlusMat(44,&HPHR[0][0],44,&Rk[0][0]);
//		ContrayMat(44,&HPHR[0][0]);//求逆
//		MultiMat(64,&PHT[0][0],44,&HPHR[0][0],&Kk[0][0]);//求得Kk
//		//第三步：求得Pk
//		MultiMat(64,&Kk[0][0],46,&Hk[0][0],&KH[0][0]);//求得KkHk
//		SubMat(66,&I[0][0],66,&KH[0][0]);
//		MultiMat(66,&I[0][0],66,&Pkyc[0][0],&Pk[0][0]);
//		//第四步：求出Xkgj
//				
//		//求出Zk/k-1(预测值)
//		Zkyc[0][0]=Xkyc[0][0];
//		Zkyc[1][0]=Xkyc[3][0];
//		Zkyc[2][0]=(Xkyc[4][0]*Xkyc[2][0]-Xkyc[1][0]*Xkyc[5][0])/(Xkyc[1][0]*Xkyc[1][0]
//			+Xkyc[4][0]*Xkyc[4][0]);
//		Zkyc[3][0]=float(sqrt(Xkyc[1][0]*Xkyc[1][0]+Xkyc[4][0]*Xkyc[4][0]));
//	}
}