kalmanview.cpp

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

				}
			}
		if(1.0==d+1.0)
		{
			delete[]is;delete[]js;
			MessageBox("矩阵奇异!");
			return FALSE;
		}
		if(is[k]!=k)
			for(j=0;j<n;j++)//行交换
			{
				u=k*n+j;v=is[k]*n+j;
				p=a[u];a[u]=a[v];a[v]=p;
			}			
		if(js[k]!=k)//列交换
			for(i=0;i<n;i++)
			{
				u=i*n+k;v=i*n+js[k];
				p=a[u];a[u]=a[v];a[v]=p;
			}
		l=k*n+k;
		a[l]=float(1.0/a[l]);//归一化
		for(j=0;j<n;j++)//归一化计算 a[k][j]=a[k][j]a[k][k]
			if(j!=k)
			{
				u=k*n+j;a[u]=a[u]*a[l];
			}
		for(i=0;i<n;i++)//消元计算 a[i][j]=a[i][j]-a[i][k]a[k][j]
			if(i!=k)
				for(j=0;j<n;j++)
					if(j!=k)
					{
						u=i*n+j;
						a[u]=a[u]-a[i*n+k]*a[k*n+j];
					}
		for(i=0;i<n;i++)//消元计算 a[i][k]=-a[i][k]a[k][k]
			if(i!=k)
			{
				u=i*n+k;a[u]=-a[u]*a[l];
			}
	}
	for(k=n-1;k>=0;k--)//恢复
	{
		if(js[k]!=k)
			for(j=0;j<n;j++)//行交换
			{
				u=k*n+j;v=js[k]*n+j;
				p=a[u];a[u]=a[v];a[v]=p;
			}
		if(is[k]!=k)
			for(i=0;i<n;i++)//列交换
			{
				u=i*n+k;v=i*n+is[k];
				p=a[u];a[u]=a[v];a[v]=p;
			}
	}
	delete[]is;delete[]js;
	return TRUE;
}

void CKalmanView::OnGass() 
{
	// TODO: Add your command handler code here
	CClientDC dc(this);
	int n,y,num=500;
	double gas,average=0.0,dx=0.0;
	dc.MoveTo(0,50);
	dc.LineTo(500,50);
	for(n=0;n<num;n++)
	{
		gas=GassRand(0);
		average+=gas;
		y=50+int(10*gas);
		if(0==n)
			dc.MoveTo(n,y);
		else
			dc.LineTo(n,y);
	}
	average/=num;
	dc.MoveTo(0,50+int(10*average));
	dc.LineTo(500,50+int(10*average));

	for(n=0;n<num;n++)
	{
		gas=GassRand(n);
		dx+=(average-gas)*(average-gas);
	}

	CString sTemp;
	sTemp.Format("均值%lf 方差%lf",average,dx/(num-1));
	dc.TextOut(200,100,sTemp);
}

double CKalmanView::GassRand(int rr)
{
	static double r=3.0;	// 种子
	if(rr) 
		r=rr;
	int i,m;
	double s,w,v,t;
	s=65536.0; w=2053.0; v=13849.0;
	t=0.0;
	for(i=1;i<=12;i++)
	{
		r=r*w+v; m=(int)(r/s);
		r-=m*s; 
		t+=r/s;
	}
	t-=6.0;
	return t;
}

void CKalmanView::OnArea() 
{
	// TODO: Add your command handler code here
	CClientDC dc(this);
	float C,R;
	C=float(circle(2.0));
	R=float(rect(4.0,5.0));
	CString str;
	str.Format("矩形面积：%5.2f  圆面积：%5.2f",R,C);
	dc.TextOut(100,100,str);
}

void CKalmanView::OnReaddata() 
{
	// TODO: Add your command handler code here
	ifstream if1("Kalmanx3.txt");
	ifstream if2("Kalmanxe3.txt");
	double y3,ye3;
	int d3,de3,num;
	
	if1>>d3;
	if1>>num;
	if2>>de3;
	if2>>num;
	//求出最大值
	int i,max3=0,maxe3=0;
	for(i=0;i<num;i++)
	{
		if1>>y3;
		if(int(y3)>max3)
			max3=int(y3);
		if2>>ye3;
		if(int(ye3)>maxe3)
			maxe3=int(ye3);
	}
	if1.close();
	if2.close();

	ifstream if11("Kalmanx3.txt");
	ifstream if22("Kalmanxe3.txt");
	//移回到第三个数	
	if11>>d3;
	if11>>num;
	if22>>de3;
	if22>>num;

	double mul;
	mul=1.0*300/max3;
			
	CPoint old3,olde3,now3,nowe3;
	CClientDC dc(this);
	CRect rect;
	GetClientRect(&rect);

	CPen pen1,pen2,pen3;
	pen1.CreatePen(PS_SOLID,2,RGB(255,0,0));
	pen2.CreatePen(PS_SOLID,1,RGB(255,0,0));
	pen3.CreatePen(PS_DOT,1,RGB(0,0,0));
	CPen *pOldpen=dc.SelectObject(&pen1);

	dc.MoveTo(20,0);//画横轴
	dc.LineTo(20,rect.Height());
	dc.MoveTo(0,400);//画纵轴
	dc.LineTo(rect.Width(),400);

	for(i=0;i<num;i++)
	{
		if11>>y3;
		if22>>ye3;
		now3.x=d3*i+20,now3.y=400-int(mul*y3);
		nowe3.x=d3*i+20,nowe3.y=400-int(mul*ye3);
		if(i>0)
		{
			dc.SelectObject(&pen2);//选择画笔2
			dc.MoveTo(old3);
			dc.LineTo(now3);
			dc.SelectObject(&pen3);//选择画笔3
			dc.MoveTo(olde3);
			dc.LineTo(nowe3);
		}
		old3=now3;
		olde3=nowe3;
	}
	if11.close();
	if22.close();
	dc.SelectObject(pOldpen);
	pen1.DeleteObject();
	pen2.DeleteObject();
	pen3.DeleteObject();
}

void CKalmanView::OnNum() 
{
	// TODO: Add your command handler code here
	CFileDialog fileDlg(TRUE);
	fileDlg.m_ofn.lpstrTitle="我的文件打开对话框";
	fileDlg.m_ofn.lpstrFilter="Text Files(*.txt)\0*.txt\0All Files(*.*)\0*.*\0\0";
	int n=0;
	if(fileDlg.DoModal()==IDOK)
	{
		CString string,filename;
		CStdioFile file(fileDlg.GetPathName(),CFile::modeRead);
		while(file.ReadString(string))
		{
			string.Replace(","," ");
			n++;
		}
		file.Close();
		CString str;
		str.Format("仿真数据数目：%d",n);
		MessageBox(str);
	}
}

void CKalmanView::OnLine() 
{
	// TODO: Add your command handler code here
	CFileDialog fileDlg(TRUE);
	fileDlg.m_ofn.lpstrTitle="我的文件打开对话框";
	fileDlg.m_ofn.lpstrFilter="Text Files(*.txt)\0*.txt\0All Files(*.*)\0*.*\0\0";
	
	CString string,filename;
	int i,n=0;
	if(fileDlg.DoModal()==IDOK)
	{
		filename=fileDlg.GetPathName();
		CStdioFile file(filename,CFile::modeRead);
		while(file.ReadString(string))
		{
			string.Replace(","," ");
			n++;
		}
		file.Close();
	}

	ifstream if1(filename);
	//求出最大值
	double y,max=0.0;
	for(i=0;i<n;i++)
	{
		if1>>y;
		if(fabs(y)>max)
			max=fabs(y);
	}
	if1.close();
		
	double mul;
	mul=0.6*300/max;
			
	CPoint old,now;
	CClientDC dc(this);
	CRect rect;
	GetClientRect(&rect);

	CPen pen1;
	pen1.CreatePen(PS_SOLID,1,RGB(255,0,0));
	CPen *pOldpen=dc.SelectObject(&pen1);

	dc.MoveTo(20,0);//画纵轴
	dc.LineTo(20,rect.Height());
	dc.MoveTo(0,250);//画横轴
	dc.LineTo(rect.Width(),250);
	
	//画刻度
	dc.MoveTo(0,250-int(10*mul));dc.LineTo(rect.Width(),250-int(10*mul));
	dc.TextOut(rect.Width()-50,230-int(10*mul),"10m");
	dc.MoveTo(0,250-int(20*mul));dc.LineTo(rect.Width(),250-int(20*mul));
	dc.TextOut(rect.Width()-50,230-int(20*mul),"20m");
	dc.MoveTo(0,250+int(10*mul));dc.LineTo(rect.Width(),250+int(10*mul));
	dc.TextOut(rect.Width()-50,255+int(10*mul),"-10m");
	dc.MoveTo(0,250+int(20*mul));dc.LineTo(rect.Width(),250+int(20*mul));
	dc.TextOut(rect.Width()-50,255+int(20*mul),"-20m");

	ifstream if2(filename);
	for(i=0;i<n;i++)
	{
		if2>>y;
		now.x=i*2+20,now.y=250-int(mul*y);
		if(i>0)
		{
			dc.MoveTo(old);
			dc.LineTo(now);
		}
		old=now;
	}
	if2.close();
	
	dc.SelectObject(pOldpen);
	pen1.DeleteObject();
}

void CKalmanView::OnLinedata() 
{
	// TODO: Add your command handler code here
	CFileDialog fileDlg(TRUE);
	fileDlg.m_ofn.lpstrTitle="我的文件打开对话框";
	fileDlg.m_ofn.lpstrFilter="Text Files(*.txt)\0*.txt\0All Files(*.*)\0*.*\0\0";
	if(fileDlg.DoModal()==IDOK)
	{
		CString filename;
		filename=fileDlg.GetPathName();
		int n=0;
		double d1,max=0.0;
		char c1=',';
		//求最大值
		ifstream if1(filename);//根据选择路径创建打开文件
		while(c1!=';')
		{
			if1>>d1>>c1;
			if(fabs(d1)>max)
				max=fabs(d1);
			n++;
			if(n>1000000)//限定循环次数100万次，防止死循环
			{
				AfxMessageBox("错误的文件结尾！");
				return;
			}
		}
		if1.close();
		//数据连线
		c1=',';//c1重新赋值为','
		n=0;
		int enlarge;
		enlarge=int(100/max);//放大倍数
		ifstream if2(filename);//根据选择路径创建打开文件
		CClientDC dc(this);
		dc.MoveTo(0,200);dc.LineTo(2000,200);//画x轴
		CString str;
		str.Format("最大值：%e ",max);
		dc.TextOut(50,50,"向上为正");
		dc.TextOut(150,50,str);
		while(c1!=';')
		{
			if2>>d1>>c1;
			n++;
			if(1==n)
				dc.MoveTo(n,200-int(enlarge*d1));
			else
				dc.LineTo(n,200-int(enlarge*d1));
			if(n>1000000)//限定循环次数100万次，防止死循环
			{
				AfxMessageBox("错误的文件结尾！");
				return;
			}
		}
		if2.close();
	}
}

BOOL CKalmanView::DestroyWindow() 
{
	// TODO: Add your specialized code here and/or call the base class
	
	return CView::DestroyWindow();
}