导航时间与坐标转换

前言：

该章节代码均在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/%E5%AF%BC%E8%88%AA%E6%97%B6%E7%A9%BA%E8%BD%AC%E6%8D%A2/time-space_transform

时间

在时间系统中，一般分为三种时间：

通用时，便是我们日常生活中使用的时间
GPS时，便是GPS卫星时间钟产生的时间
unix时，在unix系统中，时间戳开始的初始时间到现在的时间戳总数

通用时

通用时(common time)，就是我们日常生活中使用的时间。日常生活中的时间包含什么？

年月日时分秒

所以通用时的定义也是如此：

struct commontime
{
    commontime()
    {}

    commontime(vector<unsigned> data, double second)
        :_year(data[0]),
        _month(data[1]),
        _day(data[2]),
        _hour(data[3]),
        _minute(data[4]),
        _second(second)
    {}

    unsigned _year;
    unsigned _month;
    unsigned _day;
    unsigned _hour;
    unsigned _minute;
    double _second;
};

GPS时

GPS时相对来说就比较陌生了。我们来回忆一下，日月年是怎么定义的？

日，地球自转一周，叫做一天
月，月亮绕地球转一周，叫做一月
年，地球绕太阳公转一周，叫做一年

我们使用的一周是多久？7天。但是，日月年我们都知道，却从未想过为何会有周这个概念。
其实，这里的周便是GPS周，GPS一周为7天，当七天一过，计数器就从0开始计数，
就如同我们每一分钟是60秒一样，60秒再过一秒，就计数成了一分一秒而非61秒；
GPS周到了604800秒（7天*24小时*60分钟*60秒）后再过一秒，就成了一周一秒。

所以，GPS周由两部分构成：

周数
一周内的秒数（范围0~604800)

但是，和通用时也有所不同，通用时的计数是从公元0年0时0分0秒开始的；GPS时却是从1980年0月0日0时0分0秒开始计时的，也就是在1980年0月0日0时0分0秒的时候，此时卫星上的计数器为：

struct GPStime
{
    GPStime()
    {}

    GPStime(unsigned week,double second)
        :_week(week),
        _second(second)
    {}

    unsigned _week;//周数
    double _second;//一周内的秒数，叫做周内秒
};

Unix时

经常深究游戏机制的人都知道，有个概念叫时间刻
啥意思？

计算机里的所有随机都是伪随机。通过给计算机一个种子，计算机就会根据这个种子，用固定的算法生成一个序列，这个序列就是伪随机序列。打个比方：

伪随机数列是一个数组：

对每一个数组中的元素，都有一个固定的算法：

比如：第一空的算法为 $f(x)=x^2+5x$

我们在给定种子为x=2的时候，第一个空的数据就是f(2)=14

同理，一直填满所有的空

而当我们想取随机数的时候，就按顺序依次从这个数组里取，

比如我们在玩掷硬币，假设为奇数是正面，偶数是反面

那么第一次掷硬币，我们取第一个元素，14，为反面

第二次掷硬币，我们取第二个元素，g(2)

过了很长时间，我们再掷第三次硬币，只要我们不重新设置种子，还是从上一次的位置开始往下取，即取第三个元素h(2)

但是，我们很容易发现，每个种子，因为算法是固定的，2对应的f(2),g(2),h(2)永远不变，所以只要种子相同，最后产生的伪随机序列也是相同的。而这就不叫伪随机了，这就变成了可预测的随机，所以为了满足伪随机的要求，我们必须要找到一个永远在变化的数来当种子。

对，就是时间，我们把时间设成种子。但是，时间不管是年月日还是几周几秒都不是一个整数，把时间变成整数最简单的方法，就是过一秒，数+1，不去向周或者分钟小时进位，最后会得到一个非常大的整数，这个整数，就叫时间刻。

所以unix时是什么？就是从1970年0月0日0时0分0秒开始，计数器为0，过一秒加1，一直加到现在的总秒数。

因此，unix时只由秒数构成。但是，为了让他更细一点，为他加入了一秒内的毫秒数
不过，在Linux中，时间刻的类型就是using time_t = long,仅此而已。

struct gtime_t
{
    gtime_t()
    {}

    gtime_t(time_t time,double second)
        :_time(time),
        _second(second)
    {}

    time_t _time;
    double _second;
};

三种时间的相互转换

首先，我们必须要弄清楚，每个时间转换成其他时间的具体函数是什么吗？
当然不必，因为，完全可以用一个时间作为中间变量，将所有时间无论类型都转换成这个中间变量，然后向其他转换。

打个比方，正常情况下，写出每个时间转换成其他时间的具体函数，需要2x3=6个函数。
但是，如果我们知道通用时转换成unix时的函数，又知道unix时转换为GPS时的函数，那么，通用时转换为GPS时就可以先转换unix时，再通过unix时转换成GPS时，这是典型的函数复用。

gtime_t ComToUnix(const commontime& com);//已经实现
GPStime UnixToGPS(const gtime_t& gt)//已经实现

GPStime ComToGPS(const commontime& com)
{
    gtime_t tmp = ComToUnix(com);
    GPStime ret = UnixToGPS(tmp);
    return ret;
}//直接函数复用

在这里，我们把Unix时设为这个中间变量。

其次，还要明晰一个概念：三种时间关系的对应是绝对一对一固定的。
啥意思？

比如我现在是1980年0月0日0时0分0秒，这是通用时。此时GPS时是0周0秒，Unix时是315964800；
而此时时间过了一秒，通用时是1980年0月0日0时0分1秒，GPS时是0周1秒，Unix时是315964801；
之后，再不管过多长时间，通用时，GPS时，Unix时所加的时间是相同的。用专业的话说，三种时间的映射是唯一的，用人话说，只要告诉了我一个时间，其他两个时间一定是固定的，所以只要有一个时间，就能把另外两个时间求出来。

所以，我们不妨定义一个类，把所有的时间类型都包含进来

class atime //all time
{
    commontime _com;
    GPStime _gps;
    gtime_t _gt;
}

所以，构造函数可以写三个，当我们传入一种时间的时候，另外两个时间在构造函数中可以直接构造出来：

atime()
{}

atime(commontime& com)
    :_com(com)
{
    _gt = ComToUnix(_com);
    _gps = UnixToGPS(_gt);
}

atime(GPStime& gps)
    :_gps(gps)
{
    _gt = GPSToUnix(_gps);
    _com = UnixToCom(_gt);
}

atime(gtime_t& gt)
    :_gt(gt)
{
    _com = UnixToCom(_gt);
    _gps = UnixToGPS(_gt);
}

~~最后，通用时时间日期的加减就不多说了，直接从网上copy一份date的类吧，在我的gitee里也有~~

over，准备工作就做好了，那具体的时间转换怎么办？

把所有的时间转换为Unix时

通用时转换为unix时

unix时是从1970年0月0日0时0分0秒到现在的时间，那么首先肯定要算出，现在到unix时的起点有多少天
算出多少天之后，用天数乘以一天的秒数，就是日期转换为秒数
$time1=day*86400$
然后再来看一天以内的时分秒。时分秒的转换就是常识了：
$time2=hour*3600+minute*60+second$
两者相加，就是unix时的时间刻
$gtime=time1+time2$

#define DAY_SECOND 86400//一天的秒数

gtime_t ComToUnix(const commontime& com)
    {
        gtime_t ret;
        int day = Date(com._year, com._month, com._day) - Date(1970, 1, 1);
        ret._time = day * DAY_SECOND + com._hour * 3600 + com._minute * 60 + (time_t)floor(com._second);
        ret._second = com._second - floor(com._second);
        return ret;
    }

GPS时转换为unix时

unix时和GPS时起点相差的秒数，为1980年与1970年相差的秒数，可以让计算机算，但是因为时间是固定的，不如直接定义出来：

#define UG_GAP 315964800//unix时和GPS时起点相差的秒数

然后，和通用时转换为unix时是一样的。

首先，算出周数，然后周数乘以7，就是相差的天数。就对应了通用时里的time1
然后，算出一天以内的秒数，就对应了通用时里的time2
最后，两者相加，就是gtime 的时间刻

gtime_t GPSToUnix(const GPStime& gps)
    {
        gtime_t ret;
        ret._time = gps._week * 7 * DAY_SECOND + (time_t)gps._second + UG_GAP;
        ret._second = gps._second - (int)gps._second;

        return ret;
    }

把unix时转换为其他时间

unix时转换为通用时

这便是一个逆过程，那么求法也是逆过来就可以了。

首先，用时间刻除以一天的秒数，可以算出天数
然后，时间刻就被分为了两个部分：
天数
一天内的秒数
关于天数，我们直接进行Date的加减，可以得到通用时的具体哪一天
关于一天内的秒数，我们采用和求天数相同不断除余的处理方法，一直求小时，分钟和秒

commontime UnixToCom(const gtime_t& gt)
    {
        commontime ret;
        ret._day = (int)(gt._time / DAY_SECOND);
        int second = (int)(gt._time - ret._day * DAY_SECOND);
        ret._hour = second % 3600;
        ret._minute = (second - ret._hour * 3600) % 60;
        ret._second = (second - ret._hour * 3600 - ret._minute * 60) % 60;

        Date tmp = Date(1970, 1, 1) + ret._day;
        ret._year = tmp._year;
        ret._month = tmp._month;
        ret._day = tmp._day;

        return ret;
}

unix时转换为GPS时

一样，首先考虑unix时和GPS时的起点差。

然后，用减去时间差后的秒数除以一周内的秒数，可以得到周数。而除余的数，就是一周内的周内秒

GPStime UnixToGPS(const gtime_t& gt)
    {
        GPStime ret;
        double sec = gt._time - UG_GAP;
        ret._week = (int)(sec / ROUND_SECOND);
        ret._second = (double)(sec - ret._week * ROUND_SECOND) + gt._second;

        return ret;
    }

时间汇总代码：

#define DAY_SECOND 86400//一天的时间
#define ROUND_SECOND 604800//一周的时间
#define UG_GAP 315964800//Unix时起点到GPS时起点

using namespace std;
typedef long long time_t;

struct commontime
{
    commontime()
    {}

    commontime(vector<unsigned> data, double second)
        :_year(data[0]),
        _month(data[1]),
        _day(data[2]),
        _hour(data[3]),
        _minute(data[4]),
        _second(second)
    {}

    unsigned _year;
    unsigned _month;
    unsigned _day;
    unsigned _hour;
    unsigned _minute;
    double _second;
};

struct GPStime
{
    GPStime()
    {}

    GPStime(unsigned week,double second)
        :_week(week),
        _second(second)
    {}

    unsigned _week;
    double _second;
};

struct gtime_t
{
    gtime_t()
    {}

    gtime_t(time_t time,double second)
        :_time(time),
        _second(second)
    {}

    time_t _time;
    double _second;
};

//总的时间系统
//当传入一个时间之后，自动同步另外两个时间
class atime
{
public:
    atime()
    {}

    atime(commontime& com)
        :_com(com)
    {
        _gt = ComToUnix(_com);
        _gps = UnixToGPS(_gt);
    }

    atime(GPStime& gps)
        :_gps(gps)
    {
        _gt = GPSToUnix(_gps);
        _com = UnixToCom(_gt);
    }

    atime(gtime_t& gt)
        :_gt(gt)
    {
        _com = UnixToCom(_gt);
        _gps = UnixToGPS(_gt);
    }

    static gtime_t ComToUnix(const commontime& com)
    {
        gtime_t ret;
        int day = Date(com._year, com._month, com._day) - Date(1970, 1, 1);
        ret._time = day * DAY_SECOND + com._hour * 3600 + com._minute * 60 + (time_t)floor(com._second);
        ret._second = com._second - floor(com._second);
        return ret;
    }

    static gtime_t GPSToUnix(const GPStime& gps)
    {
        gtime_t ret;
        ret._time = gps._week * 7 * DAY_SECOND + (time_t)gps._second + UG_GAP;
        ret._second = gps._second - (int)gps._second;

        return ret;
    }

    static commontime UnixToCom(const gtime_t& gt)
    {
        commontime ret;
        ret._day = (int)(gt._time / DAY_SECOND);
        int second = (int)(gt._time - ret._day * DAY_SECOND);
        ret._hour = second % 3600;
        ret._minute = (second - ret._hour * 3600) % 60;
        ret._second = (second - ret._hour * 3600 - ret._minute * 60) % 60;

        Date tmp = Date(1970, 1, 1) + ret._day;
        ret._year = tmp._year;
        ret._month = tmp._month;
        ret._day = tmp._day;

        return ret;
    }

    static GPStime UnixToGPS(const gtime_t& gt)
    {
        GPStime ret;
        double sec = gt._time - UG_GAP;
        ret._week = (int)(sec / ROUND_SECOND);
        ret._second = (double)(sec - ret._week * ROUND_SECOND) + gt._second;

        return ret;
    }

    const commontime& GetCommontime()
    {
        return _com;
    }

    const GPStime& GetGPStime()
    {
        return _gps;
    }

    const gtime_t& GetGtime()
    {
        return _gt;
    }
private:
    commontime _com;
    GPStime _gps;
    gtime_t _gt;
};

为了方便大家测试，给一个时间计算的网站：

日期计算器 (time.org.cn)https://time.org.cn/riqi/~~当然，肯定不是我写的。~~

空间

对于导航的空间系统，太具体的划分就不多说了，大体可以分为两大类：

空间坐标系，就是xyz系
大地坐标系，BLH系，就是我们常说的纬度，经度，大地高。

大地坐标系可能因为在生活中用的比较少，所以在这里简单再说说。

经度和纬度

经纬度，如果记不住的话，就想想平时经常用的词：
北纬，东经

所以，这样看就很明显了：把地球看作一个球体，从正面看过去，纬度就是南北方向，经度就是东西方向。但是，经纬度一定得有个起点，那么哪里是经纬度0度方向呢？

纬度很好理解，纬度的0度起点就是赤道，往南为南纬0~90度，往北为北纬0~90度。南90度为南极，北90度为北极，这是生活中常用的。

但是，经度起点就很少听说了。经度的起点在本初子午线，说人话就是英国有个格林尼治天文台，在国际上以这个天文台的原地址为经度的起点，也就是0度，然后一直转回这个天文台形成一个360度的球。

大地高

如果把地球看作一个标准的椭球体，即参考椭球体
那么在这个椭球体上，每一个点都可以做一个唯一的法平面，并且方程很容易就可以求出来。
而因为知道法平面方程，又知道地球上的某一个点的具体坐标，我们可以求出，每一个法平面，且过地球上点，对应的的法线方程。

在这所有法线方程里，总有一条是经过卫星的，而这个法线方程对应的点，与卫星的距离，就叫做大地高。

或者简单说是什么？把地球参考椭球面看作大地，大地高就是距离这个大地的高度。

所以，空间坐标系和大地坐标系都只需要三个参数便可以表示：

double coordinate[3];

大地坐标系和空间坐标系的相互转换

两个坐标系的相互转换，实际上是一个几何问题。
一个物体，无论其在哪个坐标系下，其物理形状和特征都是固定的。比如描述一个圆，我们用极坐标和空间坐标分别表示：

空间坐标系： $x^2+y^2=1$
极坐标系： $p=1$

虽然他们的形式不同，但是无论在哪个坐标系下，他们描述的是同一个东西，不会因为坐标系发生了变化而形状发生变化。所以，和三种时间系统一样，每一个坐标系统都是唯一映射关系，我们可以用与atime相同思想的类，当传入一种坐标系统的时候，直接初始化另一个坐标系统。

template<class Base_Coordinates>
class Position
{
public:
	//若基础坐标系是空间坐标系，则传入X/Y/Z
	//若基础坐标系是大地坐标系，则传入B/L/H
	Position(double data1, double data2, double data3);

	//获取空间坐标系的坐标X/Y/Z
	double* Get__Space_Coordinates()
	{
		return _Space_Coordinates;
	}

	//获取大地坐标系的坐标B/L/H
	double* Get__Geodetic_Coordinate_System()
	{
		return _Geodetic_Coordinate_System;
	}


private:
	double _Space_Coordinates[3];
	double _Geodetic_Coordinate_System[3];
	Base_Coordinates _Base_Coordinates;
};

~~但是因为，两个坐标系统的类型都是double[3]，所以用了个模板的方法，现在想想其实挺谢特的...~~

大地坐标系和空间坐标系的关系

想要转换两个坐标系，最首先的问题就是：两个坐标系怎么联系起来。
比如，空间坐标系的x轴是什么，y轴是什么，z轴是什么，和经纬度有什么关系。这个就不是由我说了算了，在国际上有统一标准：

Z轴：Z轴指向地球北极
X轴：X轴指向0纬0经
Y轴：由右手法则确定

大地坐标系转空间坐标系

具体的计算过程推导，因为电脑实在不好写公式，所以只能拍一下笔记，见谅：

最后可以得出公式：

#define Pi 3.1415926535898
#define LONG_AXIS 6378137
#define SHORT_AXIS 6356755
#define f 1.0/298.257223563


class Geodetic_Coordinate_System
{
public:
	Geodetic_Coordinate_System(double Long_Axis = LONG_AXIS, double Short_Axis = SHORT_AXIS)
	{
		_Long_Axis = Long_Axis;
		_Short_Axis = Short_Axis;
		/*_Eccentricity = pow((pow(_Long_Axis, 2) - pow(_Short_Axis, 2)), 0.5) / _Long_Axis;*/
		_Eccentricity = sqrt(2 * f - f * f);//两个公式都可以，只是形式不同，答案相同
	}
	double To_X(double B, double L, double H)
	{
		double Curvature_Radius = _Long_Axis / pow(1 - pow(_Eccentricity * sin(B * Pi / 180), 2), 0.5);
		return (Curvature_Radius + H) * cos(B * Pi / 180) * cos(L * Pi / 180);
	}
	double To_Y(double B, double L, double H)
	{
		double Curvature_Radius = _Long_Axis / pow(1 - pow(_Eccentricity * sin(B * Pi / 180), 2), 0.5);
		return (Curvature_Radius + H) * cos(B * Pi / 180) * sin(L * Pi / 180);
	}
	double To_Z(double B, double L, double H)
	{
		double Curvature_Radius = _Long_Axis / pow(1 - pow(_Eccentricity * sin(B * Pi / 180), 2), 0.5);
		return (Curvature_Radius * (1 - pow(_Eccentricity, 2)) + H) * sin(B * Pi / 180);
	}
private:
	double _Long_Axis;//长轴
	double _Short_Axis;//短轴
	double _Eccentricity;//偏心率
};

空间坐标系转大地坐标系

计算方法是大地坐标系转空间坐标系的逆过程，就不列出具体求法了。计算公式如图：

但是，要详细说一下，B需要进行迭代计算。
为什么？

我们发现，等式左边是B，右边又有sinB，我们怎么可以用自己来求自己呢？但是，这是计算机实现的一种思想。

为了方便计算机计算，我们要把大多数计算变为线性的，说人话就是不要去让计算机解方程，而是直接告诉计算机结果表达式，比如：

$x^2+3a+2a^2=0$

尽管这样很简洁，但是你这样告诉计算机，计算机根本不知道要干什么，也不知道怎么解这个方程。所以，我们必须要把结果先预处理好，使等式为一个可以直接相等的表达式：

$x1=-a; x2=-2a;$

所以，这里的B，就是预处理后的结果。我们对B取一个初值，B会根据这个初值，一直得出新的B，一直到B收敛到了一个具体的值，即B的变化在很小很小几乎可以忽略不计的时候，此时得出的B就是我们所要求的B。

而且，一定要进行迭代计算！！！因为B就算只相差一点点，对大地高H的影响也是巨大的，所以必须要把B锁定在很高的精度，否则会极大影响大地高的精度。

#define Pi 3.1415926535898
#define LONG_AXIS 6378137
#define SHORT_AXIS 6356755
#define f 1.0/298.257223563


class Space_Coordinates
{
public:
	Space_Coordinates(double Long_Axis = LONG_AXIS, double Short_Axis = SHORT_AXIS)
	{
		_Long_Axis = Long_Axis;
		_Short_Axis = Short_Axis;
		/*_Eccentricity = pow((pow(_Long_Axis, 2) - pow(_Short_Axis, 2)), 0.5) / _Long_Axis;*/
		_Eccentricity = sqrt(2 * f - f * f);
	}
	double To_Latitude(double X, double Y, double Z)
	{
		double R = sqrt(X * X + Y * Y);
		double B1 = atan2(Z , R);
		double B2;
		while (1)
		{
			double W1 = sqrt(1 - _Eccentricity * _Eccentricity * sin(B1) * sin(B1));
			double N1 = LONG_AXIS / W1;
			B2 = atan((Z + N1 * _Eccentricity * _Eccentricity * sin(B1)) / R);
			if (abs(B2 - B1) <= 1e-11)	break;
			B1 = B2;
		}
		return B2 * 180 / Pi;
	}//迭代计算
	double To_Longitude(double X, double Y, double Z)
	{
		return atan2(Y, X) * 180 / Pi;
	}
	double To_Height(double X, double Y, double Z)
	{
		double B = To_Latitude(X, Y, Z);
		double N = LONG_AXIS / sqrt(1 - pow(_Eccentricity * sin(B * Pi / 180), 2));
		double FAI = atan(Z/pow(pow(X, 2) + pow(Y, 2), 0.5));
		return sqrt(X * X + Y * Y + Z * Z) * cos(FAI) / cos(B * Pi / 180) - N;
	}
private:
	double _Long_Axis;
	double _Short_Axis;
	double _Eccentricity;
};

并且，求出经纬度的时候，我们算出的结果是弧度，我们要对所有经纬度进行*180/Pi，这样得出来的东西才是给人看的角度。

最后，给自己叠个甲。因为自己才是导航工程大二的本科生，有些概念理解可能不到位，而又想用最容易理解的方式表达出来，所以可能正确性会稍微有些偏差。但是对初学者来说，应该不会存在太大的错误，如果可以帮到你，真的荣幸之极。还有，