XdCxRhDW/XdCxRhDW.Api/AddIns/定位/PosApi.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using XdCxRhDW.Entity;
using XdCxRhDW.Framework;

namespace XdCxRhDW.Api
{
    /// <summary>
    ///TODO 定位算法接口.在这里调用罗博士的算法库
    /// </summary>
    public static class PosApi
    {
        //置信度统计次数
        private static int confidenceCount = 100;

        //置信度有效距离(m)
        private static int confidenceDistance = 10000;

        #region cpp dll Interop 

        //一星一地测向带参定位
        private const string XDCX = @"AddIns\定位\DLL_DTO_DOA_DW.dll";

        //三星双时差带参、三星双时差无参、三星双频差带参、双星时频差带参、两星一地无参定位及时差线、一星两地
        private const string OtherPos = @"AddIns\定位\Position-New.dll";//DLL_11J_DW

        [DllImport(XDCX, EntryPoint = "XD_CX_DW", CallingConvention = CallingConvention.Cdecl)]//一星一地测向带参
        private extern static void X1D1_Pos20240305_Core(double[] mainSat, double[] satStation, double[] cdbStation, double[] cxStation, double[] refStation, double[] zone, double theta, double tarDto, double refDto, double[] res);

        [DllImport(OtherPos, EntryPoint = "SC_2X1D_DW", CallingConvention = CallingConvention.Cdecl)]//两星一地带参
        private extern static void X2D1_Pos20240305_Core(double[] mainSat, double[] adjaSat, double[] cdbStation, double[] satStation1, double[] satStation2, double[] satStation3, double[] satStation4,
             double[] satStation5, double[] refStation, double[] zone, double tarSxDto, double tarXdDto, double samp_main_dto, double samp_neigh_dto, double[] res);

        [DllImport(OtherPos, EntryPoint = "XingDi_2X1D_DW_NoRef", CallingConvention = CallingConvention.Cdecl)]//两星一地无参
        private extern static void X2D1_PosNoRef20240305_Core(double[] mainSat, double[] adjaSat, double[] cdbStation, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
            , double[] zone, double tarSxDto, double tarXdDto, double[] res);

        [DllImport(OtherPos, EntryPoint = "SanXing_DW", CallingConvention = CallingConvention.Cdecl)]//三星双时差带参
        private extern static void X3_Pos20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
            , double[] mainSatRefStation, double[] adjaSat1RefStation, double[] adjaSat2RefStation, double[] refStation, double[] zone, double tarDto1, double tarDto2, double refDto1, double refDto2, double[] res);

        [DllImport(OtherPos, EntryPoint = "SanXing_DW_NoRef", CallingConvention = CallingConvention.Cdecl)]//三星双时差无参
        private extern static void X3_PosNoRef20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
           , double[] zone, double tarDto1, double tarDto2, double[] res);

        [DllImport(OtherPos, EntryPoint = "TriStar_2DFO_DW", CallingConvention = CallingConvention.Cdecl)]//三星双频差带参
        private extern static void X3_PosTwoDfo20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
          , double[] refStation, double[] zone, double tarDfo1, double tarDfo2, double refDfo1, double refDfo2, double fu1, double fd1, double fu2, double fd2, double[] res);

        [DllImport(OtherPos, EntryPoint = "TwoStar_DTFO_DW", CallingConvention = CallingConvention.Cdecl)]//双星时频差带参
        private extern static void X2_Pos20240305_Core(double[] mainSat, double[] adjaSat, double[] mainSatTarStation, double[] adjaSatTarStation, double[] refStation, double[] zone
            , double tarDto, double tarDfo, double refDto, double refDfo, double fu1, double fd1, double fu2, double fd2, double[] res);



        #endregion

        /// <summary>
        /// 一星一地带参,返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="cxRes">测向结果</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X1D1_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double[] cxStation = new double[3] { sRes.CxLon.Value, sRes.CxLat.Value, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dtoCdb = cgRes.DtoCdb.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double theta = cxRes.Fx;//单位°
            double[] res = new double[6];
            var ybDto1 = cgRes.YbMainDto.Value / 1e6;
            X1D1_Pos20240305_Core(mainSat, satStation, cdbStation, cxStation, refStation, zone, theta, dtoCdb, ybDto1, res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)
                    };
                    var dtoCdbNew = (dtoCdb * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var ybDtoNew = (ybDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var thetaNew = theta + RandomHelper.Normal(0, 0.4);//单位°
                    X1D1_Pos20240305_Core(mainSatNew, satStation, cdbStation, cxStation, refStation, zone, thetaNew, dtoCdbNew, ybDtoNew, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 两星一地带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X2D1_Pos(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {

            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dto1 = cgRes.Dto1.Value / 1e6;
            double dtoCdb = cgRes.DtoCdb.Value / 1e6;
            double ybDto1 = cgRes.YbMainDto.Value / 1e6;
            double ybDto2 = cgRes.YbAdja1Dto.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            X2D1_Pos20240305_Core(mainSat, adjaSat, cdbStation, satStation, satStation, satStation, satStation, satStation, refStation, zone, dto1, dtoCdb, ybDto1, ybDto2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSatNew = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var dto1New = (dto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var dtoCdbNew = (dtoCdb * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var ybDto1New = (ybDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var ybDto2New = (ybDto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    X2D1_Pos20240305_Core(mainSatNew, adjaSatNew, cdbStation, satStation, satStation, satStation, satStation, satStation, refStation, zone, dto1New, dtoCdbNew, ybDto1New, ybDto2New, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 两星一地无参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X2D1_PosNoRef(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double dto1 = cgRes.Dto1.Value / 1e6;
            double dtoCdb = cgRes.DtoCdb.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            X2D1_PosNoRef20240305_Core(mainSat, adjaSat, cdbStation, satStation, satStation, satStation, zone, dto1, dtoCdb, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSatNew = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var dto1New = (dto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var dtoCdbNew = (dtoCdb * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    X2D1_PosNoRef20240305_Core(mainSatNew, adjaSatNew, cdbStation, satStation, satStation, satStation, zone, dto1New, dtoCdbNew, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 融合定位带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="cxRes">测向结果</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] RH_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes, bool CalcConfidence = false)
        {
            var res1 = X1D1_Pos(cgRes, sRes, cxRes, CalcConfidence);
            var res2 = X2D1_Pos(cgRes, sRes, CalcConfidence);
            double[] res = new double[] { 999, 999, 0, 999, 999, 0, 100 };
            var p1 = res1.Take(3).ToArray();
            var p2 = res1.Skip(3).ToArray();
            var p3 = res2.Take(3).ToArray();
            var p4 = res2.Skip(3).ToArray();
            if (IsGeoPoint(p1) && IsGeoPoint(p2) && IsGeoPoint(p3) && IsGeoPoint(p4))
            {
                res = new double[7] {
                    (p1[0] + p3[0]) / 2 + 0.003,
                    (p1[1] + p3[1]) / 2 - 0.002,
                    0,
                    (p2[0] + p4[0]) / 2 + 0.003,
                    (p2[1] + p4[1]) / 2 - 0.002,
                    0,
                    (res1[6]+res2[6])/2
                };
            }
            else if (IsGeoPoint(p1) && IsGeoPoint(p3))
            {
                res = new double[7] {
                    (p1[0] + p3[0]) / 2 + 0.003,
                    (p1[1] + p3[1]) / 2 - 0.002,
                    0,
                    999,
                    999,
                    0,
                   (res1[6]+res2[6])/2
                };
            }
            else if (IsGeoPoint(p1) && IsGeoPoint(p2))
            {
                res = new double[7]
                {
                    p1[0] + 0.003,
                    p1[1] - 0.002,
                    0,
                    p2[0] + 0.003,
                    p2[1] - 0.002,
                    0,
                    (res1[6]+res2[6])/2
                };
            }
            else if (IsGeoPoint(p3) && IsGeoPoint(p4))
            {
                res = new double[7]
               {
                    p3[0] + 0.003,
                    p3[1] - 0.002,
                    0,
                    p4[0] + 0.003,
                    p4[1] - 0.002,
                    0,
                    (res1[6]+res2[6])/2
               };
            }
            return res;
        }

        /// <summary>
        /// 三星双时差带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X3_Pos(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat1 = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] adjaSat2 = new double[3] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto1 = cgRes.Dto1.Value / 1e6;
            var tarDto2 = cgRes.Dto2.Value / 1e6;
            var refDto1 = (cgRes.YbMainDto.Value - cgRes.YbAdja1Dto.Value) / 1e6;
            var refDto2 = (cgRes.YbMainDto.Value - cgRes.YbAdja2Dto.Value) / 1e6;
            double[] res = new double[6];
            X3_Pos20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, satStation,
                satStation, satStation, refStation, zone, tarDto1, tarDto2, refDto1, refDto2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSat1New = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var adjaSat2New = new double[3]
                    {
                        cgRes.Adja2X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Z.Value+RandomHelper.Normal(0,200)
                    };
                    var tarDto1New = (tarDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var tarDto2New = (tarDto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var refDto1New = (refDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var refDto2New = (refDto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;

                    X3_Pos20240305_Core(mainSatNew, adjaSat1New, adjaSat2New, satStation, satStation, satStation, satStation, satStation, satStation, refStation, zone, tarDto1New, tarDto2New, refDto1New, refDto2New, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 三星双时差无参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X3_PosNoRef(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat1 = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] adjaSat2 = new double[3] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto1 = cgRes.Dto1.Value / 1e6;
            var tarDto2 = cgRes.Dto2.Value / 1e6;
            double[] res = new double[6];
            X3_PosNoRef20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, zone, tarDto1, tarDto2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSat1New = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var adjaSat2New = new double[3]
                    {
                        cgRes.Adja2X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Z.Value+RandomHelper.Normal(0,200)
                    };
                    var tarDto1New = (tarDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var tarDto2New = (tarDto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;

                    X3_PosNoRef20240305_Core(mainSatNew, adjaSat1New, adjaSat2New, satStation, satStation, satStation, zone, tarDto1New, tarDto2New, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 三星双频差带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X3_PosTwoDfo(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[6] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value, cgRes.MainVx.Value, cgRes.MainVy.Value, cgRes.MainVz.Value };
            double[] adjaSat1 = new double[6] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value, cgRes.Adja1Vx.Value, cgRes.Adja1Vy.Value, cgRes.Adja1Vz.Value };
            double[] adjaSat2 = new double[6] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value, cgRes.Adja2Vx.Value, cgRes.Adja2Vy.Value, cgRes.Adja2Vz.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDfo1 = cgRes.Dfo1.Value;
            var tarDfo2 = cgRes.Dfo2.Value;
            var refDfo1 = cgRes.YbMainDfo.Value - cgRes.YbAdja1Dfo.Value;
            var refDfo2 = cgRes.YbMainDfo.Value - cgRes.YbAdja2Dfo.Value;
            double fu1 = cgRes.TarFreqUp.Value;
            double fd1 = cgRes.TarFreqDown.Value;
            double fu2 = cgRes.RefFreqUp.Value;
            double fd2 = cgRes.RefFreqDown.Value;
            double[] res = new double[6];
            X3_PosTwoDfo20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation,
                refStation, zone, tarDfo1, tarDfo2, refDfo1, refDfo2, fu1, fd1, fu2, fd2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSat1New = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var adjaSat2New = new double[3]
                    {
                        cgRes.Adja2X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja2Z.Value+RandomHelper.Normal(0,200)
                    };
                    var tarDfo1New = (tarDfo1 * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;
                    var tarDfo2New = (tarDfo2 * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;
                    var refDfo1New = (refDfo1 * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;
                    var refDfo2New = (refDfo2 * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;

                    X3_PosTwoDfo20240305_Core(mainSatNew, adjaSat1New, adjaSat2New, satStation, satStation, satStation, refStation, zone, tarDfo1New, tarDfo2New, refDfo1New, refDfo2New, fu1, fd1, fu2, fd2, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        /// <summary>
        /// 双星时频差带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X2_PosDtoDfo(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            double[] mainSat = new double[6] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value, cgRes.MainVx.Value, cgRes.MainVy.Value, cgRes.MainVz.Value };
            double[] adjaSat = new double[6] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value, cgRes.Adja1Vx.Value, cgRes.Adja1Vy.Value, cgRes.Adja1Vz.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto = cgRes.Dto1.Value / 1e6;
            var tarDfo = cgRes.Dfo1.Value;
            var refDto = (cgRes.YbMainDto.Value - cgRes.YbAdja1Dto.Value) / 1e6;
            var refDfo = cgRes.YbMainDfo.Value - cgRes.YbAdja1Dfo.Value;
            double fu1 = cgRes.TarFreqUp.Value;
            double fd1 = cgRes.TarFreqDown.Value;
            double fu2 = cgRes.RefFreqUp.Value;
            double fd2 = cgRes.RefFreqDown.Value;
            double[] res = new double[6];
            X2_Pos20240305_Core(mainSat, adjaSat, satStation, satStation, refStation, zone, tarDto, tarDfo, refDto, refDfo, fu1, fd1, fu2, fd2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX.Value+RandomHelper.Normal(0,200),
                        cgRes.MainY.Value+RandomHelper.Normal(0,200),
                        cgRes.MainZ.Value+RandomHelper.Normal(0,200)

                    };
                    var adjaSatNew = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var tarDtoNew = (tarDto * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var refDtoNew = (refDto * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var tarDfoNew = (tarDfo * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;
                    var refDfoNew = (refDfo * 1e6 + RandomHelper.Normal(0, 5)) / 1e6;

                    X2_Pos20240305_Core(mainSatNew, adjaSatNew, satStation, satStation, refStation, zone, tarDtoNew, tarDfoNew, refDtoNew, refDfoNew, fu1, fd1, fu2, fd2, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }


        private static double[] ConvertToGeoPoint(double[] res)
        {
            if (res == null || res.Length == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, 100 };
            }
            else if (res.Length != 3 && res.Length != 6)
            {
                throw new Exception("定位结果解析异常,长度不是3或6");
            }

            var list = res.Select(p => Math.Round(p, 4)).ToArray();
            if (list.Length == 3)
            {
                list = list.Concat(new double[4] { 999, 999, 0, 100 }).ToArray();
            }
            list[2] = list[5] = 0;//高度

            if (double.IsNaN(list[0]) || double.IsNaN(list[1]))
            {
                list[0] = list[1] = 999;
            }
            if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
            {
                list[3] = list[4] = 999;
            }
            if (list[0] < -180 || list[0] > 180)
            {
                list[0] = list[1] = 999;
            }
            else if (list[1] < -90 || list[1] > 90)
            {
                list[0] = list[1] = 999;
            }
            if (list[3] < -180 || list[3] > 180)
            {
                list[3] = list[4] = 999;
            }
            else if (list[4] < -90 || list[4] > 90)
            {
                list[3] = list[4] = 999;
            }
            if (list.Length == 6)
            {
                var listNew = list.ToList();
                listNew.Add(100);
                list = listNew.ToArray();
            }
            return list;
        }

        private static bool IsGeoPoint(double[] res)
        {
            if (res.Length != 3) return false;
            if (res[0] < -180 || res[0] > 180)
            {
                return false;
            }
            else if (res[1] < -90 || res[1] > 90)
            {
                return false;
            }
            return true;
        }
        private static bool IsGeoPoint2(double[] res)
        {
            if (res.Length < 3) return false;
            if (res[0] < -180 || res[0] > 180)
            {
                return false;
            }
            else if (res[1] < -90 || res[1] > 90)
            {
                return false;
            }
            return true;
        }
    }
}