ENU站心坐标系
站心坐标系也叫做站点坐标系、东-北-天坐标系ENU,英文名称是local Cartesian coordinates coordinate system,主要是用于需了解以观察者为中心的其他物体运动规律。
站心直角坐标系
定义:以站心(如GPS接收天线中心)为坐标系原点O,Z轴与椭球法线重合,向上为正(天向),y与椭球短半轴重合(北向),x轴与地球椭球的长半轴重合(东向)所构成的直角坐标系,称为当地东北天坐标系(ENU)。
站心极坐标系
定义:以站心为坐标极点O,以水平面(即xoy平面)为基准面,以东向轴(即x轴)为极轴,ρ为卫星到站点的距离,az为星视方向角(azimuth angle),el为星视仰角(elevation)。
baike.baidu.com/item/站心坐标系/4542391?fr=ge_ala
地球短轴南北方向y,长轴东西方向x。
注意站心天向(法向量)与赤道面相交不一定会经过球心
ECEF地心地固坐标系
地心地固坐标系(Earth-Centered,Earth-Fixed,简称ECEF)简称地心坐标系,是一种以地心为原点的地固坐标系(也称地球坐标系),是一种笛卡儿坐标系。原点O (0,0,0)为地球质心,z轴与地轴平行指向北极点,x轴指向本初子午线与赤道的交点,y轴垂直于xOz平面(即东经90度与赤道的交点)构成右手坐标系。
baike.baidu.com/item/地心地固坐标系/22657523?fr=ge_ala
从ECEF转换到ENU
从ENU转换到ECEF
其中,也是一个4x4矩阵,用于将ENU坐标转换为ECEF坐标。
代码
//ECEF转ENU
//参数:pts为ENU原点在ECEF坐标系下的坐标
//参数:pt为ECEF坐标系下任意一点
//返回值:pt在ENU坐标系下的坐标
myPoint ECEF2ENU(myPoint pts, myPoint pt)
{
double lon, lat, altitude;
ECEFtoWGS84(pts, lon, lat, altitude);
lon = degreeToRadian(lon);
lat = degreeToRadian(lat);
myMatrix4x4 moveMatrix;
moveMatrix.Identity();
moveMatrix.Move(pts.Reversal());
myMatrix4x4 rotateMatrix;
rotateMatrix.Identity();
rotateMatrix.SetElement(0, 0, -sin(lon));
rotateMatrix.SetElement(0, 1, cos(lon));
rotateMatrix.SetElement(1, 0, -sin(lat) * cos(lon));
rotateMatrix.SetElement(1, 1, -sin(lat) * cos(lon));
rotateMatrix.SetElement(1, 2, cos(lat));
rotateMatrix.SetElement(2, 0, cos(lat) * cos(lon));
rotateMatrix.SetElement(2, 1, cos(lat) * cos(lon));
rotateMatrix.SetElement(2, 2, sin(lat));
myPoint ptRet = moveMatrix.MultiplyPoint(pt);
return rotateMatrix.MultiplyPoint(ptRet);
}
//ENU转ECEF
//参数:pts为ENU原点在ECEF坐标系下的坐标
//参数:pt为ENU坐标系下任意一点
//返回值:pt在ECEF坐标系下的坐标
myPoint ENU2ECEF(myPoint pts, myPoint pt)
{
double lon, lat, altitude;
ECEFtoWGS84(pts, lon, lat, altitude);
lon = degreeToRadian(lon);
lat = degreeToRadian(lat);
myMatrix4x4 rotateMatrix;
rotateMatrix.Identity();
rotateMatrix.SetElement(0, 0, -sin(lon));
rotateMatrix.SetElement(0, 1, -cos(lon) * sin(lat));
rotateMatrix.SetElement(0, 2, cos(lon) * sin(lat));
rotateMatrix.SetElement(1, 0, cos(lon));
rotateMatrix.SetElement(1, 1, -sin(lon) * sin(lat));
rotateMatrix.SetElement(1, 2, sin(lon) * sin(lat));
rotateMatrix.SetElement(2, 0, 0);
rotateMatrix.SetElement(2, 1, cos(lat));
rotateMatrix.SetElement(2, 2, sin(lat));
myMatrix4x4 moveMatrix;
moveMatrix.Identity();
moveMatrix.Move(pts);
myPoint ptRet = rotateMatrix.MultiplyPoint(pt);
return moveMatrix.MultiplyPoint(ptRet);
}
class myPoint
{
public:
myPoint();
myPoint(double x, double y, double z);
myPoint(const myPoint&ref);
myPoint operator=(const myPoint& rhs);
void SetX(double x);
void SetY(double y);
void SetZ(double z);
double X()const;
double Y()const;
double Z()const;
myPoint add(myPoint& rhs)const;
myPoint Sub(myPoint& rhs)const;
myPoint cross(myPoint& rhs)const;
myPoint Normalize() const;
double Length() const;
myPoint Scale(const double& rhs) const;
double Dot(const myPoint& rhs) const;
myPoint Reversal() const;
private:
double m_x, m_y, m_z;
};
///////////////////////////////////////
#include "myPoint.h"
class myMatrix4x4
{
public:
myMatrix4x4();
void Zero();
void Identity();
void SetElement(int i, int j, double value);
void Move(const myPoint& vec);//平移矩阵
//参考void QMatrix4x4::rotate(float angle, float x, float y, float z)
void Rotate(const myPoint& vec, const double& radian);//绕vec向量旋转radian度的矩阵
myPoint MultiplyPoint(const myPoint& pt)const;//return this*pt;
private:
double m_Element[4][4];
};
#include "myPoint.h"
#include <cmath>
myPoint::myPoint()
{
m_x = m_y = m_z = 0;
}
myPoint::myPoint(double x, double y, double z):m_x(x),m_y(y),m_z(z)
{
}
myPoint::myPoint(const myPoint& ref) : m_x(ref.X()), m_y(ref.Y()), m_z(ref.Z())
{
}
myPoint myPoint::operator=(const myPoint& rhs)
{
SetX(rhs.X());
SetY(rhs.Y());
SetZ(rhs.Z());
return *this;
}
void myPoint::SetX(double x)
{
m_x = x;
}
void myPoint::SetY(double y)
{
m_y = y;
}
void myPoint::SetZ(double z)
{
m_z = z;
}
double myPoint::X()const
{
return m_x;
}
double myPoint::Y()const
{
return m_y;
}
double myPoint::Z()const
{
return m_z;
}
myPoint myPoint::add(myPoint& rhs)const
{
myPoint ret(0.0, 0.0, 0.0);
ret.SetX(m_x + rhs.X());
ret.SetY(m_y + rhs.Y());
ret.SetZ(m_z + rhs.Z());
return ret;
}
myPoint myPoint::Sub(myPoint& rhs)const
{
myPoint ret(0.0, 0.0, 0.0);
ret.SetX(m_x - rhs.X());
ret.SetY(m_y - rhs.Y());
ret.SetZ(m_z - rhs.Z());
return ret;
}
myPoint myPoint::cross(myPoint& rhs)const
{
myPoint ret(0.0, 0.0, 0.0);
ret.SetX(m_y * rhs.Z() - m_z * rhs.Y());
ret.SetY(m_z * rhs.X() - m_x * rhs.Z());
ret.SetZ(m_x * rhs.Y() - m_y * rhs.X());
return ret;
}
myPoint myPoint::Normalize() const
{
myPoint ret(0.0, 0.0, 0.0);
ret.SetX(m_x / Length());
ret.SetY(m_y / Length());
ret.SetZ(m_z / Length());
return ret;
}
double myPoint::Length() const
{
return sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
}
myPoint myPoint::Scale(const double& rhs) const
{
myPoint ret(0.0, 0.0, 0.0);
ret.SetX(m_x * rhs);
ret.SetY(m_y * rhs);
ret.SetZ(m_z * rhs);
return ret;
}
double myPoint::Dot(const myPoint& rhs) const
{
return m_x * rhs.X() + m_y * rhs.Y() + m_z * rhs.Z();
}
myPoint myPoint::Reversal() const
{
return myPoint(-X(), -Y(), -Z());
}
#include "myMatrix4x4.h"
#include <cmath>
myMatrix4x4::myMatrix4x4()
{
Identity();
}
void myMatrix4x4::Zero()
{
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
m_Element[i][j] = 0;
}
}
}
void myMatrix4x4::Identity()
{
for (int i = 0; i < 4; ++i)
{
for (int j = 0; j < 4; ++j)
{
if (i == j)
{
m_Element[i][j] = 1.0;
}
else
{
m_Element[i][j] = 0.0;
}
}
}
}
void myMatrix4x4::SetElement(int i, int j, double value)
{
m_Element[i][j] = value;
}
void myMatrix4x4::Move(const myPoint& vec)
{
Identity();
SetElement(0, 3, vec.X());
SetElement(1, 3, vec.Y());
SetElement(2, 3, vec.Z());
}
void myMatrix4x4::Rotate(const myPoint& vec, const double& radian)
{
//D:\Qt6\6.8.0\Src\qtbase\src\gui\math3d\qmatrix4x4.cpp
//void QMatrix4x4::rotate(float angle, float x, float y, float z)
Identity();
float dX = vec.X() / vec.Length();
float dY = vec.Y() / vec.Length();
float dZ = vec.Z() / vec.Length();
double c = cos(radian);
double s = sin(radian);
SetElement(0, 0, c + (dX * dX) * (1 - c));
SetElement(1, 0, dX * dY * (1 - c) - dZ * s);
SetElement(2, 0, dX * dZ * (1 - c) + dY * s);
SetElement(0, 1, dX * dY * (1 - c) + dZ * s);
SetElement(1, 1, c + dY * dY * (1 - c));
SetElement(2, 1, dY * dZ * (1 - c) - dX * s);
SetElement(0, 2, dX * dZ * (1 - c) - dY * s);
SetElement(1, 2, dZ * dY * (1 - c) + dX * s);
SetElement(2, 2, c + dZ * dZ * (1 - c));
}
myPoint myMatrix4x4::MultiplyPoint(const myPoint& pt)const
{
myPoint retPt;
double v0 = pt.X();
double v1 = pt.Y();
double v2 = pt.Z();
double v3 = 1;
double ret[4];
for (int i=0;i<4;i++)
{
ret[i] = m_Element[i][0] * v0 + m_Element[i][1] * v1 + m_Element[i][2] * v2 + m_Element[i][3] * v3;
}
retPt.SetX(ret[0]);
retPt.SetY(ret[1]);
retPt.SetZ(ret[2]);
return retPt;
}