卡尔曼滤波算法航迹预测.rar

用C++实现了卡尔曼滤波对飞行轨迹的预测算法，并附有测试数据

#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 < N;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 < N;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 < N; i++）  
	{  
		for （j = 0; j < N; j++）  
		{  
			t[i][j] = a2[i][j];  
		}  
	}  
	//初始化B矩阵为单位阵  
	for （i = 0; i < N; i++）  
	{  
		for （j = 0; j < N; j++）  
		{  
			b2[i][j] = （i == j） ? （double）1 : 0;  
		}  
	}  
	for （i = 0; i < N; i++）  
	{  
		//寻找主元  
		max = t[i][i];  
		k = i;  
		for （j = i + 1; j < N; j++）  
		{  
			if （fabs（t[j][i]） > fabs（max））  
			{  
				max = t[j][i];  
				k = j;  
			}  
		}  
		//如果主元所在行不是第i行，进行行交换  
		if （k != i）  
		{  
			for （j = 0; j < N; 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