Java 使用AR计算设备位置

首先，我知道开始AR会话时的位置。 因此，我在一个点与我的lat-lon；我走了几米，我想我的位置在B点不使用GPS或其他技术，只使用ARCore。 我试着用姿势来表示角度和距离，但我有一个很大的错误

在做了20米的路径后，误差是5.3815米。 我不知道这是正常的还是我的代码中有一些错误

initialBearingAR=LocationScene.deviceOrientation.aziration

所以这是使用传感器计算的方向

double distanceAR = Math.sqrt(Math.pow((camPose1.tx() - camPose2.tx()),2)/ 2.0 +
                     Math.pow((camPose1.ty() - camPose2.ty()),2) / 2.0 +
                     Math.pow((camPose1.tz() - camPose2.tz()),2) / 2.0);

camPose1是在点A处计算的姿势，camPose2是在点B处计算的姿势。两者都是相机的姿势

double[] latLonEnd = travel(latStartAR, lonStartAR, initialBearingAR, distanceAR);

latEndAR=latLonEnd[0]；lonEndAR=latLonEnd[1]

旅行方式：

 public double[] travel(double qrLat, double qrLon , double initialBearing, double distance) {
        double bR = Math.toRadians(initialBearing);
        distance = distance/1000;
        double lat1R = Math.toRadians(qrLat);
        double lon1R = Math.toRadians(qrLon);
        double dR = distance/6371.01; //Mettere diviso radius earth

        double a = Math.sin(dR) * Math.cos(lat1R);
        double lat2 = Math.asin(Math.sin(lat1R) * Math.cos(dR) + a * Math.cos(bR));
        double lon2 = lon1R
                + Math.atan2(Math.sin(bR) * a, Math.cos(dR) - Math.sin(lat1R) * Math.sin(lat2));
        double[] latlon = new double[2];
        latlon[0] = Math.toDegrees(lat2);
        latlon[1] = Math.toDegrees(lon2);
        return latlon;
    }

还有其他想法吗？
你能分享你是如何计算的以及误差有多大的详细信息吗？我现在已经把它和我的代码一起放在了问题中