kalmandoc.cpp

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

// KalmanDoc.cpp : implementation of the CKalmanDoc class
//

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

#include "KalmanDoc.h"

#include "matrix.h"
#include "buffer.h"
#include "vfunc.h"
#include "math.h"
#include "fstream.h"

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

/////////////////////////////////////////////////////////////////////////////
// CKalmanDoc

IMPLEMENT_DYNCREATE(CKalmanDoc, CDocument)

BEGIN_MESSAGE_MAP(CKalmanDoc, CDocument)
	//{{AFX_MSG_MAP(CKalmanDoc)
	ON_COMMAND(IDM_WHITENOISE, OnWhitenoise)
	ON_COMMAND(IDM_SQUARE, OnSquare)
	ON_COMMAND(IDM_MARKOV, OnMarkov)
	ON_COMMAND(IDM_NAVIGATION, OnNavigation)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CKalmanDoc construction/destruction

CKalmanDoc::CKalmanDoc()
{
	// TODO: add one-time construction code here
	w=7.2921e-5;
	g=9.78;
	RM=6378245;
	RN=6378245;
	ua=ub=ur=0.0;
}

CKalmanDoc::~CKalmanDoc()
{

}

BOOL CKalmanDoc::OnNewDocument()
{
	if (!CDocument::OnNewDocument())
		return FALSE;

	// TODO: add reinitialization code here
	// (SDI documents will reuse this document)

	return TRUE;
}



/////////////////////////////////////////////////////////////////////////////
// CKalmanDoc serialization

void CKalmanDoc::Serialize(CArchive& ar)
{
	if (ar.IsStoring())
	{
		// TODO: add storing code here
	}
	else
	{
		// TODO: add loading code here
	}
}

/////////////////////////////////////////////////////////////////////////////
// CKalmanDoc diagnostics

#ifdef _DEBUG
void CKalmanDoc::AssertValid() const
{
	CDocument::AssertValid();
}

void CKalmanDoc::Dump(CDumpContext& dc) const
{
	CDocument::Dump(dc);
}
#endif //_DEBUG

/////////////////////////////////////////////////////////////////////////////
// CKalmanDoc commands

void CKalmanDoc::OnWhitenoise() 
{
	// TODO: Add your command handler code here
	ofstream of1("C:\\Documents and Settings\\fanghb\\桌面\\noise1.txt");
	ofstream of2("C:\\Documents and Settings\\fanghb\\桌面\\noise2.txt");
	static double aa[2][2]={{2.0,0.0},{0.0,2.0}};
	matrix r(aa,2,2);
	matrix rr(2);
	gassvector x(r);
	int n=50000;
	for(int i=0;i<n;i++)
	{
		rr=x();
		if(i==n-1)
		{
			of1<<rr(0)<<';';
			of2<<rr(1)<<';';
		}
		else
		{
			of1<<rr(0)<<',';
			of2<<rr(1)<<',';
		}
	}
	of1.close();
	of2.close();
	AfxMessageBox("保存完毕！");
}

void CKalmanDoc::OnSquare() 
{
	// 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,avr=0.0,dx=0.0,Rms=0.0;
		char c1=',';
		//求平均值
		ifstream if1(filename);//根据选择路径创建打开文件
		while(c1!=';')
		{
			if1>>d1>>c1;
			avr+=d1;//求和
			n++;
			if(n>1000000)//限定循环次数100万次，防止死循环
			{
				AfxMessageBox("错误的文件结尾！");
				return;
			}
		}
		avr/=n;//求平均值
		if1.close();
		c1=',';//给c1重新赋值
        //求方差
		ifstream if2(filename);//根据选择路径创建打开文件
		n=0;
		while(c1!=';')
		{
			if2>>d1>>c1;
			dx+=pow((d1-avr),2.0);
			n++;
			if(n>1000000)//限定循环次数100万次，防止死循环
			{
				AfxMessageBox("错误的文件结尾！");
				return;
			}
		}
		dx/=n;//求方差
		Rms=sqrt(dx);//求标准差
		if2.close();

		CString str;
		str.Format("平均值：%e,方差：%e,标准差：%e",avr,dx,Rms);
		AfxMessageBox(str);
	}
}

void CKalmanDoc::OnMarkov() 
{
	// TODO: Add your command handler code here
	ofstream of1("C:\\Documents and Settings\\fanghb\\桌面\\markov.txt");
	double sgt=1.0;//相关时间设为1小时
	double T=1.0;//隔1秒产生一个随机数
	double R=0.01*0.01;//随机漂移均方差为0.01°/h。
	double q=2.0*R*T/(sgt*3600);
	double a=1-T/(sgt*3600);
//	double q=R*(1-pow(2.71828,-2*T/(sgt*3600)));
//	double a=pow(2.71828,-T/(sgt*3600));
	static double aa[3][3]={{q,0.0,0.0},{0.0,q,0.0},{0.0,0.0,q}};
//	static double aa[1]={q};
	matrix r(aa,3,3);                        
	gassvector x(r);//产生指定方差的高斯白噪声
	matrix rr(3);
	rr.set(0,0.0);rr.set(1,0.0);rr.set(2,0.0);//设初值
	
	int n=20000;//产生1万个markov随机数，隔一秒产生一个数。
	double y=0.01;//设markov过程初值为0
	double dy=0.0;
	for(int i=0;i<n;i++)
	{
	//	rr=x();
	//	y=a*y+rr(0);//数，标量
		rr=a*rr+x();
//		rr=x();
//		dy=-2.78e-4*y+2.357e-4*rr(0);
//		y+=0.1*dy;//步长为0.01
	//	if((0==i%10))//0.1秒存一个数
		{
			if(i==n-1)
	//		if(i==10*((n-1)/10))
	        //  of1<<y<<';';
				of1<<rr(0)<<';';
			else
			//	of1<<y<<',';
				of1<<rr(0)<<',';
		}
	}
	of1.close();
	AfxMessageBox("保存完毕！");
}

void CKalmanDoc::OnNavigation() 
{
	// TODO: Add your command handler code here
	doadjust=0;//计算中不使用拉近技术
	//保存仿真数据
	ofstream of1("平台误差角α.txt");ofstream of2("平台误差角β.txt");
	ofstream of3("平台误差角γ.txt");
	ofstream of4("纬度误差φ.txt");ofstream of5("经度误差λ.txt");
	ofstream of6("速度误差δVx.txt");ofstream of7("速度误差δVy.txt");
	ofstream of8("陀螺随机漂移.txt");
//	ofstream of9("matrix.txt");
	//设定初始值
	A=B=1/(60*57.3);R=3/(60*57.3);//平台初始误差角
//	ax=ay=0.0;//加速度计零位误差
	gcx=gcy=gcz=0.0;//陀螺常值漂移，设为0
	ax=ay=1.0e-4*g;//加速度计零位误差
//	gcx=gcy=gcz=0.01/(57.3*3600);//陀螺常值漂移
	Vcx=Vcy=1.0;//计算机中装定的速度
	Vx=Vy=0.0;//真实速度
	wd=30.1/57.3;jd=120.1/57.3;//计算机中装定的经纬度
	zswd=30.0/57.3,zsjd=120.0/57.3;//真实经纬度
	grx=gry=grz=0.01/(57.3*3600);//t0时刻x,y,z轴随机漂移真实值为0.01°/h
	
	int j;
	double T,h,sjt,Filcycle,a[4],k[4];
	T=0.5;//仿真时间，单位：小时
	Filcycle=10.0;//卡尔曼滤波周期，单位：秒(定为10秒)
	h=0.1;//仿真步长0.1秒
	sjt=2.0;//产生随机噪声的时间间隔2秒,2.0内随机噪声数值相同
	int sjstepnum=int(sjt/h);//一个随机数变化周期含有的步长数
	int savestep=int(T*3600/(1000*h));//保存仿真数据时间含有的步长数
	int Filstep=int(Filcycle/h);//一个卡尔曼滤波周期含有的步长数
	double sgt=2.0;//相关时间设为2小时
	double fsgt=1.0/(sgt*3600);//反相关时间常数
	double Rfc=0.01*0.01/(57.3*3600*57.3*3600);//随机漂移均方差为0.01°/h。
//	double Rfc=0.01*0.01;//随机漂移方差为(0.01°/h)2。
	double q=2.0*fsgt*Rfc*sjt;//激励白噪声的方差，单位：弧度/秒
	double axs=1-fsgt*sjt;//相关系数
	static double aa[3][3]={{q,0.0,0.0},{0.0,q,0.0},{0.0,0.0,q}};//随机漂移0.01°/h
	matrix r(aa,3,3);//协方差阵                             
	gassvector gyronoise(r);//产生陀螺噪声
	matrix rr(3);//列向量
	rr.set(0,grx);rr.set(1,gry);rr.set(2,grz);//马尔可夫随机漂移t0时刻初始值(真实值)
	
	//对卡尔曼滤波初值进行设定
	matrix I(unit(10));	//10阶单位阵
	matrix x(10);//状态变量矩阵类
	x=0.0;//设置估计初值x0为0.0
	matrix f(10,10);//10阶状态转移矩阵
	f=I;
	matrix Qt(unit(3));//连续系统噪声方差阵
	Qt*=q/sjt;//单位：(弧度)2/秒3
	matrix G(10,3);
	G=0.0;
	G.set(7,0,1.0);G.set(8,1,1.0);G.set(9,2,1.0);
	matrix GQGt(10,10);