kalmanview.cpp

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

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

#include "stdafx.h"
#include "Kalman.h"
#include "math.h"
#include "KalmanDoc.h"
#include "KalmanView.h"

#include "Select.h"
#include "Observe.h"

#include "area.h"
#include "fstream.h"
#include "matrix.h"
#include "buffer.h"
#include "vfunc.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)
	ON_COMMAND(IDM_DRGPS, OnDrgps)
	ON_COMMAND(IDM_GASS, OnGass)
	ON_COMMAND(IDM_AREA, OnArea)
	ON_COMMAND(IDM_READDATA, OnReaddata)
	ON_COMMAND(IDM_NUM, OnNum)
	ON_COMMAND(IDM_LINE, OnLine)
	ON_COMMAND(IDM_LINEDATA, OnLinedata)
	//}}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

}

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
	CRect rc;
	GetClientRect(&rc);
	pDC->BitBlt(rc.left,rc.top,rc.Width(),rc.Height(),pDC,0,0,SRCCOPY);
}

/////////////////////////////////////////////////////////////////////////////
// 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



void CKalmanView::OnDrgps() 
{
	// TODO: Add your command handler code here
	CObserve dlg;
	if(dlg.DoModal()!=IDOK)
		return;
	UpdateData(TRUE);
	int kind=dlg.m_nObserve;
		
	double aa[6][6]={{1.0,1.0,0.5,0.0,0.0,0.0},  //aa为状态转移矩阵
		              {0.0,1.0,1.0,0.0,0.0,0.0},
					  {0.0,0.0,1.0,0.0,0.0,0.0},
					  {0.0,0.0,0.0,1.0,1.0,0.5},
					  {0.0,0.0,0.0,0.0,1.0,1.0},
					  {0.0,0.0,0.0,0.0,0.0,1.0}};//qq为驱动噪声协方差阵
	double qq[6][6]={{5.3832e-3,0.01218,0.0116,0.0,0.0,0.0},
		             {0.01218,0.03026,0.03596,0.0,0.0,0.0},
					 {0.0116,0.03596,0.07782,0.0,0.0,0.0},
					 {0.0,0.0,0.0,5.3832e-3,0.01218,0.0116},
					 {0.0,0.0,0.0,0.01218,0.03026,0.03596},
					 {0.0,0.0,0.0,0.0116,0.03596,0.07782}};
	matrix x(6,1);//x为状态变量
	x=0.0;//先将初始值设为全零
	x.set(0,1,10.0);//再将0行1列设为10.0
	x.set(0,4,10.0);//再将0行4列设为10.0
	matrix f(aa,6,6);//将状态转移矩阵包装到f

	matrix r;//观测噪声协方差阵
	matrix h;//观测矩阵
	switch(kind) 
	{
	case 0:   //只有GPS信号
		r=matrix(2,2);
		r=0.0;
		r.set(0,0,225.0);r.set(1,1,256.0);
		h=matrix(2,6);
		h=0.0;//先设为全零
		h.set(0,0,1.0);h.set(1,3,1.0);
		break;
	case 1:   //只有DR信号
		r=matrix(2,2);
		r=0.0;
		r.set(0,0,2.5e-5);r.set(1,1,0.49);
		h=matrix(2,6);
		h=0.0;//先设为全零
		break;
	case 2:   //GPS和DR组合定位
		r=matrix(4,4);
		r=0.0;//先将矩阵元素设为全零
		r.set(0,0,225.0);r.set(1,1,256.0);
		r.set(2,2,2.5e-5);r.set(3,3,0.49);
		h=matrix(4,6);
		h=0.0;//先设为全零
		h.set(0,0,1.0);h.set(1,3,1.0);
		break;
	default:   //GPS信号不做处理
		r=matrix(2,2);
		r=0.0;
		r.set(0,0,225.0);r.set(1,1,256.0);
		h=matrix(2,6);
		h=0.0;//先设为全零
		h.set(0,0,1.0);h.set(1,3,1.0);
		break;
	}
/*	matrix r(4,4);//观测噪声协方差阵
	matrix r(2,2);//观测噪声协方差阵
	r=0.0;//先将矩阵元素设为全零
	r.set(0,0,225.0);r.set(1,1,256.0);
	r.set(0,0,2.5e-5);r.set(1,1,0.49);*/

	matrix q(qq,6,6);//q存放驱动噪声协方差阵

/*	matrix h(2,6);//h为观测矩阵
	matrix h(4,6);//h为观测矩阵
	h=0.0;//先设为全零
	h.set(0,0,1.0);//再将0行0列设为1
	h.set(1,3,1.0);//再将0行0列设为1*/
	
	linemodel l(f,h,q,r,x);//初始化线性观测模型类变量l
	matrix p(6,6);//初始化估计误差协方差阵
	p=0.0;//设为全零
	p.set(0,0,100.0);p.set(3,3,100.0);
	p.set(1,1,1.0);p.set(4,4,1.0);
	p.set(2,2,0.04);p.set(5,5,0.04);
	kalman k(f,q,r,h,x,p);//初始化一Kalman滤波类变量k
	matrix xyc;//放状态变量的预测值
	matrix y,xe;//y放观测值，xe放Kalman滤波对状态向量的估计值
	
	ofstream of1("DRGPS.txt");
	double h1,h2,h3,h4,h5,h6;
	for(int i=0;i<300;i++)//滤波300个点
	{
		y=l.next(kind);//从l获得新的观测值到y
	//	y=l.next();
		x=l.x;//将新的真值送到x，跟预测值进行对比
//		xyc=k.next();//没有获得新信息前的状态变量估计值
		//得到估计值后，重设观测矩阵
		if(1==kind||2==kind)
		{
			h1=(xyc(5,0)*xyc(1,0)-2*xyc(1,0)*xyc(4,0)*xyc(2,0)-xyc(5,0)*xyc(4,0)*xyc(4,0))/
			    ((xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0))*(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0)));
			h2=xyc(4,0)/(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0));
			h3=(xyc(2,0)*xyc(1,0)+2*xyc(1,0)*xyc(4,0)*xyc(5,0)-xyc(2,0)*xyc(4,0)*xyc(4,0))/
				((xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0))*(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0)));
			h4=-xyc(1,0)/(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0));
			h5=xyc(1,0)/sqrt(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0));
			h6=xyc(4,0)/sqrt(xyc(4,0)*xyc(4,0)+xyc(1,0)*xyc(1,0));
		}
		
		switch(kind) 
		{
		case 1:
			h.set(0,1,h1);h.set(0,2,h2);h.set(0,4,h3);h.set(0,5,h4);
			h.set(1,1,h5);h.set(1,4,h6);
			k.seth(h);//重新设置观测矩阵
			break;
		case 2:
			h.set(2,1,h1);h.set(2,2,h2);h.set(2,4,h3);h.set(2,5,h4);
			h.set(3,1,h5);h.set(3,4,h6);
			k.seth(h);//重新设置观测矩阵
			break;
		default:
			break;
		}
		
/*		h.set(2,1,h1);h.set(2,2,h2);h.set(2,4,h3);h.set(2,5,h4);
		h.set(3,1,h5);h.set(3,4,h6);
		h.set(0,1,h1);h.set(0,2,h2);h.set(0,4,h3);h.set(0,5,h4);
		h.set(1,1,h5);h.set(1,4,h6);*/

	//	k.seth(h);//重新设置观测矩阵
	//	xe=k.next(y);
		xe=k.next(y,kind);//从k由观测值得到对x的估值xe
		if(i==299)
		{
			if(3==kind)
				of1<<x(0)-y(0)<<';';
			else
				of1<<x(0)-xe(0)<<';';
		}
		else
		{
			if(3==kind)
				of1<<x(0)-y(0)<<',';
			else
				of1<<x(0)-xe(0)<<',';
		}
	}
	of1.close();
	MessageBox("计算完毕!");
}

void CKalmanView::ReserMat(int n, float a[], float b[])//矩阵转置
{
	int row=n/10,col=n%10;
	int i,j;
	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			b[j*row+i]=a[i*col+j];
}

BOOL CKalmanView::MultiMat(int m, float a[], int n, float b[], float multi[])//矩阵相乘
{
	if((m%10)!=(n/10))
	{
		MessageBox("此两矩阵不能相乘!");
		return FALSE;
	}
		
	int row,col;
	row=m/10;col=n%10;
	
	int i,j,k;

	for(i=0;i<row*col;i++)
		multi[i]=0;//清零

	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			for(k=0;k<(m%10);k++)
				multi[i*col+j]+=a[i*(m%10)+k]*b[k*(n%10)+j];
	return TRUE;
}

BOOL CKalmanView::PlusMat(int m,float a[],int n,float b[])
{
	if(m!=n)
	{
		MessageBox("此两矩阵不能相加!");
		return FALSE;
	}
	
	int row,col;
	row=m/10;col=m%10;
	int i,j;
	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			a[i*col+j]+=b[i*col+j];
	return TRUE;
}

BOOL CKalmanView::SubMat(int m, float a[], int n, float b[])
{
	if(m!=n)
	{
		MessageBox("此两矩阵不能相减!");
		return FALSE;
	}
		
	int row,col;
	row=m/10;col=m%10;
	int i,j;
	for(i=0;i<row;i++)
		for(j=0;j<col;j++)
			a[i*col+j]-=b[i*col+j];
	return TRUE;	
}

BOOL CKalmanView::ContrayMat(int n, float a[])
{
	int *is,*js,i,j,k,l,u,v;
	float d,p;
	if(NULL==(is=new int[n]))//记忆行交换信息空间
	{
		MessageBox("内存不足!");
		return FALSE;
	}
	if(NULL==(js=new int[n]))//记忆列交换信息空间
	{
		MessageBox("内存不足!");
		return FALSE;
	}

	for(k=0;k<n;k++)
	{
		d=0.0;
		for(i=k;i<n;i++)//选主元
			for(j=k;j<n;j++)
			{
				l=i*n+j;
				p=float(fabs(a[l]));
				if(p>d)//记忆行列交换信息
				{
					d=p;is[k]=i;js[k]=j;