斜抛轨迹预估算法程序

本程序是用来对带有空气阻力物体在空中飞行时的轨迹进行预估的采用了最小二乘法进行轨迹拟合，效果误差可以限制在厘米级

#include“Kalman_Filter.h“
#include 
#include 

Kalman_Filter::Kalman_Filter（）
	{
		
	}
Kalman_Filter::~Kalman_Filter（）
	{
		
	}
void Kalman_Filter::OutputMatrix（double a[]int mint n）
	{
		printf（“\nThe Matrix is:\n“）;
		for（int i = 0;i			{
				printf（“\n“）;
				for（int j = 0;j					printf（“%f\t“a[i*n+j]）;
			}
			printf（“\n“）;

	}
void Kalman_Filter::MatrixDotMultiple（double a[]double bint mint ndouble c[]）
	{
		for（int i = 0;i			for（int j = 0;j				c[i*n+j] = a[i*n+j]*b;
	}
void Kalman_Filter::MatrixEvaluate（double a[]double b[]int mint n）//把矩阵a的值赋给b
	{
		for（int i = 0;i			for（int j = 0;j				b[i*n+j] = a[i*n+j];
	}
void Kalman_Filter::MatrixAdd（double a[]double b[]int mint ndouble c[]）
	{
		for（int i = 0;i			for（int j = 0;j				c[i*n+j] = a[i*n+j]+b[i*n+j];
	}
void Kalman_Filter::MatrixTransepose（double a[]int mint ndouble b[]）					
				{
				for （int i=0;i					{
						for （int j=0;j						{
							b[j*m+i]=a[i*n+j];
						}
					}
				}
void Kalman_Filter::MatrixMultiple（double a[]double b[]int  mint nint kdouble c[]）
	
{ 
	int ijlu;
	for （i=0; i<=m-1; i++）
		for （j=0; j<=k-1; j++）
		{ 
			u=i*k+j; c[u]=0.0;
			for （l=0; l<=n-1; l++）
				c[u]=c[u]+a[i*n+l]*b[l*k+j];
		}
		return;
}
int Kalman_Filter::MatrixInv（double a[]int n） 
							  
{
	int *is*jsijkluv;
	double dp;
	is=new int[n*sizeof（int）];
	js=new int[n*sizeof（int）];
	//is=（int *）malloc（n*sizeof（int））;
	//js=（int *）malloc（n*sizeof（int））;
	for （k=0; k<=n-1; k++）
	{
		d=0.0;
		for （i=k; i<=n-1; i++）
			for （j=k; j<=n-1; j++）
			{ 
				l=i*n+j; p=fabs（a[l]）;
				if （p>d） 
				{ 
					d=p; is[k]=i; js[k]=j;
				}
			}
			if （d+1.0==1.0）
			{
				delete is; 
				delete js; 
				printf（“err**not inv\n“）;
				return（0）;
			}
			if （is[k]!=k）
				for （j=0; j<=n-1; 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-1; 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]=1.0/a[l];
					for （j=0; j<=n-1; j++）
						if （j!=k）
						{ 
							u=k*n+j; a[u]=a[u]*a[l];
						}
						for （i=0; i<=n-1; i++）
							if （i!=k）
								for （j=0; j<=n-1; 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-1; i++）
										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-1; 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-1; i++）
				{ 
					u=i*n+k; v=i*n+is[k];
					p=a[u]; a[u]=a[v]; a[v]=p;
				}
	}
	free（is）; 
	free（js）;
	return（1）;
}
int Kalman_Filter::Kalman_Algorithm_X（double p0double period_from_l

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      69632  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Debug\Track Prediction.exe

     文件     473728  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Debug\Track Prediction.ilk

     文件     887808  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Debug\Track Prediction.pdb

     文件       8764  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\BuildLog.htm

     文件      60164  2009-07-23 09:26  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Kalman_Filter.obj

     文件         69  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\mt.dep

     文件        663  2009-06-15 19:17  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Track Prediction.exe.embed.manifest

     文件        728  2009-06-15 19:17  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Track Prediction.exe.embed.manifest.res

     文件        621  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Track Prediction.exe.intermediate.manifest

     文件      75321  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Track Prediction.obj

     文件      44529  2009-07-19 17:33  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\Track.obj

     文件     240640  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\vc90.idb

     文件     217088  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug\vc90.pdb

     文件      15702  2009-07-23 09:25  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Kalman_Filter.cpp

     文件       1549  2009-06-15 16:34  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Kalman_Filter.h

     文件       1356  2009-04-27 17:21  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\ReadMe.txt

     文件        303  2009-04-27 17:21  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\stdafx.cpp

     文件        320  2009-04-27 17:21  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\stdafx.h

     文件        765  2009-04-27 17:21  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\targetver.h

     文件      16856  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track Prediction.cpp

     文件      20163  2009-07-19 17:31  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track Prediction.h

     文件       4289  2009-06-03 00:02  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track Prediction.vcproj

     文件       1427  2009-07-29 17:36  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track Prediction.vcproj.4C3A2BE6FCE647A.zhaoliang.user

     文件       5598  2009-07-19 17:33  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track.cpp

     文件        767  2009-06-15 19:16  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Track.h

     文件    2296832  2009-07-29 17:36  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction.ncb

     文件        914  2009-04-27 17:59  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction.sln

    ..A..H.     38912  2009-07-29 17:36  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction.suo

     目录          0  2009-07-23 09:57  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Track Prediction\Debug

     目录          0  2009-07-23 09:52  Track Predictingbased On Class\Track Prediction based on unstable period\Track Prediction\Debug

............此处省略8个文件信息