计算两个 GPS 坐标与高度之间的垂直方位

Posted 2023-04-14

【中文标题】计算两个 GPS 坐标与高度之间的垂直方位

【英文标题】：Calculate vertical bearing between two GPS coordinates with altitudes

【发布时间】：2017-05-23 09:16:19

【问题描述】：

我正计划构建一个天线跟踪器。我需要从具有高度的 GPS 点 A 和具有高度的 GPS 点 B 获取方位和倾斜。

这是示例点：

latA = 39.099912
lonA = -94.581213
altA = 273.543
latB = 38.627089
lonB = -90.200203
altB = 1380.245

我已经得到了水平轴承的公式，它给了我 97.89138167122422

这是代码：

function toRadian(num) 
    return num * (Math.PI / 180);


function toDegree(num) 
    return num * (180 / Math.PI);


function getHorizontalBearing(fromLat, fromLon, toLat, toLon) 
    fromLat = toRadian(fromLat);
    fromLon = toRadian(fromLon);
    toLat = toRadian(toLat);
    toLon = toRadian(toLon);

    let dLon = toLon - fromLon;
    let x = Math.tan(toLat / 2 + Math.PI / 4);
    let y = Math.tan(fromLat / 2 + Math.PI / 4);
    let dPhi = Math.log(x / y);
    if (Math.abs(dLon) > Math.PI) 
        if (dLon > 0.0) 
            dLon = -(2 * Math.PI - dLon);
         else 
            dLon = (2 * Math.PI + dLon);
        
    

    return (toDegree(Math.atan2(dLon, dPhi)) + 360) % 360;


let n = getHorizontalBearing(39.099912, -94.581213, 38.627089, -90.200203);
console.info(n);

但我不知道如何找到倾斜角度。谁能帮帮我？

【参考方案1】：

我想我在搜索后得到了答案。

这是完整的代码，如果你认为这是错误的，请随时纠正我。

function toRadian(num) 
    return num * (Math.PI / 180);


function toDegree(num) 
    return num * (180 / Math.PI);


// North is 0 degree, South is 180 degree
function getHorizontalBearing(fromLat, fromLon, toLat, toLon, currentBearing) 
    fromLat = toRadian(fromLat);
    fromLon = toRadian(fromLon);
    toLat = toRadian(toLat);
    toLon = toRadian(toLon);

    let dLon = toLon - fromLon;
    let x = Math.tan(toLat / 2 + Math.PI / 4);
    let y = Math.tan(fromLat / 2 + Math.PI / 4);
    let dPhi = Math.log(x / y);
    if (Math.abs(dLon) > Math.PI) 
        if (dLon > 0.0) 
            dLon = -(2 * Math.PI - dLon);
         else 
            dLon = (2 * Math.PI + dLon);
        
    

    let targetBearing = (toDegree(Math.atan2(dLon, dPhi)) + 360) % 360;
    return targetBearing - currentBearing;


// Horizon is 0 degree, Up is 90 degree
function getVerticalBearing(fromLat, fromLon, fromAlt, toLat, toLon, toAlt, currentElevation) 
    fromLat = toRadian(fromLat);
    fromLon = toRadian(fromLon);
    toLat = toRadian(toLat);
    toLon = toRadian(toLon);

    let fromECEF = getECEF(fromLat, fromLon, fromAlt);
    let toECEF = getECEF(toLat, toLon, toAlt);
    let deltaECEF = getDeltaECEF(fromECEF, toECEF);

    let d = (fromECEF[0] * deltaECEF[0] + fromECEF[1] * deltaECEF[1] + fromECEF[2] * deltaECEF[2]);
    let a = ((fromECEF[0] * fromECEF[0]) + (fromECEF[1] * fromECEF[1]) + (fromECEF[2] * fromECEF[2]));
    let b = ((deltaECEF[0] * deltaECEF[0]) + (deltaECEF[2] * deltaECEF[2]) + (deltaECEF[2] * deltaECEF[2]));
    let elevation = toDegree(Math.acos(d / Math.sqrt(a * b)));
    elevation = 90 - elevation;

    return elevation - currentElevation;


function getDeltaECEF(from, to) 
    let X = to[0] - from[0];
    let Y = to[1] - from[1];
    let Z = to[2] - from[2];

    return [X, Y, Z];


function getECEF(lat, lon, alt) 
    let radius = 6378137;
    let flatteningDenom = 298.257223563;
    let flattening = 0.003352811;
    let polarRadius = 6356752.312106893;

    let asqr = radius * radius;
    let bsqr = polarRadius * polarRadius;
    let e = Math.sqrt((asqr-bsqr)/asqr);
    // let eprime = Math.sqrt((asqr-bsqr)/bsqr);

    let N = getN(radius, e, lat);
    let ratio = (bsqr / asqr);

    let X = (N + alt) * Math.cos(lat) * Math.cos(lon);
    let Y = (N + alt) * Math.cos(lat) * Math.sin(lon);
    let Z = (ratio * N + alt) * Math.sin(lat);

    return [X, Y, Z];


function getN(a, e, latitude) 
    let sinlatitude = Math.sin(latitude);
    let denom = Math.sqrt(1 - e * e * sinlatitude * sinlatitude);
    return a / denom;


let n = getHorizontalBearing(39.099912, -94.581213, 39.099912, -94.588032, 0.00);
console.info("Horizontal bearing:\t", n);

let m = getVerticalBearing(39.099912, -94.581213, 273.543, 39.099912, -94.588032, 873.543, 0.0);
console.info("Vertical bearing:\t", m);

【参考方案2】：

Don Cross 的javascript code 产生了不错的效果。它考虑了地球的曲率加上地球是oblate这一事实。

例子：

var elDegrees = calculateElevationAngleCosineKitty(
    latitude: 35.346257, longitude: -97.863801, altitudeMetres: 10,
    latitude: 34.450545, longitude: -96.500167, altitudeMetres: 9873
);
console.log("El: " + elDegrees);

/***********************************
Code by Don Cross at cosinekitty.com
http://cosinekitty.com/compass.html
************************************/
    function calculateElevationAngleCosineKitty(source, target)
    
        var oblate = true;

        var a = 'lat':source.latitude, 'lon':source.longitude, 'elv':source.altitudeMetres;
        var b = 'lat':target.latitude, 'lon':target.longitude, 'elv':target.altitudeMetres;

        var ap = LocationToPoint(a, oblate);
        var bp = LocationToPoint(b, oblate);

        var bma = NormalizeVectorDiff(bp, ap);
        var elevation = 90.0 - (180.0 / Math.PI)*Math.acos(bma.x*ap.nx + bma.y*ap.ny + bma.z*ap.nz);
        return elevation;
    

    function NormalizeVectorDiff(b, a)
    
        // Calculate norm(b-a), where norm divides a vector by its length to produce a unit vector.
        var dx = b.x - a.x;
        var dy = b.y - a.y;
        var dz = b.z - a.z;
        var dist2 = dx*dx + dy*dy + dz*dz;
        if (dist2 == 0) 
            return null;
        
        var dist = Math.sqrt(dist2);
        return  'x':(dx/dist), 'y':(dy/dist), 'z':(dz/dist), 'radius':1.0 ;
    

    function EarthRadiusInMeters (latitudeRadians)      // latitude is geodetic, i.e. that reported by GPS
    
        // http://en.wikipedia.org/wiki/Earth_radius
        var a = 6378137.0;  // equatorial radius in meters
        var b = 6356752.3;  // polar radius in meters
        var cos = Math.cos (latitudeRadians);
        var sin = Math.sin (latitudeRadians);
        var t1 = a * a * cos;
        var t2 = b * b * sin;
        var t3 = a * cos;
        var t4 = b * sin;
        return Math.sqrt ((t1*t1 + t2*t2) / (t3*t3 + t4*t4));
    

    function GeocentricLatitude(lat)
    
        // Convert geodetic latitude 'lat' to a geocentric latitude 'clat'.
        // Geodetic latitude is the latitude as given by GPS.
        // Geocentric latitude is the angle measured from center of Earth between a point and the equator.
        // https://en.wikipedia.org/wiki/Latitude#Geocentric_latitude
        var e2 = 0.00669437999014;
        var clat = Math.atan((1.0 - e2) * Math.tan(lat));
        return clat;
    

    function LocationToPoint(c, oblate)
    
        // Convert (lat, lon, elv) to (x, y, z).
        var lat = c.lat * Math.PI / 180.0;
        var lon = c.lon * Math.PI / 180.0;
        var radius = oblate ? EarthRadiusInMeters(lat) : 6371009;
        var clat   = oblate ? GeocentricLatitude(lat)  : lat;

        var cosLon = Math.cos(lon);
        var sinLon = Math.sin(lon);
        var cosLat = Math.cos(clat);
        var sinLat = Math.sin(clat);
        var x = radius * cosLon * cosLat;
        var y = radius * sinLon * cosLat;
        var z = radius * sinLat;

        // We used geocentric latitude to calculate (x,y,z) on the Earth's ellipsoid.
        // Now we use geodetic latitude to calculate normal vector from the surface, to correct for elevation.
        var cosGlat = Math.cos(lat);
        var sinGlat = Math.sin(lat);

        var nx = cosGlat * cosLon;
        var ny = cosGlat * sinLon;
        var nz = sinGlat;

        x += c.elv * nx;
        y += c.elv * ny;
        z += c.elv * nz;

        return 'x':x, 'y':y, 'z':z, 'radius':radius, 'nx':nx, 'ny':ny, 'nz':nz;
    
/***********************
END cosinekitty.com code
************************/