卫星位置解算

前言：

本章节代码均在Gitee中开源：

卫星位置计算代码https://gitee.com/Ehundred/navigation-engineering/tree/master/%E5%8D%AB%E6%98%9F%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86/GPS%E5%8D%AB%E6%98%9F%E4%BD%8D%E7%BD%AE%E8%A7%A3%E7%AE%97/Satellite_position_calculation其中涉及到时空转换的部分，在另一章节有详细讲解，如有疑惑可以先阅读上一章节：

时空坐标转换https://blog.csdn.net/qq_74260823/article/details/139271746?spm=1001.2014.3001.5501

~~因为这章是学校作业，所以稍微正经点.~~

卫星参数的读取

首先，关于导航电文的理解和构成，如果还不太了解，请移步另一篇文章：

卫星信号的构成http://t.csdnimg.cn/6yY29

导航电文会发出很多参数，最终会被接收机接收到，然后转换为人类可以看懂的具体的数字。接收机在转换之后，会把适用于我们定位的数据单独列出来，然后写入文件之中成为广播星历，其中文件的格式长这样：

其具体对应的含义是：

因为写入的格式一定是固定的，所以我们读取数据的时候，也可以利用这些固定的格式，按顺序定义一个结构体：

struct satellite_data_block
{double Serial;//序列号double year;double month;double day;double hour;double minute;double second;//年月日时分秒double a0;//卫星钟差double a1;//卫星钟偏double a2;//卫星钟偏移double AODE;//数据龄期double Crs;//轨道半径改正项double deltan;//平均角速度改正项double M0;//平近点角double Cuc;//升交点角距改正项double e;//轨道偏心率double Cus;//升交点角距改正项double sqrtA;//轨道长半轴平方根double TOE;//星历的参考时刻  周内秒double Cic;//轨道倾角的改正项double OMEGA;//升交点经度double Cis;//轨道倾角改正项double i0;//轨道倾角double Crc;//轨道半径的改正项double omega;//近地点角距double deltaomega;//升交点赤经变化double IDOT;//轨道倾角的变率//对于单点定位，以上数据足以，但是为了保持卫星数据完整，以下保持了所有的数据double CA;//CA码double GPSweek;//gps周数double L2P;//L2P码double SVA;//卫星精度double SVH;//卫星健康double TGD;//电离层延迟double IODC;//数据质量double shoot_time;//发射时间double h;//拟合区间
};

为什么序列号，年月日时分秒也要用double，在待会会讲解。

而读取的时候，因为格式是固定的，每一个完整的数据都有8行，我们可以每次就读8行，然后把这8行合并成同一个字符串，用来表示同一个数据块。而这个函数只负责读取，把读取的数据转换为可用的数据并导入在结构体中，就交给另外一个函数来完成。

void read()
{fstream ifs(_path_name, ios_base::in);if (!ifs)	exit(-1);while (!ifs.eof()){string data;for (int i = 0; i < 8; i++){//采用的是读取整个数据块的方法char buffer[82];ifs.getline(buffer, 81);data += buffer;}get_data(data);}
}

这个把字符串转换成有用的数据的方法，可以有很多实现形式，完全凭个人习惯。因为有36个数据，我们就定义一个36大小的double数组，每次读一个数据，就把数据放在double数组之中，一直到读完整个36个数据。
而读完之后，因为结构体采用的是内存连续区间，而数组也是内存连续区间，我们之间用memcpy，把数组的内存copy到结构体的内存上，就省略了大量的代码量。

void get_data(string& data)
{double init[36];int cur = 0;for (int i = 0; i < 36; i++){string tmp = get_part(data, cur);//获取一串数字的字符串init[i] = _to_double(tmp);//把获得的字符串转换成double//两个函数的具体实现看个人习惯，我的实现可以去看完整代码}satellite_data_block ins;memcpy(&ins, init, sizeof(double) * 36);_serial.insert(make_pair(ins.Serial, _data.size()));_data.push_back(ins);
}

读取之后，我们直接把他放在数组中，然后接着读取下一个。最终数组中便完整记录了所有卫星的参数，我们可以访问任意卫星的参数。
但是，这里会有一个问题：我们怎么知道数组中对应的是哪个卫星？难道第5个元素对应的一定是序号为5的卫星吗？当然不一定。我们有两个解决方案：

每次遍历数组，查看每个元素的序列号
用哈希表，把元素下标对应上卫星的序列号

第一个方法的时间复杂度每次都是O(n)，自然是比较不靠谱的，所以我们就采用了第二个方法，用哈希表的搜索降低时间复杂度。

卫星位置的解算

导航电文是两个小时更新一次的。因为这个特性，注定了导航电文无法发送即时的位置数据，所以卫星只能发送更新时刻的位置，然后根据一些参数来模拟卫星的运动，推算出卫星每一刻的位置。

用最简单的思想来看待这个问题，其实也很简单：

只不过具体实现的时候，要考虑的影响因素有很多就是了。

因为每个卫星参数对应的位置是独立的，所以对每一个卫星参数，我们都用一个类来表示，类中包含所有可能要用到的参数：

class calculate
{
private:double sqrtA;double angular_velocity;double _tk;double _angular_velocity;double _Mk;double _Ek;double _Vk;double _argument_of_latitude;double _Cuk;double _Crk;double _Cik;double _Uk;double _Rk;double _Ik;double _OMGk;
};

具体的符号会在计算的过程中讲解。

1.计算角速度

第一个在广播星历中已经给出，我们只需要计算第二个：

#define GM 3.9860050E14void sqrt_A(double sqrt_a)
{sqrtA = sqrt_a;angular_velocity = sqrt(GM) / pow(sqrtA, 3);
}

2.计算卫星运动时间

因为广播星历给出的时间Toe是每次更新的瞬间时间，而Tobs是当前的观测时间，所以Tobs-Toe，再消去卫星信号传播时间导致的误差，就是卫星运动的时间Tk。
注意：在广播星历中，给出的Toe是周内秒，而实际的计算计算的是总时间差。这里的Toe并非表示周内秒，而是GPS包含周的总时间。为了计算方便，我们可以直接把他转换成Unix时来进行加减。

double get_time(double Toe,double week,atime cur = curtime)
{//因为没有考虑信号传播时间，最终有小部分误差GPStime _bt(week, Toe);atime bt(_bt);double ret = cur.GetGtime()._time-bt.GetGtime()._time;if (ret > ROUND_SECOND/2)	ret -= ROUND_SECOND;else if (ret < -ROUND_SECOND / 2)	ret += ROUND_SECOND;//ROUND_SECOND GPS周一周的秒数_tk = ret;return ret;
}

3.对平均角速度改正

在广播星历中，会给出一个参数：Δn，这个参数就是给我们用以修正角速度的，我们直接把计算出的角速度修正上Δn便可以了：

double new_angular_velocity(double deltan)
{_angular_velocity = angular_velocity + deltan;return _angular_velocity;
}

4.计算平近点角

广播星历会给出一个参数M0，是在数据更新时刻的近点角。而此时，卫星运动了Tk时刻，我们用角速度n乘以运动时间Tk，就能得出卫星距离更新时刻转动的角度，再加上更新时刻的角度，就是当前时刻的平近点角

double mean_anomaly(double M0)
{_Mk = M0 + _tk * _angular_velocity;return _Mk;
}

5.计算偏近点角

公式： $E=M+e*sinE$

和时空转换一样，我们发现，这个公式左边有一个E，右边有一个E，怎么可以用自己来计算自己呢？这里就要用迭代的方法，取E一个初值En，然后不断求下一个自己的值En+1，一直到En+1和En相差很小很小，就代表他们收敛到了一个具体的值上，这个值就是我们要求得的值。

double Eccentric_Anomaly(double e)
{double Mk = _Mk;auto get_Ek = [e,Mk](double Ep){return Ep - (Ep - e * sin(Ep) - Mk) / (1 - e * cos(Ep));};double E0 = _Mk;double E1 = get_Ek(E0);while (abs(E1 - E0) >= 1E-14){E0 = E1;E1 = get_Ek(E0);}_Ek = E1;return _Ek;
}

6.计算真近点角

double True_anomaly(double e)
{_Vk = atan((sqrt(1 - e * e) * sin(_Ek) )/ (cos(_Ek) - e));if (abs(_Vk - _Mk) > Pi / 2)_Vk += _Vk > _Mk ? -Pi : Pi;return _Vk;
}

这里也可以直接用atan2来实现。

有一点要注意：导航电文中给出的所有数据都是弧度，而非角度，所以我们在使用sin等函数的时候，直接使用广播星历中的数据便可以了，不需要角度转弧度即*180/Pi

7.计算升交角距

double argument_of_latitude(double omega)
{_argument_of_latitude = _Vk + omega;return _argument_of_latitude;
}

8.计算二阶调和改正数

double Correction_of_uk(double cuc,double cus)
{_Cuk = cus * sin(2 * _argument_of_latitude) + cuc * cos(2 * _argument_of_latitude);return _Cuk;
}double Correction_of_rk(double crc, double crs)
{_Crk = crs * sin(2 * _argument_of_latitude) + crc * cos(2 * _argument_of_latitude);return _Crk;
}double Correction_of_ik(double cic, double cis)
{_Cik = cis * sin(2 * _argument_of_latitude) + cic * cos(2 * _argument_of_latitude);return _Cik;
}

9.计算改正后的开普勒轨道根数

double get_uk()
{_Uk = _argument_of_latitude + _Cuk;return _Uk;
}double get_rk(double e)
{_Rk = sqrtA*sqrtA * (1 - e * cos(_Ek)) + _Crk;return _Rk;
}double get_ik(double i0, double idot)
{_Ik = i0 + _Cik + idot * _tk;return _Ik;
}double get_OMGk(double OMG0,double OMGd,double toe)
{_OMGk = OMG0 + (OMGd - OMGE) * _tk - OMGE * toe;return _OMGk;
}

10.计算卫星的位置

在求出卫星在轨道平面坐标系的坐标后，把该坐标绕x轴旋转i角（轨道倾角），绕z轴旋转OmegaK角（升交点经度），得到的便是在地心地固坐标系下的坐标。这个部分是线代中的坐标系转换，具体的方程感兴趣可以自己推导

vector<double> get_xyz()
{double _xk = _Rk * cos(_Uk);double _yk = _Rk * sin(_Uk);double x = _xk * cos(_OMGk) - _yk * cos(_Ik) * sin(_OMGk);double y = _xk * sin(_OMGk) + _yk * cos(_Ik) * cos(_OMGk);double z = _yk * sin(_Ik);return { x,y,z };
}

最终，便可以得到卫星坐标的位置xyz。

代码汇总：

我们采用了一个类来完成，在传入一个卫星数据块后，便在构造函数里直接计算出该数据块对应的卫星位置。而因为读取卫星数据，计算卫星数据是分离的，所以我们可以再设计另外一个接口，将两个类的功能结合在一起，而这个实现方法可以根据个人习惯，也可以直接参考我完整的代码。

#define GM 3.9860050E14
#define OMGE 7.2921151467E-5commontime _curtime = commontime({ 2021,2,1,0,0 }, 30);//当前观测时间
atime curtime = atime(_curtime);//计算的完整过程，必须按顺序依次进行
class calculate
{
public:vector<double> _calculate(const satellite_data_block& s){sqrt_A(s.sqrtA);get_time(s.TOE, s.GPSweek);new_angular_velocity(s.deltan);mean_anomaly(s.M0);Eccentric_Anomaly(s.e);True_anomaly(s.e);argument_of_latitude(s.omega);Correction_of_uk(s.Cuc, s.Cus);Correction_of_rk(s.Crc, s.Crs);Correction_of_ik(s.Cic, s.Cis);get_uk();get_rk(s.e);get_ik(s.i0, s.IDOT);get_OMGk(s.OMEGA, s.deltaomega, s.TOE);vector<double> xyz = get_xyz();return xyz;}
private:void sqrt_A(double sqrt_a){sqrtA = sqrt_a;angular_velocity = sqrt(GM) / pow(sqrtA, 3);}double get_time(double Toe,double week,atime cur = curtime){//因为没有考虑信号传播时间，最终有小部分误差GPStime _bt(week, Toe);atime bt(_bt);double ret = cur.GetGtime()._time-bt.GetGtime()._time;if (ret > ROUND_SECOND/2)	ret -= ROUND_SECOND;else if (ret < -ROUND_SECOND / 2)	ret += ROUND_SECOND;_tk = ret;return ret;}double new_angular_velocity(double deltan){_angular_velocity = angular_velocity + deltan;return _angular_velocity;}double mean_anomaly(double M0){_Mk = M0 + _tk * _angular_velocity;return _Mk;}double Eccentric_Anomaly(double e){double Mk = _Mk;auto get_Ek = [e,Mk](double Ep){return Ep - (Ep - e * sin(Ep) - Mk) / (1 - e * cos(Ep));};double E0 = _Mk;double E1 = get_Ek(E0);while (abs(E1 - E0) >= 1E-14){E0 = E1;E1 = get_Ek(E0);}_Ek = E1;return _Ek;}double True_anomaly(double e){_Vk = atan((sqrt(1 - e * e) * sin(_Ek) )/ (cos(_Ek) - e));if (abs(_Vk - _Mk) > Pi / 2)_Vk += _Vk > _Mk ? -Pi : Pi;return _Vk;}double argument_of_latitude(double omega){_argument_of_latitude = _Vk + omega;return _argument_of_latitude;}double Correction_of_uk(double cuc,double cus){_Cuk = cus * sin(2 * _argument_of_latitude) + cuc * cos(2 * _argument_of_latitude);return _Cuk;}double Correction_of_rk(double crc, double crs){_Crk = crs * sin(2 * _argument_of_latitude) + crc * cos(2 * _argument_of_latitude);return _Crk;}double Correction_of_ik(double cic, double cis){_Cik = cis * sin(2 * _argument_of_latitude) + cic * cos(2 * _argument_of_latitude);return _Cik;}double get_uk(){_Uk = _argument_of_latitude + _Cuk;return _Uk;}double get_rk(double e){_Rk = sqrtA*sqrtA * (1 - e * cos(_Ek)) + _Crk;return _Rk;}double get_ik(double i0, double idot){_Ik = i0 + _Cik + idot * _tk;return _Ik;}double get_OMGk(double OMG0,double OMGd,double toe){_OMGk = OMG0 + (OMGd - OMGE) * _tk - OMGE * toe;return _OMGk;}vector<double> get_xyz(){double _xk = _Rk * cos(_Uk);double _yk = _Rk * sin(_Uk);double x = _xk * cos(_OMGk) - _yk * cos(_Ik) * sin(_OMGk);double y = _xk * sin(_OMGk) + _yk * cos(_Ik) * cos(_OMGk);double z = _yk * sin(_Ik);return { x,y,z };}private:double sqrtA;double angular_velocity;double _tk;double _angular_velocity;double _Mk;double _Ek;double _Vk;double _argument_of_latitude;double _Cuk;double _Crk;double _Cik;double _Uk;double _Rk;double _Ik;double _OMGk;
};