通过读取原始星历文件数据推算GPS卫星位置坐标

一、GPS 广播卫星星历文件如下（其中还有北斗、伽利略等各种卫星，共78995条数据，下面是部分数据格式）：

​

二、读取数据

    网上看到的代码都是用编程语言本身的一些字符串处理函数来写的，感觉不仅很复杂，并且可读性很差吧！

    之前写爬虫程序的时候接触过正则表达式，后来发现C++也支持用正则表达式，那么用正则表达式匹配的话，很方便就能得到自己需要的数据了。正则匹配及读取数据代码如下： 

	vector prn;
	vector> vt;								// 二维动态数组
	vector vi;										// 一维动态数组
	this->inFile.open(path1, ios::in);									// 打开文件
	this->oFile.open(path2, ios::out | ios::trunc);
	if (this->inFile && this->oFile)					// 若文件打开成功则继续下面操作	
	{
		string s;												// 存放读取的每一行字符串
		smatch m;												// 存放正则匹配到的元素
		regex r("G\\d\\d.*|-?\\d.\\d{12}e[-+]\\d\\d");			// 正则匹配每个卫星单元
		regex r1("\\d{4}|\\d\\d|-?\\d\\.\\d{12}e[+-]\\d\\d");	// 正则匹配非首行
		int i = 0;
		while (getline(this->inFile, s))								// 打开文件
		{
			if (++i == 3220) break;
			if (regex_search(s, m, r))							// 匹配条件为 True 进入
				if (regex_search(s, m, regex("G\\d\\d")))		// 首行则进入
				{
					prn.push_back(s.substr(0, 3));				// 将卫星 prn 推入
					vt.push_back(vi);							// 将之前的遍历作为一个推入 vt	
					vi.clear();									// 清空vi，为下一次遍历做准备
					for (sregex_iterator it(s.begin() + 3, s.end(), r1), end_it; it != end_it; ++it)	// sregex_iterator 正则迭代作用
						vi.push_back(stold(it->str()));			// 将匹配的每一个参数转为浮点型再推入vi
				}
				else
					for (sregex_iterator it(s.begin(), s.end(), r1),
						end_it; it != end_it; ++it) vi.push_back(stold(it->str()));			// 将匹配的每一个参数转为浮点型再推入vi		
		}
		vt.push_back(vi);	// 将最后一次遍历的值推入 vt
		// 写入文档首行条目
		this->oFile << "n" << "," << "tk" << "," << "mk" << "," << "ek" << "," << "vk" << "," << "pa" << "," << "cu" << "," <<
			"cr" << "," << "ci" << "," << "uk" << "," << "rk" << "," << "ik" << "," << "xk" << "," << "yk" << "," << "dk" <<
			"," << "Xk" << "," << "Yk" << "," << "Zk" << endl;
		
	}
	else cout << "打开文件失败！请关闭 Excel 后重试！" << endl;	// 若文件打开失败则提示

三、计算坐标

    我将其封装在类 Satellites 中的 calData() 方法中, 代码如下：
 

// 计算数据函数
void Satellites::calData()
{
	this->n = sqrt(U) / pow(this->sqrtA, 3) + this->a_poor; //	计算平均角速度 n
	this->tk = this->getgpst('s') - this->toe;				//	计算归化时间 tk
	this->mk = this->m0 + (this->n * this->tk);				// 观测时间 mk
	this->ek = this->mk;									// 计算偏近角
	long double temp = 0;
	while (fabs(this->ek - temp) > 0.10e-12)
	{
		temp = this->ek;
		this->ek = this->mk + (this->e * sin(temp));
	}
	this->vk = atan((sqrt(1 - pow(this->e, 2)) * sin(this->ek) / (cos(this->ek) - this->e)));			// 计算真近角 vk
	this->pa = this->vk + this->w;																		// 计算升交距角
	this->cu = (this->Cuc * cos(2 * this->pa)) + (this->Cus * sin(2 * this->pa));						// 计算摄动改正项
	this->cr = (this->Crc * cos(2 * this->pa)) + (this->Crs * sin(2 * this->pa));	
	this->ci = (this->Cic * cos(2 * this->pa)) + (this->Cis * sin(2 * this->pa));
	this->uk = this->pa + this->cu;																		// 计算经过摄动改正的参数
	this->rk = pow(this->sqrtA, 2) * (1 - this->e * cos(this->ek)) + this->cr;
	this->ik = this->i0 + this->ci + (this->i * this->tk);
	this->xk = this->rk * cos(this->uk);																// 平面直角坐标系中的坐标
	this->yk = this->rk * sin(this->uk);
	this->dk = this->Ra0 + ((this->Ra - E) * this->tk) - (E * this->toe);								// 升交点纬度计算
	this->Xk = this->xk * cos(this->dk) - (this->yk * cos(this->ik) * sin(this->dk));					// 计算在地心固定坐标系中的直角坐标
	this->Yk = this->xk * sin(this->dk) + (this->yk * cos(this->ik) * sin(this->dk));
	this->Zk = this->yk * sin(this->ik);
}

四、全部代码（包括将计算结果写入 Excel）

1、main.cpp(主函数)

#pragma once
#include 
#include "Satellites.h"
using namespace std;

int main()
{
	string path1 = "C:\\Users\\amir\\Desktop\\GPS\\example\\brdm01001.19p";	 // GPS 卫星星历在 PC 上的路径
	string path2 = "E:\\results.csv";										// GPS 位置计算结果存储的路径
	Satellites s(path1, path2);												// 实例化类，并传入地址
	s.calData();						// 调用计算卫星位置的函数
	system("pause");
	return 0;
}

 

2、Satellites.h(自定义头文件）

#pragma once
#include 
#include 
#include 
#include 
using namespace std;

class Satellites
{
public:
	Satellites(string path, string path2);
	~Satellites();
	void calData();
	static int ydcount(int year);					// 获取每一年的天数
	static int ydcount(int year, int month);		// 获取每一个月的天数
	long double getgpst(char c);					// 计算 GPS 周或 周内秒
	void wdata();						// 输出计算结果
private:
	long double n, tk, mk, ek, vk, pa, cu, cr, ci, uk, rk, ik, xk, yk, dk, Xk, Yk, Zk, X, Y, Z;		// 平均角速度 n
	ifstream inFile;
	ofstream oFile;

	string prn;	// 卫星号
	int year, month, day, hour, min, second;	// 年，月，日，时，分，秒
	long double af0;	// 卫星钟差
	long double af1;	// 卫星钟速
	long double af2;	// 卫星钟速变率

	long double IODE;	// 数据龄期
	long double Crs;	// 轨道半径正弦调和项改正的振幅
	long double a_poor;	// 平均角速度之差
	long double m0;		// 平近点角

	long double Cuc;	// 纬度幅角的余弦调和项改正的振幅
	long double e;		// 轨道偏心率
	long double Cus;	// 纬度幅角的余弦调和项改正的振幅
	long double sqrtA;	// 轨道长半径的平方根

	long double toe;	// 轨道参数的参考时间
	long double Cic;	// 轨道倾角的余弦调和项改正的振幅
	long double Ra0;	// 升交点赤经
	long double Cis;	// 轨道倾角的正弦调和项改正的振幅

	long double i0;		// 轨道倾角
	long double Crc;	// 轨道半径的余弦调和项改正的振幅
	long double w;		// 近地点角距
	long double Ra;		// 升焦点赤经变化率

	long double i;		// 倾角变化率
	long double L2;		// L2上的码
	long double g_week;	// GPS 周数
	long double L2P;	// L2上的P码

	long double acc;	// 卫星精度
	long double state;	// 健康状态
	long double Tgd;	// 电离层时延迟差
	long double IDOC;	// 星钟的数据龄期

	long double s_time;	// 电文发送时刻
	long double f_val;	// 拟合区间
};

 

3、Satellites.cpp(类的实现)

#pragma once
#include 
#include 
#include 
#include "Satellites.h"
using namespace std;
#define U 3.986005e+14	// WGS-84 中地球引力常数
#define E 7.29211567e-5 // 地球自转速率

Satellites::Satellites(string path1, string path2)
{
	vector prn;
	vector> vt;								// 二维动态数组
	vector vi;										// 一维动态数组
	this->inFile.open(path1, ios::in);									// 打开文件
	this->oFile.open(path2, ios::out | ios::trunc);
	if (this->inFile && this->oFile)													// 若文件打开成功则继续下面操作	
	{
		string s;												// 存放读取的每一行字符串
		smatch m;												// 存放正则匹配到的元素
		regex r("G\\d\\d.*|-?\\d.\\d{12}e[-+]\\d\\d");			// 正则匹配每个卫星单元
		regex r1("\\d{4}|\\d\\d|-?\\d\\.\\d{12}e[+-]\\d\\d");	// 正则匹配非首行
		int i = 0;
		while (getline(this->inFile, s))								// 打开文件
		{
			if (++i == 3220) break;
			if (regex_search(s, m, r))							// 匹配条件为 True 进入
				if (regex_search(s, m, regex("G\\d\\d")))		// 首行则进入
				{
					prn.push_back(s.substr(0, 3));				// 将卫星 prn 推入
					vt.push_back(vi);							// 将之前的遍历作为一个推入 vt	
					vi.clear();									// 清空vi，为下一次遍历做准备
					for (sregex_iterator it(s.begin() + 3, s.end(), r1), end_it; it != end_it; ++it)	// sregex_iterator 正则迭代作用
						vi.push_back(stold(it->str()));			// 将匹配的每一个参数转为浮点型再推入vi
				}
				else
					for (sregex_iterator it(s.begin(), s.end(), r1),
						end_it; it != end_it; ++it) vi.push_back(stold(it->str()));			// 将匹配的每一个参数转为浮点型再推入vi		
		}
		vt.push_back(vi);	// 将最后一次遍历的值推入 vt
		// 写入文档首行条目
		this->oFile << "n" << "," << "tk" << "," << "mk" << "," << "ek" << "," << "vk" << "," << "pa" << "," << "cu" << "," <<
			"cr" << "," << "ci" << "," << "uk" << "," << "rk" << "," << "ik" << "," << "xk" << "," << "yk" << "," << "dk" <<
			"," << "Xk" << "," << "Yk" << "," << "Zk" << endl;
		
	}
	else cout << "打开文件失败！请关闭 Excel 后重试！" << endl;	// 若文件打开失败则提示
	//cout << prn.size() << " " << vt.size() << " " << vt[0][0]  << endl;
	for (int i = 1; i < vt.size(); i++)	// 将数据存储在变量
	{
		cout << i << endl;
		this->prn = prn[i-1];
		this->year = vt[i][0];
		this->month = vt[i][1];
		this->day = vt[i][2];
		this->hour = vt[i][3];
		this->min = vt[i][4];
		this->second = vt[i][5];
		this->af0 = vt[i][6];
		this->af1 = vt[i][7];
		this->af2 = vt[i][8];

		this->IODE = vt[i][9];
		this->Crs = vt[i][10];
		this->a_poor = vt[i][11];
		this->m0 = vt[i][12];

		this->Cuc = vt[i][13];
		this->e = vt[i][14];
		this->Cus = vt[i][15];
		this->sqrtA = vt[i][16];

		this->toe = vt[i][17];
		this->Cic = vt[i][18];
		this->Ra0 = vt[i][19];
		this->Cis = vt[i][20];

		this->i0 = vt[i][21];
		this->Crc = vt[i][22];
		this->w = vt[i][23];
		this->Ra = vt[i][24];

		this->i = vt[i][25];
		this->L2 = vt[i][26];
		this->g_week = vt[i][27];
		this->L2P = vt[i][28];

		this->acc = vt[i][29];
		this->state = vt[i][30];
		this->Tgd = vt[i][31];
		this->IDOC = vt[i][32];

		//this->s_time = vt[i][33];
		//this->f_val = vt[i][34];

		this->calData();		// 计算数据
		this->wdata();			// 存储数据

	}
	
}

// 析构函数，关闭文件
Satellites::~Satellites()
{
	this->inFile.close();		// 关闭文件
	this->oFile.close();				// 关闭文件
}

// 计算数据函数
void Satellites::calData()
{
	this->n = sqrt(U) / pow(this->sqrtA, 3) + this->a_poor; //	计算平均角速度 n
	this->tk = this->getgpst('s') - this->toe;				//	计算归化时间 tk
	this->mk = this->m0 + (this->n * this->tk);				// 观测时间 mk
	this->ek = this->mk;									// 计算偏近角
	long double temp = 0;
	while (fabs(this->ek - temp) > 0.10e-12)
	{
		temp = this->ek;
		this->ek = this->mk + (this->e * sin(temp));
	}
	this->vk = atan((sqrt(1 - pow(this->e, 2)) * sin(this->ek) / (cos(this->ek) - this->e)));			// 计算真近角 vk
	this->pa = this->vk + this->w;																		// 计算升交距角
	this->cu = (this->Cuc * cos(2 * this->pa)) + (this->Cus * sin(2 * this->pa));						// 计算摄动改正项
	this->cr = (this->Crc * cos(2 * this->pa)) + (this->Crs * sin(2 * this->pa));	
	this->ci = (this->Cic * cos(2 * this->pa)) + (this->Cis * sin(2 * this->pa));
	this->uk = this->pa + this->cu;																		// 计算经过摄动改正的参数
	this->rk = pow(this->sqrtA, 2) * (1 - this->e * cos(this->ek)) + this->cr;
	this->ik = this->i0 + this->ci + (this->i * this->tk);
	this->xk = this->rk * cos(this->uk);																// 平面直角坐标系中的坐标
	this->yk = this->rk * sin(this->uk);
	this->dk = this->Ra0 + ((this->Ra - E) * this->tk) - (E * this->toe);								// 升交点纬度计算
	this->Xk = this->xk * cos(this->dk) - (this->yk * cos(this->ik) * sin(this->dk));					// 计算在地心固定坐标系中的直角坐标
	this->Yk = this->xk * sin(this->dk) + (this->yk * cos(this->ik) * sin(this->dk));
	this->Zk = this->yk * sin(this->ik);
}


// 计算每一年的天数
int Satellites::ydcount(int year)				
{
	int count = 0;
	if (year == 1980)	count = 360;													// 若是 1980 年的话，按照算法 360 天
	else if ((year % 4 == 0) && (year % 100 != 0) || (year % 400 == 0))	count = 366;	// 若是闰年，则 1 年有 366 天
	else	count = 365;																// 若平闰年，则 1 年有 365 天
	return count;
}   

// 计算一年中每个月的天数
int Satellites::ydcount(int year, int month)	
{
	int count = 0;																			// 存储每个月的天数
	if (year == 1980 && month == 1) count = 25;												// 根据算法，1980 年 1 月算作 25 天
	else if (month == 4 || month == 6 || month == 9 || month == 11) count = 30;				// 四六九冬 30 整
	else if (month == 1 || month == 3 || month == 5 || month == 7 || month == 8 || month == 10 || month == 12) count = 31;
	else
	{
		if ((year % 4 == 0) && (year % 100 != 0) || (year % 400 == 0))	count = 29;			// 若是闰年，则 2 月有 29 天
		else count = 28;
	}	
	return count;
}

// c = 'w' 则返回 gps 周， c = 's' 则返回 gpa 周内秒，默认返回周内秒
long double Satellites::getgpst(char c = 's')				
{
	if (c != 'w' && c != 's') return -1;											// 若传递的参数不正确，则返回-1
	int days = this->day;
	for (int i = 1980; i < this->year; i++)	days += ydcount(i);						// 计算年天数
	for (int i = 1; i < this->month; i++)	days += ydcount(this->year, i);			// 计算月天数
	return c == 'w' ? days / 7 : (days % 7) * 86400 + (this->hour * 3600) + (this->min * 60) + this->second;
}

// 将数据写入文件
void Satellites::wdata()
{
	cout << this->prn << endl;
	this->oFile << this->prn << this->n << "," << this->tk << "," << this->mk << "," << this->ek << ","
		<< this->vk << "," << this->pa << "," << this->cu << "," << this->cr << 
		"," << this->ci << "," << this->uk << "," << this->rk << "," << this->ik 
		<< "," << this->xk << "," << this->yk << "," << this->dk << "," << this->Xk << 
		"," << this->Yk << "," << this->Zk << endl;
}