#include
#include
#include?
#include
#include
#define?N?5??//狀態值維數?

using?namespace?std;

ifstream?infile;
//measuredValue用于存儲從輸入的飛行數據?
typedef?struct{
	double?time;//時間?
	double?x;//緯度?
	double?y;//經度?
	double?v;//地速?
	double?h;//高度?
	double?sita;//航向?	
}measuredValue;
measuredValue?mValue[2];
//kalmanInfo用于存儲狀態值信息?
typedef?struct
{
	double?filterValue[N];//最優估計?n*1
	double?kg[N][N];//卡爾曼增益?n*m
	double?A[N][N];//狀態轉移矩陣?n*n?
	double?H[N][N];//狀態映射矩陣?m*n
	double?Q[N][N];//過程噪聲協方差?n*n
	double?R[N][N];//?測量噪聲協方差??n*n
	double?P[N][N];//估計誤差協方差?n*n
}kalmanInfo;
kalmanInfo?kal;
///卡爾曼濾波算法初始化?
void?kalmanInit（）
{
	
	?//狀態值初始化
	?for（int?i?=?0;?i<5;i++）
	?{
	?	kal.filterValue[i]?=?0;
	}?
	?//狀態轉移矩陣初始化，需要更新?
	?for（int?i?=?0;i?	?{
	?	kal.A[i][i]?=?1;
	?}
	?kal.A[0][3]?=?0;?
	?kal.A[1][3]?=?0;
	?
	?//過程噪聲協方差初始化，根據實際情況給出，不需更新?
	?for（int?i?=?0;i	?{
	?	kal.Q[i][i]?=?1e-8;
	?}
	?
	?//測量噪聲協方差初始化，根據實際給出，不需要更新?
	?for（int?i?=?0;i	?{
	?	kal.R[i][i]?=?1e-8;
	?}
	?
	?//狀態映射矩陣初始化，不需要更新?
	?for（int?i?=?0;i	?{
	?	kal.H[i][i]?=?1;
	?}
	?//估計誤差協方差矩陣，任意給定，會逐漸收斂?
	??for（int?i?=?0;i	?{
	?	kal.P[i][i]?=?1;
	?}
}?


void?updataData（）
{
	mValue[0].x?=?mValue[1].x;
	mValue[0].y?=?mValue[1].y;
	mValue[0].h?=?mValue[1].h;
	mValue[0].v?=?mValue[1].v;
	mValue[0].sita?=?mValue[1].sita;
	mValue[0].time?=?mValue[1].time;
}
//getData（），獲取一組飛行數據?
//從文件“predictValue.txt”中讀取?
//（時間，緯度，經度，高度，速度，磁航向）?
void?getData（）
{
	updataData（）;
	infile?>>?mValue[1].time>>mValue[1].x>>mValue[1].y>>mValue[1].h>>mValue[1].v>>mValue[1].sita;
}

//打印每一時刻的預測信息，輸出到文件“predictValue.txt“?
void?printPredictValue（double?PV[]）
{
	ofstream?outfile;
	outfile.open（“predictValue.txt“ios::app）;
	if（!outfile）
	{
		cout<<“OutFile?Not?Open!“<	}
	outfile<	for（int?i?=?0;i?	{
		outfile<	}
	outfile<	outfile.close（）;
}
//矩陣求逆?
bool?matrixInverse（double?a2[][N]?double?b2[][N]）??
{??
	int?i?j?k;??
	double?max?temp;??
	double?t[N][N];	//臨時矩陣??
	//將A矩陣存放在臨時矩陣t[n][n]中??
	for?（i?=?0;?i?	{??
		for?（j?=?0;?j?		{??
			t[i][j]?=?a2[i][j];??
		}??
	}??
	//初始化B矩陣為單位陣??
	for?（i?=?0;?i?	{??
		for?（j?=?0;?j?		{??
			b2[i][j]?=?（i?==?j）???（double）1?:?0;??
		}??
	}??
	for?（i?=?0;?i?	{??
		//尋找主元??
		max?=?t[i][i];??
		k?=?i;??
		for?（j?=?i?+?1;?j?		{??
			if?（fabs（t[j][i]）?>?fabs（max））??
			{??
				max?=?t[j][i];??
				k?=?j;??
			}??
		}??
		//如果主元所在行不是第i行，進行行交換??
		if?（k?!=?i）??
		{??
			for?（j?=?0;?j?			{??
				temp?=?t[i][j];??
				t[i][j]?=?t[k][j];??
				t[k][j]?=?temp;??
				//B伴隨交換??
				temp?=?b2[i][j];??
				b2[i][j]?=?b2[k][j];??
				b2[k][j]?=?temp;??
			}??
		}??
		//判斷主元是否為0?若是則矩陣A不是滿秩矩陣不存在逆矩陣??
		if?（t[i][i]?

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

?????文件???????7949??2019-07-22?09:29??卡爾曼濾波算法航跡預測\kalmanFileter.cpp

?????文件????1937403??2019-07-22?09:31??卡爾曼濾波算法航跡預測\kalmanFileter.exe

?????文件??????????0??2019-07-22?09:32??卡爾曼濾波算法航跡預測\predictValue.txt

?????文件???????2070??2019-07-22?09:30??卡爾曼濾波算法航跡預測\testData.txt

?????目錄??????????0??2019-07-22?09:31??卡爾曼濾波算法航跡預測

-----------?---------??----------?-----??----

??????????????1947422????????????????????5