一种双站交叉定位算法及其模拟

地球椭球及其切面方程定义

地球椭球X轴向半径定义为R，Y半轴长度和X半轴一致，地球椭球曲率e 。

地球椭球坐标系定义：
$$A{x}^2+B{x}^2+C{x}^2-1=0$$
其中：

$A=1/R^2$,
$B=1.0/R^2$,
$C=(R\ast (1-e))\ast (R\ast (1-e));$

地球表面某点的卯酉圈曲率半径为：

$$R_{\tau }=R/sqrt(1-e\ast sin(lat)\ast sin(lat))$$

在椭球上过某点$(x_i,y_i,z_i)$的切面方程：
$$A_{\tau }{x}_{i}x+B_{\tau }{y}_{i}y+C_{\tau }{z}_{i}z-1=0$$
其中:

$A_{\tau }=1/R_{\tau }^2$,
$B_{\tau }=1.0/R_{\tau }^2$,
$C_{\tau }=(R_{\tau }\ast (1-e))\ast (R_{\tau }\ast (1-e));$

则可以得到椭球的切面方程系数为：
$$N(A_{\tau }{x}_i,B_{\tau }{y}_i,C_{\tau }{z}_i,-1)=N(A_1,B_1,C_1,-1)$$
其中$(A_1,B_1,C_1)$为切面法向量。

算法原理概述

设观测站点的坐标位置为：$$P_1(x_1,y_1,z_1),P_2(x_2,y_2,z_2)$$

获得世界系下的测向线

首先我们在站点系下，站1获取到了该信号的来波方向，则该信号可在站点系下z=0的平面上映射一条射线出去，该射线被定义为测向线 ，将测向线转换到地心系，即可获得一个在地心坐标系下的测向射线$Lin{e}_1(P_1(x1,y1,z1),P_{c1}(x_{c1},y_{c1},z_{c1}))$ 。
站点系转地心系操作伪代码：

//将雷达的GPS转化为大地直角坐标
double Rx,Ry,Rz;
llh2xyz(lon, lat, hgt, Rx, Ry, Rz);

double lat	= DEG2RAD *(90-lat);
double lon	= DEG2RAD *(90+lon);

	//
double XX[9] = {1,	0,	0,
		0,	cos(lat) , -sin(lat),
		0,	sin(lat),  cos(lat)};

	//
	double ZZ[9] = {
	    cos(lon),  -sin(lon), 0,
		sin(lon),  cos(lon),  0,
		0,         0,          1};

//计算相对观测站世界系的位置

	double Rotate[9];
	metric_3x3(ZZ, XX, Rotate);
	double tmp[3];
	double xyz[3];
	xyz[0] = x;
	xyz[1] = y;
	xyz[2] = z;
	metric_33x31(Rotate,xyz,tmp);

	X = tmp[0] + Rx;
	Y = tmp[1] + Ry;
	Z = tmp[2] + Rz;

同理可以获得站点2的测向线$Lin{e}_2$

获得测向面

从地心引出一条射线经过站点，则该射线的向量为：$Lin{e}_{F_1}(x_1,y_1,z_1)$,且经过$P_1$点，通过$Line1$和$Lin{e}_{F_1}$即可确定一个测向面，$Line1$和$Lin{e}_{F_1}$经过叉乘可获得该测向面的法线向量$N_{F_1}$，并且已知该面经过$P_1$,则可得到一个测向面:

p l a n e 1 = { N F 1 , P 1 } plane1 = \{N_{F_1},P_1\} plane1={NF1​​,P1​}

3维坐标向量叉乘：

	n1为向量1, n2为向量2,n为叉乘结果
	
	n[0] = n1[1] * n2[2] -  n2[1] * n1[2];
	n[1] = n1[2] * n2[0] -  n2[2] * n1[0];
	n[2] = n1[0] * n2[1]  - n2[0] * n1[1];

同理可以获得测向面$plane2$
p l a n e 2 = { N F 2 , P 2 } plane2 = \{N_{F_2},P_2\} plane2={NF2​​,P2​}

原理目标定位

通过以上步骤获得了两个测向面，通过两个测向面进行相交并可以获得一个由地心引出的两个测向面交叉的射线$crossLine$，$crossLine$和两个平面向量都保持正交，所以该线的向量，仅需对两个面的法向量进行叉乘即可。

在原理上该射线处在同一经纬度，仅需找到线上的正方向一点即可确定目标的经纬度。

以下给出已知两个平面，求交叉线向量及交叉点的实现过程

 p1 ：平面1上的一点
 n1 ：平面1法向量
 p2 ：平面2上的一点
 n2 ：平面2法向量
 
 p  ：交线上的一点
 n  ：交线法向量
 
	double N1[3] = {0};
	double N2[3] = {0};

	//对向量进行归一化
	normal_arr(n1, 3, N1);
	normal_arr(n2, 3, N2);

	求解cross 得到交叉线向量
	n[0] = N1[1]*N2[2]-N2[1]*N1[2];
	n[1] = N1[2]*N2[0]-N2[2]*N1[0];
	n[2] = N1[0]*N2[1]-N2[0]*N1[1];

	//确定该射线上的一点
	double D1 = -(p1[0]*N2[0] + p1[1]*N2[1] + p1[2]*N1[2]); 
	double D2 = - (p2[0]*N1[0] + p2[1]*N2[1] + p2[2]*N2[2]);
	const double fn00 = 1.0;
	const double fn11 = 1.0;
	const double fn01 = n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2]; //点积
	const double det = fn00*fn11 - fn01*fn01;

	if (fabs(det) < EPSELION )
		return false;

	const double invdet = 1.0 / det;
	const double fc0 = (fn11*-D1 + fn01*D2) * invdet;
	const double fc1 = (fn00*-D2 + fn01*D1) * invdet;

	p[0] = N2[0] * fc0 + N1[0] * fc1;
	p[1] = N2[1] * fc0 + N1[1] * fc1;
	p[2] = N1[2] * fc0 + N2[2] * fc1;

	return true;

测角误差处理

对测角误差的信号可通过一定的滤波方式进行处理，如通过观测站引出射线到定位点，累计多个这样的射线，并将这些射线统一到某Z=0的平面上，如椭球面的某一切面上（在文章前面已经给出了椭球切面方程），或者任何其他不与测向面都不平行的面（如与世界系平行的某一局部坐标系等），或者到站点系下的局部坐标系，再进行最小二乘交点估计，获得一个（x，y）值，将该坐标逆算回世界坐标系，即可获得更精确的目标位置。

由多次测向线和最小二乘理论，可以建立直线方 程组如下：
$$Y＝ＨＸ＋ｖ$$
$$H=\left[\begin{array}{cc}K1& -1\\ K2& -1\\ ...& \\ Kn& -1\end{array}\right]\text{\hspace{1em}(3)}$$
$$X=\left[\begin{array}{c}x\\ y\end{array}\right]\text{\hspace{1em}(3)}$$$$Y=\left[\begin{array}{c}{K}_1\ast x1-y1\\ K_2\ast x2-y2\\ ...\\ K_n\ast x_n-y_n\end{array}\right]\text{\hspace{1em}(3)}$$
对于目标位置的估计为：
$$X_g=(H^{T}H{)}^{-1}{H}^{T}Y$$

代码实现流程：

    MatTrans(&H, &H_T);                   //转置
	MatMul(&H_T, &H, &T1);			 //相乘
	MatInv(&T1, &T2);						 //求逆
	MatMul(&T2, &H_T, &T3);			 //相乘
	MatMul(&T3, &Y, &X);

	pos.lon = X.element[0][0];
	pos.lat = X.element[1][0];

	//数值是否有效
	bool nan = !isnan(X.element[0][0]) && !isnan(X.element[1][0]);
	bool localvalid = nan && (pos.lon >= -180 && pos.lon <= 180) &&
		(pos.lat >= -90 && pos.lat <= 90);

设置场景

基站1的位置定位在（120，34）度，与基线垂直的方向为70度，基线长度10Km，测角误差为$\pm 1$度
最终得到360度的定位误差为：

从图中可以看出，70度和250度左右误差值最小，此时目标在基线的正前方和后方，在160度和340度有误差峰值，是因为目标与两个基站在同一条基线上。