如何将球面速度坐标转换为笛卡尔坐标
How to convert a spherical velocity coordinates into cartesian
我有一个高度、经度、高度的速度矢量，我想将它转换为笛卡尔坐标 vx、vy、vz。格式来自WGS84标准。
这里是公式
//------------------------------------------------------------------------------
template <class T>
TVectorXYZ<T> WGS84::ToCartesian(T latitude, T longitude, T elevation)
//------------------------------------------------------------------------------
{
double sinlat, coslat;
double sinlon, coslon;
sincos_degree(latitude, sinlat, coslat);
sincos_degree(longitude, sinlon, coslon);
const double v = a / sqrt(1 - WGS84::ee * sinlat*sinlat);
TVectorXYZ<T> coord
(
static_cast<T>((v + elevation) * coslat * sinlon),
static_cast<T>(((1 - WGS84::ee) * v + elevation) * sinlat),
static_cast<T>((v + elevation) * coslat * coslon)
);
return coord;
}
采用用于将位置从地理坐标转换为笛卡尔坐标的公式。那是一些向量 p(λ,φ,h) ∈ ℝ³，即将纬度、经度和高度转换为 x、y、z 坐标的三元素向量。现在计算该公式关于三个参数的偏导数。您将获得三个向量，它们应该相互正交。关于经度 λ 的导数应该局部指向东，关于纬度 φ 的导数指向北，关于高度 h 的导数指向上。将这些向量与您必须获得笛卡尔速度向量的速度相乘。
观察单位如何匹配：位置以米为单位，前两个导数是米/度，速度是度/秒。或者其他，也许是英里和弧度。
所有这些对于球体来说都相当容易。对于WGS84椭球，位置公式有点复杂，复杂度会贯穿计算。
基于您之前的 和长评论流程，假设您的输入是：
lon [rad], lat [rad], alt [m] // WGS84 position
vlon [m/s], vlat [m/s], alt [m/s] // speed in WGS84 lon,lat,alt directions but in [m/s]
并且想要输出：
x,y,z // Cartesian position [m/s]
vx,vy,vz // Cartesian velocity [m/s]
并有效转换为笛卡尔坐标，供您使用这是我的：
void WGS84toXYZ(double &x,double &y,double &z,double lon,double lat,double alt) // [rad,rad,m] -> [m,m,m]
{
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
const double _aa=_earth_a*_earth_a;
const double _ee=_earth_e*_earth_e;
double a,b,x,y,z,h,l,c,s;
a=lon;
b=lat;
h=alt;
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_ee*s*s));
x=(l+h)*c*cos(a);
y=(l+h)*c*sin(a);
z=(((1.0-_ee)*l)+h)*s;
}
以及将向量归一化为单位大小的例程：
void normalize(double &x,double &y,double &z)
{
double l=sqrt(x*x+y*y+z*z);
if (l>1e-6) l=1.0/l;
x*=l; y*=l; z*=l;
}
是的，您可以尝试推导 lihe @MvG 建议的公式，但根据您的新手错误，我强烈怀疑它是否会导致成功的结果。相反，您可以这样做：
为您的位置获取 lon,lat,alt 方向向量 (x,y,z)
这很简单，只需在 WGS84 位置中使用一些小步长增量即可转换为笛卡尔减法并归一化为单位向量。我们将这些方向基向量称为 U,V,W.
double Ux,Uy,Uz; // [m]
double Vx,Vy,Vz; // [m]
double Wx,Wy,Wz; // [m]
double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
double dl=1.0; // [m] altitide step 1.0 m
WGS84toXYZ( x, y, z,lon ,lat,alt ); // actual position
WGS84toXYZ(Ux,Uy,Uz,lon+da,lat,alt ); // lon direction Nort
WGS84toXYZ(Vx,Vy,Vz,lon,lat+da,alt ); // lat direction East
WGS84toXYZ(Wx,Wy,Wz,lon,lat ,alt+dl); // alt direction High/Up
Ux-=x; Uy-=y; Uz-=z;
Vx-=x; Vy-=y; Vz-=z;
Wx-=x; Wy-=y; Wz-=z;
normalize(Ux,Uy,Uz);
normalize(Vx,Vy,Vz);
normalize(Wx,Wy,Wz);
将速度从 lon,lat,alt 转换为 vx,vy,vz
vx = vlon*Ux + vlat*Vx + valt*Wx;
vy = vlon*Uy + vlat*Vy + valt*Wy;
vz = vlon*Uz + vlat*Vz + valt*Wz;
希望已经足够清楚了。像往常一样注意单位 deg/rad 和 m/ft/km 因为单位很重要。
Btw U,V,W 基向量形式 NEH reference frame 同时是方向导数 MvG 提到。
[Edit1] 更精确的转换
//---------------------------------------------------------------------------
//--- WGS84 transformations ver: 1.00 ---------------------------------------
//---------------------------------------------------------------------------
#ifndef _WGS84_h
#define _WGS84_h
//---------------------------------------------------------------------------
// http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
//---------------------------------------------------------------------------
// WGS84(a,b,h) = (long,lat,alt) [rad,rad,m]
// XYZ(x,y,z) [m]
//---------------------------------------------------------------------------
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
//const double _earth_e=sqrt(1.0-((_earth_b/_earth_a)*(_earth_b/_earth_a)));
const double _earth_ee=_earth_e*_earth_e;
//---------------------------------------------------------------------------
const double kmh=1.0/3.6; // [km/h] -> [m/s]
//---------------------------------------------------------------------------
void XYZtoWGS84 (double *abh ,double *xyz ); // [m,m,m] -> [rad,rad,m]
void WGS84toXYZ (double *xyz ,double *abh ); // [rad,rad,m] -> [m,m,m]
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel); // [rad,rad,m],[m/s,m/s,m/s] -> [m,m,m],[m/s,m/s,m/s]
void WGS84toNEH (reper &neh ,double *abh ); // [rad,rad,m] -> NEH [m]
void WGS84_m2rad (double &da,double &db,double *abh); // [rad,rad,m] -> [rad],[rad] representing 1m angle step
void XYZ_interpolate (double *pt,double *p0,double *p1,double t); // [m,m,m] pt = p0 + (p1-p0)*t in ellipsoid space t = <0,1>
//---------------------------------------------------------------------------
void XYZtoWGS84(double *abh,double *xyz)
{
int i;
double a,b,h,l,n,db,s;
a=atanxy(xyz[0],xyz[1]);
l=sqrt((xyz[0]*xyz[0])+(xyz[1]*xyz[1]));
// estimate lat
b=atanxy((1.0-_earth_ee)*l,xyz[2]);
// iterate to improve accuracy
for (i=0;i<100;i++)
{
s=sin(b); db=b;
n=divide(_earth_a,sqrt(1.0-(_earth_ee*s*s)));
h=divide(l,cos(b))-n;
b=atanxy((1.0-divide(_earth_ee*n,n+h))*l,xyz[2]);
db=fabs(db-b);
if (db<1e-12) break;
}
if (b>0.5*pi) b-=pi2;
abh[0]=a;
abh[1]=b;
abh[2]=h;
}
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
{
double a,b,h,l,c,s;
a=abh[0];
b=abh[1];
h=abh[2];
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
xyz[0]=(l+h)*c*cos(a);
xyz[1]=(l+h)*c*sin(a);
xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
}
//---------------------------------------------------------------------------
void WGS84toNEH(reper &neh,double *abh)
{
double N[3],E[3],H[3]; // [m]
double p[3],xyzpos[3];
const double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
const double dl=1.0; // [m] altitide step 1.0 m
vector_copy(p,abh);
// actual position
WGS84toXYZ(xyzpos,abh);
// NEH
p[0]+=da; WGS84toXYZ(N,p); p[0]-=da;
p[1]+=da; WGS84toXYZ(E,p); p[1]-=da;
p[2]+=dl; WGS84toXYZ(H,p); p[2]-=dl;
vector_sub(N,N,xyzpos);
vector_sub(E,E,xyzpos);
vector_sub(H,H,xyzpos);
vector_one(N,N);
vector_one(E,E);
vector_one(H,H);
neh._rep=1;
neh._inv=0;
// axis X
neh.rep[ 0]=N[0];
neh.rep[ 1]=N[1];
neh.rep[ 2]=N[2];
// axis Y
neh.rep[ 4]=E[0];
neh.rep[ 5]=E[1];
neh.rep[ 6]=E[2];
// axis Z
neh.rep[ 8]=H[0];
neh.rep[ 9]=H[1];
neh.rep[10]=H[2];
// gpos
neh.rep[12]=xyzpos[0];
neh.rep[13]=xyzpos[1];
neh.rep[14]=xyzpos[2];
neh.orto(1);
}
//---------------------------------------------------------------------------
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel)
{
reper neh;
WGS84toNEH(neh,abhpos);
neh.gpos_get(xyzpos);
neh.l2g_dir(xyzvel,abhvel);
}
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
{
// WGS84 from eccentricity
double p[3],rr;
WGS84toXYZ(p,abh);
rr=(p[0]*p[0])+(p[1]*p[1]);
da=divide(1.0,sqrt(rr));
rr+=p[2]*p[2];
db=divide(1.0,sqrt(rr));
}
//---------------------------------------------------------------------------
void XYZ_interpolate(double *pt,double *p0,double *p1,double t)
{
const double mz=_earth_a/_earth_b;
const double _mz=_earth_b/_earth_a;
double p[3],r,r0,r1;
// compute spherical radiuses of input points
r0=sqrt((p0[0]*p0[0])+(p0[1]*p0[1])+(p0[2]*p0[2]*mz*mz));
r1=sqrt((p1[0]*p1[0])+(p1[1]*p1[1])+(p1[2]*p1[2]*mz*mz));
// linear interpolation
r = r0 +(r1 -r0 )*t;
p[0]= p0[0]+(p1[0]-p0[0])*t;
p[1]= p0[1]+(p1[1]-p0[1])*t;
p[2]=(p0[2]+(p1[2]-p0[2])*t)*mz;
// correct radius and rescale back
r/=sqrt((p[0]*p[0])+(p[1]*p[1])+(p[2]*p[2]));
pt[0]=p[0]*r;
pt[1]=p[1]*r;
pt[2]=p[2]*r*_mz;
}
//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------
但是他们需要基本的 3D 矢量数学，请参阅此处的方程式：
我有一个高度、经度、高度的速度矢量，我想将它转换为笛卡尔坐标 vx、vy、vz。格式来自WGS84标准。
这里是公式
//------------------------------------------------------------------------------
template <class T>
TVectorXYZ<T> WGS84::ToCartesian(T latitude, T longitude, T elevation)
//------------------------------------------------------------------------------
{
double sinlat, coslat;
double sinlon, coslon;
sincos_degree(latitude, sinlat, coslat);
sincos_degree(longitude, sinlon, coslon);
const double v = a / sqrt(1 - WGS84::ee * sinlat*sinlat);
TVectorXYZ<T> coord
(
static_cast<T>((v + elevation) * coslat * sinlon),
static_cast<T>(((1 - WGS84::ee) * v + elevation) * sinlat),
static_cast<T>((v + elevation) * coslat * coslon)
);
return coord;
}
采用用于将位置从地理坐标转换为笛卡尔坐标的公式。那是一些向量 p(λ,φ,h) ∈ ℝ³，即将纬度、经度和高度转换为 x、y、z 坐标的三元素向量。现在计算该公式关于三个参数的偏导数。您将获得三个向量，它们应该相互正交。关于经度 λ 的导数应该局部指向东，关于纬度 φ 的导数指向北，关于高度 h 的导数指向上。将这些向量与您必须获得笛卡尔速度向量的速度相乘。
观察单位如何匹配：位置以米为单位，前两个导数是米/度，速度是度/秒。或者其他，也许是英里和弧度。
所有这些对于球体来说都相当容易。对于WGS84椭球，位置公式有点复杂，复杂度会贯穿计算。
基于您之前的
lon [rad], lat [rad], alt [m] // WGS84 position
vlon [m/s], vlat [m/s], alt [m/s] // speed in WGS84 lon,lat,alt directions but in [m/s]
并且想要输出：
x,y,z // Cartesian position [m/s]
vx,vy,vz // Cartesian velocity [m/s]
并有效转换为笛卡尔坐标，供您使用这是我的：
void WGS84toXYZ(double &x,double &y,double &z,double lon,double lat,double alt) // [rad,rad,m] -> [m,m,m]
{
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
const double _aa=_earth_a*_earth_a;
const double _ee=_earth_e*_earth_e;
double a,b,x,y,z,h,l,c,s;
a=lon;
b=lat;
h=alt;
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_ee*s*s));
x=(l+h)*c*cos(a);
y=(l+h)*c*sin(a);
z=(((1.0-_ee)*l)+h)*s;
}
以及将向量归一化为单位大小的例程：
void normalize(double &x,double &y,double &z)
{
double l=sqrt(x*x+y*y+z*z);
if (l>1e-6) l=1.0/l;
x*=l; y*=l; z*=l;
}
是的，您可以尝试推导 lihe @MvG 建议的公式，但根据您的新手错误，我强烈怀疑它是否会导致成功的结果。相反，您可以这样做：
为您的位置获取
lon,lat,alt方向向量(x,y,z)这很简单，只需在 WGS84 位置中使用一些小步长增量即可转换为笛卡尔减法并归一化为单位向量。我们将这些方向基向量称为
U,V,W.double Ux,Uy,Uz; // [m] double Vx,Vy,Vz; // [m] double Wx,Wy,Wz; // [m] double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction double dl=1.0; // [m] altitide step 1.0 m WGS84toXYZ( x, y, z,lon ,lat,alt ); // actual position WGS84toXYZ(Ux,Uy,Uz,lon+da,lat,alt ); // lon direction Nort WGS84toXYZ(Vx,Vy,Vz,lon,lat+da,alt ); // lat direction East WGS84toXYZ(Wx,Wy,Wz,lon,lat ,alt+dl); // alt direction High/Up Ux-=x; Uy-=y; Uz-=z; Vx-=x; Vy-=y; Vz-=z; Wx-=x; Wy-=y; Wz-=z; normalize(Ux,Uy,Uz); normalize(Vx,Vy,Vz); normalize(Wx,Wy,Wz);将速度从
lon,lat,alt转换为vx,vy,vzvx = vlon*Ux + vlat*Vx + valt*Wx; vy = vlon*Uy + vlat*Vy + valt*Wy; vz = vlon*Uz + vlat*Vz + valt*Wz;
希望已经足够清楚了。像往常一样注意单位 deg/rad 和 m/ft/km 因为单位很重要。
Btw U,V,W 基向量形式 NEH reference frame 同时是方向导数 MvG 提到。
[Edit1] 更精确的转换
//---------------------------------------------------------------------------
//--- WGS84 transformations ver: 1.00 ---------------------------------------
//---------------------------------------------------------------------------
#ifndef _WGS84_h
#define _WGS84_h
//---------------------------------------------------------------------------
// http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
//---------------------------------------------------------------------------
// WGS84(a,b,h) = (long,lat,alt) [rad,rad,m]
// XYZ(x,y,z) [m]
//---------------------------------------------------------------------------
const double _earth_a=6378137.00000; // [m] WGS84 equator radius
const double _earth_b=6356752.31414; // [m] WGS84 epolar radius
const double _earth_e=8.1819190842622e-2; // WGS84 eccentricity
//const double _earth_e=sqrt(1.0-((_earth_b/_earth_a)*(_earth_b/_earth_a)));
const double _earth_ee=_earth_e*_earth_e;
//---------------------------------------------------------------------------
const double kmh=1.0/3.6; // [km/h] -> [m/s]
//---------------------------------------------------------------------------
void XYZtoWGS84 (double *abh ,double *xyz ); // [m,m,m] -> [rad,rad,m]
void WGS84toXYZ (double *xyz ,double *abh ); // [rad,rad,m] -> [m,m,m]
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel); // [rad,rad,m],[m/s,m/s,m/s] -> [m,m,m],[m/s,m/s,m/s]
void WGS84toNEH (reper &neh ,double *abh ); // [rad,rad,m] -> NEH [m]
void WGS84_m2rad (double &da,double &db,double *abh); // [rad,rad,m] -> [rad],[rad] representing 1m angle step
void XYZ_interpolate (double *pt,double *p0,double *p1,double t); // [m,m,m] pt = p0 + (p1-p0)*t in ellipsoid space t = <0,1>
//---------------------------------------------------------------------------
void XYZtoWGS84(double *abh,double *xyz)
{
int i;
double a,b,h,l,n,db,s;
a=atanxy(xyz[0],xyz[1]);
l=sqrt((xyz[0]*xyz[0])+(xyz[1]*xyz[1]));
// estimate lat
b=atanxy((1.0-_earth_ee)*l,xyz[2]);
// iterate to improve accuracy
for (i=0;i<100;i++)
{
s=sin(b); db=b;
n=divide(_earth_a,sqrt(1.0-(_earth_ee*s*s)));
h=divide(l,cos(b))-n;
b=atanxy((1.0-divide(_earth_ee*n,n+h))*l,xyz[2]);
db=fabs(db-b);
if (db<1e-12) break;
}
if (b>0.5*pi) b-=pi2;
abh[0]=a;
abh[1]=b;
abh[2]=h;
}
//---------------------------------------------------------------------------
void WGS84toXYZ(double *xyz,double *abh)
{
double a,b,h,l,c,s;
a=abh[0];
b=abh[1];
h=abh[2];
c=cos(b);
s=sin(b);
// WGS84 from eccentricity
l=_earth_a/sqrt(1.0-(_earth_ee*s*s));
xyz[0]=(l+h)*c*cos(a);
xyz[1]=(l+h)*c*sin(a);
xyz[2]=(((1.0-_earth_ee)*l)+h)*s;
}
//---------------------------------------------------------------------------
void WGS84toNEH(reper &neh,double *abh)
{
double N[3],E[3],H[3]; // [m]
double p[3],xyzpos[3];
const double da=1.567e-7; // [rad] angular step ~ 1.0 m in lon direction
const double dl=1.0; // [m] altitide step 1.0 m
vector_copy(p,abh);
// actual position
WGS84toXYZ(xyzpos,abh);
// NEH
p[0]+=da; WGS84toXYZ(N,p); p[0]-=da;
p[1]+=da; WGS84toXYZ(E,p); p[1]-=da;
p[2]+=dl; WGS84toXYZ(H,p); p[2]-=dl;
vector_sub(N,N,xyzpos);
vector_sub(E,E,xyzpos);
vector_sub(H,H,xyzpos);
vector_one(N,N);
vector_one(E,E);
vector_one(H,H);
neh._rep=1;
neh._inv=0;
// axis X
neh.rep[ 0]=N[0];
neh.rep[ 1]=N[1];
neh.rep[ 2]=N[2];
// axis Y
neh.rep[ 4]=E[0];
neh.rep[ 5]=E[1];
neh.rep[ 6]=E[2];
// axis Z
neh.rep[ 8]=H[0];
neh.rep[ 9]=H[1];
neh.rep[10]=H[2];
// gpos
neh.rep[12]=xyzpos[0];
neh.rep[13]=xyzpos[1];
neh.rep[14]=xyzpos[2];
neh.orto(1);
}
//---------------------------------------------------------------------------
void WGS84toXYZ_posvel(double *xyzpos,double *xyzvel,double *abhpos,double *abhvel)
{
reper neh;
WGS84toNEH(neh,abhpos);
neh.gpos_get(xyzpos);
neh.l2g_dir(xyzvel,abhvel);
}
//---------------------------------------------------------------------------
void WGS84_m2rad(double &da,double &db,double *abh)
{
// WGS84 from eccentricity
double p[3],rr;
WGS84toXYZ(p,abh);
rr=(p[0]*p[0])+(p[1]*p[1]);
da=divide(1.0,sqrt(rr));
rr+=p[2]*p[2];
db=divide(1.0,sqrt(rr));
}
//---------------------------------------------------------------------------
void XYZ_interpolate(double *pt,double *p0,double *p1,double t)
{
const double mz=_earth_a/_earth_b;
const double _mz=_earth_b/_earth_a;
double p[3],r,r0,r1;
// compute spherical radiuses of input points
r0=sqrt((p0[0]*p0[0])+(p0[1]*p0[1])+(p0[2]*p0[2]*mz*mz));
r1=sqrt((p1[0]*p1[0])+(p1[1]*p1[1])+(p1[2]*p1[2]*mz*mz));
// linear interpolation
r = r0 +(r1 -r0 )*t;
p[0]= p0[0]+(p1[0]-p0[0])*t;
p[1]= p0[1]+(p1[1]-p0[1])*t;
p[2]=(p0[2]+(p1[2]-p0[2])*t)*mz;
// correct radius and rescale back
r/=sqrt((p[0]*p[0])+(p[1]*p[1])+(p[2]*p[2]));
pt[0]=p[0]*r;
pt[1]=p[1]*r;
pt[2]=p[2]*r*_mz;
}
//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------
但是他们需要基本的 3D 矢量数学，请参阅此处的方程式：