#pragma?once
#include?“StdAfx.h“
#include?“EKF.h“
//#include?“SettingDlg.h“
#include?
//#include?“RealTrack.h“


EKF::EKF（void）
{
	//memset（X_EKF?0?sizeof（X_EKF））;
}


EKF::~EKF（void）
{
}

void?EKF::EKFDelete（CPtrArray?&X）
{
	#pragma?omp?parallel?for
	for?（int?i?=?0;?i?	{
		delete?（StoreData?*）X.GetAt（i）;
	}
	X.RemoveAll（）;
}

/*EKF::EKF（double?x1?double?x2?double?x3?double?x4）
{
	memset（X_EKF?0?sizeof（X_EKF））;
	this->X_EKF[0][0]?=?x1;
	this->X_EKF[1][0]?=?x2;
	this->X_EKF[2][0]?=?x3;
	this->X_EKF[3][0]?=?x4;
}*/

void?EKF::EKFfiltering（double?*z?UINT?m_length）
{
	//CMatrix?fai（4?4）;
????//fai（1?1）?=?fai（1?2）?=?fai（2?2）?=?fai（3?3）?=?fai（3?4）?=?fai（4?4）?=?1;

????//CMatrix?gama（4?2）;
????//gama（1?1）?=?gama（3?2）?=?0.5;
????//gama（2?1）?=?gama（4?2）?=?1;

????CMatrix?Pplus（4?4）;
	Pplus?=?glb.Pplus;

????CMatrix?xhat（4?1）;
	xhat?=?glb.xhat;
	
	StoreData?*ekf?=?new?StoreData（xhat（1?1）?xhat（2?1）?xhat（3?1）?xhat（4?1））;
	X.Add（ekf）;

	//CMatrix?I（4?4）;
	//I（1?1）?=?I（2?2）?=?I（3?3）?=?I（4?4）?=?1;

	//CMatrix?Im（2?2）;
	//Im（1?1）?=?Im（2?2）?=?Qn;

	CMatrix?H（1?dim）;
	CMatrix?K（dim?1）;
	//CMatrix?tem（1?1）;
	//CMatrix?CMte（dim?1）;
	CMatrix?Znum（1?1）;

	int?num?=?1;//第0個未知存放的是占位符0，并不是真正的測量值，因此應當從下標1開始取測量值
	for?（UINT?count?=?TimeInterval;?count?<=?m_length;?count?+=?TimeInterval）
	{
		Pplus?=?glb.fai?*?Pplus?*?（~glb.fai）?+?glb.gama?*?glb.Im?*?（~glb.gama）;

		H（1?1）?=?-xhat（3?1）?/?（（1?+?pow（xhat（3?1）?/?xhat（1?1）?2））?*?pow（xhat（1?1）?2））;
		H（1?2）?=?H（1?4）?=?0;
		H（1?3）?=?1?/?（（1?+?pow（xhat（3?1）?/?xhat（1?1）?2））?*?xhat（1?1））;

		//CMatrix?tem（1?1）;
		Znum（1?1）?=?Rn;//For?saving?memory
		Znum?=?H?*?Pplus?*?（~H）?+?Znum;
?		K?=?（（double）1?/?Znum（1?1））?*Pplus?*?（~H）;

		xhat?=?glb.fai?*?xhat;
		
		xhat?=?xhat?+?（z[num]?-?atan（xhat（3?1）?/?xhat（1?1）））?*?K;

		Pplus?=?（glb.I?-?K?*?H）?*?Pplus;
	
		++num;

		StoreData?*ekf?=?new?StoreData（xhat（1?1）?xhat（2?1）?xhat（3?1）?xhat（4?1））;
		X.Add（ekf）;
	}
}

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件????????924??2013-06-06?19:07??Nonlinear_Filtering.sln

?????文件???15469286??2014-06-21?19:47??Debug\help.pdf

?????文件????9637888??2015-02-13?22:13??Debug\Nonlinear_Filtering.exe

?????文件???????2119??2013-06-14?21:52??Nonlinear_Filtering\EKF.cpp

?????文件????????440??2013-06-14?15:07??Nonlinear_Filtering\EKF.h

?????文件???????5169??2014-06-21?19:34??Nonlinear_Filtering\FifthCKF.cpp

?????文件????????501??2013-06-14?15:12??Nonlinear_Filtering\FifthCKF.h

?????文件???????2932??2014-06-21?19:35??Nonlinear_Filtering\Globals.cpp

?????文件???????1408??2013-06-15?01:46??Nonlinear_Filtering\Globals.h

?????文件??????93055??2015-02-13?22:10??Nonlinear_Filtering\Graph.cpp

?????文件???????4964??2015-02-13?22:06??Nonlinear_Filtering\Graph.h

?????文件????????918??2015-02-13?22:10??Nonlinear_Filtering\GraphDataColor.cpp

?????文件???????1265??2015-02-13?22:06??Nonlinear_Filtering\GraphDataColor.h

?????文件????????981??2015-02-13?22:10??Nonlinear_Filtering\GraphDataSet.cpp

?????文件???????1245??2015-02-13?22:06??Nonlinear_Filtering\GraphDataSet.h

?????文件???????2404??2015-02-13?22:06??Nonlinear_Filtering\GraphLegend.cpp

?????文件???????1341??2015-02-13?22:07??Nonlinear_Filtering\GraphLegend.h

?????文件????????945??2013-06-08?12:40??Nonlinear_Filtering\GraphLegendSet.cpp

?????文件???????1301??2015-02-13?22:07??Nonlinear_Filtering\GraphLegendSet.h

?????文件???????2148??2015-02-13?22:10??Nonlinear_Filtering\GraphSeries.cpp

?????文件???????1466??2015-02-13?22:07??Nonlinear_Filtering\GraphSeries.h

?????文件???????3866??2014-06-21?19:34??Nonlinear_Filtering\ICKF.cpp

?????文件????????467??2013-06-14?15:07??Nonlinear_Filtering\ICKF.h

?????文件??????10688??2013-06-16?21:03??Nonlinear_Filtering\MainFrm.cpp

?????文件???????1237??2013-06-17?23:24??Nonlinear_Filtering\MainFrm.h

?????文件???????8587??2013-12-07?18:38??Nonlinear_Filtering\Matrix.cpp

?????文件???????2071??2013-12-07?18:39??Nonlinear_Filtering\Matrix.h

?????文件?????133952??2013-06-18?23:46??Nonlinear_Filtering\Nonlinear_Filtering.aps

?????文件???????5250??2013-06-09?09:52??Nonlinear_Filtering\Nonlinear_Filtering.cpp

?????文件????????741??2013-06-06?19:07??Nonlinear_Filtering\Nonlinear_Filtering.h

............此處省略35個文件信息