寫在前面

之前自己寫的word丟了，為避免丟失，在簡書上發(fā)一下，主要是備忘，有些表達(dá)不嚴(yán)謹(jǐn)請，見諒。

方法和模型圖片來自引文:張靜.杜劍平.蔣俊,基于球體模型的短波固定多站交叉定位選站方法[j].信息工程大學(xué)學(xué)報,2020,(1),9-14 26

再吐槽知網(wǎng):下個論文收費3.5，表示理解；充值最小30，每次下載都要收一遍，手機(jī)app上藏得老深了，我就想要張圖，有這功夫自己都畫出來了。

另外，電腦沒在身邊，手機(jī)碼字，寫公式不易，轉(zhuǎn)發(fā)引用請注明出處。

計算模型

當(dāng)目標(biāo)與側(cè)向站不在同一平面時，側(cè)向交叉定位必須考慮地球曲率的影響，將地球看成球體，半徑,考慮到側(cè)向誤差遠(yuǎn)大于地球橢球偏心率影響，簡化計算把地球看成正球。此時，側(cè)向交叉定位如圖1所示。

圖1 基于球體模型的側(cè)向交叉定位示意圖

圖中為輻射源;,為側(cè)向站，為對的側(cè)向角真實值，為測量值。
為便于描述觀察模型，建立以球形為原點的空間大地坐標(biāo)系，用緯度，經(jīng)度和大地高來表示空間位置，如圖2所示。

圖2 空間大地坐標(biāo)系示意圖

在空間大地坐標(biāo)系中，的坐標(biāo)為,兩點與北極點形成球面三角形，以表示球面三角形大圓弧，以球面角表示球面三角形中的三個角,則由球面三角形的余切定理可得: (1)
由經(jīng)緯度的定義可知:
(2)
由側(cè)向角的定義可知:
(3)

求解過程

將式(2)(3)代入(1)得:
(4)
即:
(5)
和差化積:
(6)
令已知數(shù): (7)
令未知數(shù): (8)
將(7)(8)代入(6)簡化后得:
(9)
令已知數(shù):
(10)
將(10)代入(9)簡化后:
(11)
已知兩個側(cè)向站的坐標(biāo)和基本三角公式可聯(lián)立方程組:
(12)
令已知數(shù):
(13)
如果,輻射源經(jīng)度為0或180度，或者3點在同一經(jīng)線圓上。
得到2組經(jīng)度解:
(14)
令已知數(shù):
(15)
由(14)代入可知B有2個值，已知數(shù)，也可以使用1號或者2號側(cè)向站代入計算，避免,如果,代表在兩極。
得到2組緯度解:
(16)
對應(yīng)每個有2個解，共4組解。
分別對使用atan2函數(shù)計算經(jīng)緯度得到4組經(jīng)緯度坐標(biāo)，其中兩組緯度坐標(biāo)不在范圍內(nèi)，剔除后得到兩組坐標(biāo)，是球面上的過心對稱點。
是用Haversin函數(shù)，分別求兩個側(cè)向站到兩組坐標(biāo)的距離，得到4個值，其中最小值就是目標(biāo)點到其中一個站的最小距離，對應(yīng)的坐標(biāo)就是最終目標(biāo)點的坐標(biāo)。

Python 測試代碼

import numpy as np
import math

def get_lmn(s):
    lon,lat,az = s
    a,b,c,d,e,f = np.sin(lon),np.cos(lon),np.sin(lat),np.cos(lat),np.sin(az),np.cos(az)
    return (e*d,e*c*b-f*a,e*c*a+f*b)

def cov2cood(sincos):
    claCoord = lambda scsc:(math.atan2(scsc[2],scsc[3]),math.atan2(scsc[0],scsc[1]))
    result = [claCoord(sincos[0]),claCoord(sincos[1]),claCoord(sincos[2]),claCoord(sincos[3])]
    return np.rad2deg(result)


#在s點在p1,p2 的連線上，沒有處理
def cross_location(s1,s2):
    '''
    目標(biāo)點為S(xlon,xlat)
    點P(lon,lat,az)與S的關(guān)系(1)
    (1):sin(az)/cos(az) = cos(xlat)sin(xlon-lon)/[cos(lat)sin(xlat)-sin(lat)cos(xlat)cos(xlon-lon)]
    令x,y,u,v = sin(xlon),cos(xlon),sin(xlat),cos(xlat)
    (1)可將簡化為(2):l*u=m*vy+n*vx
    兩點分別帶入(2),得到2個方程
    加上(3):u^2+v^2=1，(4):x^2+y^2=1,共4個方程形成的4元2次方程組
    令 a = (l1*n2-l2*n1)/(l2*m1-l1*m2)
    x=±√(1/(a^2+1)),y=ax
    令 b = (m1*y+n1*x)/l2,換成m2,n2,l2也可以
    v=±√(1/(b^2+1)),u=bv
    得到4組(x,y,u,v)
    換算成經(jīng)緯角(arctan2(x,y),arctan2(u,v)) =>(xlon,xlat) 
    排除xlat < -pi/2 ,或 xlat > pi/2 ,只剩兩組坐標(biāo)
    這兩個點是過心對稱點
    '''
    s1 = np.deg2rad(s1)
    s2 = np.deg2rad(s2)
    l1,m1,n1 = get_lmn(s1)
    l2,m2,n2= get_lmn(s2)
    if (l2*m1-l1*m2) != 0:
        A = (l1*n2-l2*n1)/(l2*m1-l1*m2) 
        X = np.array([np.sqrt(1/(A**2+1)),-np.sqrt(1/(A**2+1))])
        Y = A * X
    else:
        X = np.array([0,0])
        Y = np.array([1,-1])
    #防止除0錯誤
    if l1!=0 or l2!=0:
        L,M,N = l1,m1,n1
        if l1 == 0:
            L,M,N = l2,m2,n2
        calb = lambda x,y:(M*y+N*x)/L
        b0 = calb(X[0],Y[0])
        b1 = calb(X[1],Y[1])
        V0 = np.array([np.sqrt(1/(b0**2+1)),-np.sqrt(1/(b0**2+1))])
        U0 = V0*b0
        V1 = np.array([np.sqrt(1/(b1**2+1)),-np.sqrt(1/(b1**2+1))])
        U1 = V1*b1
    else:
        # cos(lat)不會為0 ，不然就az就沒有意義只有 sin(az)==0 即az均為 =0 或 180，指向極點 
        U0=[1,1]
        U1=[-1,1]
        V0=[0,-0.1]
        V1=[0,-0.1] 
    result = [(U0[0],V0[0],X[0],Y[0]),(U0[1],V0[1],X[0],Y[0]),(U1[0],V1[0],X[1],Y[1]),(U1[1],V1[1],X[1],Y[1])]
    result = cov2cood(result)
    frsl = []

    for i in range(4):
        lat = result[i][1] 
        if lat>= -90 and lat <=90:
            frsl.append(result[i])
    return np.array(frsl)

def distance_haversine(p1,p2,r=1):
    '''
    hav(x) = sin(x/2)^2 = (1-cos(x))/2
    a(alpha) 兩點過心角
    hav(a) = hav(lat1-lat2)+cos(lat1)*cos(lat2)*hav(lon1-lon2)
    input:
    p1:[lon,lat] in degree
    p2:[lon,lat] in degree
    r:球半徑
    return:球面距離
    '''
    p1=np.deg2rad(p1)
    p2=np.deg2rad(p2)
    hav_lon = math.sin((p1[0]-p2[0])/2)**2
    hav_lat = math.sin((p1[1]-p2[1])/2)**2
    hav_a = hav_lat + math.cos(p1[1])*math.cos(p2[1])*hav_lon
    a = 2*math.atan2(math.sqrt(hav_a),math.sqrt(1-hav_a))
    return a*r

def distance_greate_circle(p1,p2,r=1):
    '''
    使用弦長計算角,求弧長
    '''
    dpp = distance_chord_line(p1,p2)
    #余弦定理求角
    a = math.acos((2-dpp**2)/2)
    return r*a
    

def distance_chord_line(p1,p2,r=1):
    '''
    計算弦長
    x= cos(lat)sin(lon)
    y= cos(lat)cos(lon)
    z= sin(lat)
    '''
    to_xyz = lambda lon,lat:(math.cos(lat)*math.sin(lon),math.cos(lat)*math.cos(lon),math.sin(lat))
    p1=np.deg2rad(p1)
    p2=np.deg2rad(p2)
    p1 = to_xyz(p1[0],p1[1])
    p2 = to_xyz(p2[0],p2[1])
    d = (p1[0]-p2[0])**2+(p1[1]-p2[1])**2+(p1[2]-p2[2])**2
    d = math.sqrt(d)
    return r*d

def pick_nearest_root(s1,s2,roots):
    '''
    選出離側(cè)向站最近的點
    '''
    nroot = len(roots)
    adist = []
    for i in range(nroot):
        adist.append(distance_chord_line(s1,roots[i]))
        adist.append(distance_chord_line(s2,roots[i]))
    minv = math.pi*2
    min_i = 0
    for i in range(len(adist))  :
        if minv > adist[i] :
            minv = adist[i]
            min_i = i
    return roots[min_i//2]

def cross_location_nearest(s1,s2):
    roots = cross_location(s1,s2)
    return pick_nearest_root(s1[:2],s2[:2],roots)

if __name__ == "__main__":
    s1,s2 = [61.1,32.2,120],[52,28.1,33]
    print("s1[lon,lat,az]  ---------------------------\n",s1)
    print("s2[lon,lat,az]  ---------------------------\n",s2)
    locs = cross_location(s1,s2)
    print("roots of equation set[lon,lat]  -----------\n",locs)
    loc=cross_location_nearest(s1,s2)
    print("nearest loaction[lon,lat]  ----------------\n",loc)
    print("dist to s1  ---------------------------\n",distance_haversine(s1[:2],loc))
    print("dist to s2  ---------------------------\n",distance_haversine(s2[:2],loc))