# 2. 推导

## 2.1. BLH->XYZ

$\begin{cases} Z = y\\ X = x \cdot cosL\\ Y = x \cdot sinL\\ \end{cases} \tag{1}$

$\frac{x^2}{a^2} + \frac{y^2}{b^2} = 1 \tag{1.2}$

$\frac{dy}{dx} = -\frac{b^2}{a^2} \cdot \frac{x}{y} \tag{2}$

$\frac{dy}{dx} = tan(90^o + B) = -cotB \tag{3}$

$y = x(1-e^2)tanB \tag{4}$

$e = -\frac{\sqrt{a^2-b^2}}{a}$

$x = NcosB \tag{4-2}$

$y = N(1-e^2)sinB \tag{4-3}$

$\begin{cases} X = NcosBcosL\\ Y = NcosBsinL\\ Z = N(1-e^2)sinB\\ \end{cases} \tag{5}$

$\frac{x^2}{a^2} + \frac{x^2(1-e^2)^2tan^2B}{b^2} = 1$

$x = \frac{acosB}{\sqrt{1-e^2sin^2B}} \tag{6}$

$N = \frac{a}{\sqrt{1-e^2sin^2B}} \tag{6}$

P点在椭球面上的点为$$P_0$$，那么根据矢量相加的性质，有：

$P = P_0 + H \cdot n \tag{6}$

$n = \left[ \begin{matrix} cosBcosL \\ cosBsinL \\ sinB \\ \end{matrix} \right] \tag{7}$

$P = \left[ \begin{matrix} X \\ Y \\ Z \\ \end{matrix} \right]= \left[ \begin{matrix} (N+H)cosBcosL \\ (N+H)cosBsinL \\ [N(1-e^2) + H]sinB \\ \end{matrix} \right] \tag{8}$

$N = \frac{a}{\sqrt{1-e^2sin^2B}} \tag{9}$

## 2.2. XYZ->BLH

$\frac{Y}{X} = \frac{(N+H)cosBsinL}{(N+H)cosBcosL} = tanL$

$L = arctan(\frac{Y}{X}) \tag{10}$

$y = PQsinB$

$PQ = N(1-e^2)$

$Pn = N = PQ + Qn$

$Qn = Ne^2$

$\begin{cases} PP'' = Z\\ OP'' = \sqrt{x^2+y^2}\\ PP''' = OK_p = QK_psinB = Ne^2sinB\\ P''P''' = PP''' + PP'' \end{cases}$

$tanB = \frac{Z+Ne^2sinB}{\sqrt{x^2+y^2}} \tag{11}$

$H = \frac{Z}{sinB} - N(1-e^2) \tag{12}$

$\begin{cases} L = arctan(\frac{Y}{X})\\ tanB = \frac{Z+Ne^2sinB}{\sqrt{x^2+y^2}}\\ H = \frac{Z}{sinB} - N(1-e^2)\\ \end{cases}$

# 3. 实现

#include <iostream>

using namespace std;

const double epsilon = 0.000000000000001;
const double pi = 3.14159265358979323846;
const double d2r = pi / 180;
const double r2d = 180 / pi;

const double a = 6378137.0;		//椭球长半轴
const double f_inverse = 298.257223563;			//扁率倒数
const double b = a - a / f_inverse;
//const double b = 6356752.314245;			//椭球短半轴

const double e = sqrt(a * a - b * b) / a;

void Blh2Xyz(double &x, double &y, double &z)
{
double L = x * d2r;
double B = y * d2r;
double H = z;

double N = a / sqrt(1 - e * e * sin(B) * sin(B));
x = (N + H) * cos(B) * cos(L);
y = (N + H) * cos(B) * sin(L);
z = (N * (1 - e * e) + H) * sin(B);
}

void Xyz2Blh(double &x, double &y, double &z)
{
double tmpX =  x;
double temY = y ;
double temZ = z;

double curB = 0;
double N = 0;
double calB = atan2(temZ, sqrt(tmpX * tmpX + temY * temY));

int counter = 0;
while (abs(curB - calB) * r2d > epsilon  && counter < 25)
{
curB = calB;
N = a / sqrt(1 - e * e * sin(curB) * sin(curB));
calB = atan2(temZ + N * e * e * sin(curB), sqrt(tmpX * tmpX + temY * temY));
counter++;
}

x = atan2(temY, tmpX) * r2d;
y = curB * r2d;
z = temZ / sin(curB) - N * (1 - e * e);
}

int main()
{
double x = 113.6;
double y = 38.8;
double z = 100;

printf("原大地经纬度坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
Blh2Xyz(x, y, z);

printf("地心地固直角坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
Xyz2Blh(x, y, z);
printf("转回大地经纬度坐标：%.10lf\t%.10lf\t%.10lf\n", x, y, z);
}


# 4. 参考

posted @ 2021-08-29 14:08  charlee44  阅读(4635)  评论(0编辑  收藏  举报