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

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Security.Policy;
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 gzPos = @"AddIns\定位\DLL_GZDW.dll";

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

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


        [DllImport(gzPos, EntryPoint = "DW_Analysis", CallingConvention = CallingConvention.Cdecl)]//两星一地和三星的解析定位算法(精度差、速度快)
        private extern static void DW_Analysis(double[] mainSatXYZ, double[] adja1XYZ, double[] adja2XYZ, double[] refStation, double tarDto1, double tarDto2, double refDto1, double refDto2, double[] posRes, int flag);


        [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)
        {
            if (cgRes.DtoCdb.Value == 0) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            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>
        /// 两星一地带参解析定位(精度差速度快,主要用于置信度分析等需要多次调用的情况)
        /// </summary>
        /// <param name="cgRes"></param>
        /// <param name="sRes"></param>
        /// <returns></returns>
        public static double[] X2D1_GzPos(CgRes cgRes, StationRes sRes)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.DtoCdb.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            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[] 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 ybDto = ybDto1 - ybDto2;
            double[] posRes = new double[3];
            DW_Analysis(mainSat, adjaSat, cdbStation, refStation, dto1, -dtoCdb, ybDto, -ybDto1, posRes, 1);
            return posRes;
        }
        /// <summary>
        /// 三星双时差带参解析定位(精度差速度快,主要用于置信度分析等需要多次调用的情况)
        /// </summary>
        /// <param name="cgRes"></param>
        /// <param name="sRes"></param>
        /// <returns></returns>
        public static double[] X3Dto_GzPos(CgRes cgRes, StationRes sRes)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.Dto2.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adja1Sat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] adja2Sat = new double[3] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dto1 = cgRes.Dto1.Value / 1e6;
            double dto2 = cgRes.Dto2.Value / 1e6;
            double ybDto1 = cgRes.YbMainDto.Value / 1e6;
            double ybDto2 = cgRes.YbAdja1Dto.Value / 1e6;
            double ybDto3 = cgRes.YbAdja2Dto.Value / 1e6;
            double refDto1 = ybDto1 - ybDto2;
            double refDto2 = ybDto1 - ybDto3;
            double[] posRes = new double[3];
            DW_Analysis(mainSat, adja1Sat, adja2Sat, refStation, dto1, dto2, refDto1, refDto2, posRes, 0);
            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)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.DtoCdb.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            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))
            {
                var posResGz = X2D1_GzPos(cgRes, sRes);
                if (IsGeoPoint2(posResGz))
                {
                    int succeedCount = 0;
                    var cgResNew = cgRes.Clone();
                    for (int i = 0; i < confidenceCount; i++)
                    {
                        cgResNew.MainX = cgRes.MainX + RandomHelper.Normal(0, 500);
                        cgResNew.MainY = cgRes.MainY + RandomHelper.Normal(0, 500);
                        cgResNew.MainZ = cgRes.MainZ + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1X = cgRes.Adja1X + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1Y = cgRes.Adja1Y + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1Z = cgRes.Adja1Z + RandomHelper.Normal(0, 500);
                        cgResNew.Dto1 = (cgRes.Dto1 * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.DtoCdb = (cgRes.DtoCdb * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.YbMainDto = (cgRes.YbMainDto * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.YbAdja1Dto = (cgRes.YbAdja1Dto * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        res = X2D1_GzPos(cgResNew, sRes);
                        var posResNew = ConvertToGeoPoint(res);
                        if (IsGeoPoint2(posResNew))
                        {
                            var distance = PhysicsHelper.DistanceGeo((posResGz[0], posResGz[1], 0), (posResNew[0], posResNew[1], 0));
                            if (distance <= confidenceDistance)
                            {
                                succeedCount++;
                            }
                        }
                    }
                    posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
                }
                else
                {
                    posRes[6] = 0;
                }
            }
            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)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.DtoCdb.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            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);
            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;
        }

        public static double[] X2D1_PosNoRef_ZL(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.DtoCdb.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            double startLon = (int)sRes.CdbTxLon.Value - 20;
            double endLon = (int)sRes.CdbTxLon.Value + 20;
            double startLat = (int)sRes.CdbTxLat.Value - 20;
            double endLat = (int)sRes.CdbTxLat.Value + 20;
            List<(double, double, double, double)> list = new List<(double, double, double, double)>();
            var recXYZ = PhysicsHelper.GeoToEcef((sRes.SatTxLon, sRes.SatTxLat, 0));
            var cdbXYZ = PhysicsHelper.GeoToEcef((sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0));
            for (double lon = startLon; lon < endLon; lon += 0.0001)
            {
                int flag = 0;
                for (double lat = startLat; lat < endLat; lat += 0.0001)
                {
                    var posXYZ = PhysicsHelper.GeoToEcef((lon, lat, 0));

                    //目标-主星-接收站的时间(us)
                    var dt1 = PhysicsHelper.Dto(posXYZ, (cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value), recXYZ) * 1e6;

                    //目标-超短站的时间(us)
                    var dt3 = PhysicsHelper.Dto(posXYZ, cdbXYZ) * 1e6;
                    var dto2 = Math.Abs(dt1 - dt3 - cgRes.DtoCdb.Value);
                    if (dto2 > 400)
                    {
                        lat += 1;
                        if (flag < 1)
                            flag = 1;
                        continue;
                    }
                    else if (dto2 > 200)
                    {
                        lat += 0.5;
                        if (flag < 2)
                            flag = 2;
                        continue;
                    }
                    else if (dto2 > 100)
                    {
                        lat += 0.2;
                        if (flag < 3)
                            flag = 3;
                        continue;
                    }
                    else if (dto2 > 10)
                    {
                        lat += 0.01;
                        if (flag < 4)
                            flag = 4;
                        continue;
                    }
                    //else if (dto2 > 2)
                    //{
                    //    lat += 0.001;
                    //    if (flag < 5)
                    //        flag = 5;
                    //    continue;
                    //}
                    //目标-邻星-接收站的时间(us)
                    var dt2 = PhysicsHelper.Dto(posXYZ, (cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value), recXYZ) * 1e6;
                    var dto1 = Math.Abs(dt1 - dt2 - cgRes.Dto1.Value);
                    if (dto1 > 400)
                    {
                        lat += 1;
                        if (flag < 1)
                            flag = 1;
                        continue;
                    }
                    if (dto1 > 200)
                    {
                        lat += 0.5;
                        if (flag < 2)
                            flag = 2;
                        continue;
                    }
                    else if (dto1 > 100)
                    {
                        lat += 0.2;
                        if (flag < 3)
                            flag = 3;
                        continue;
                    }
                    else if (dto1 > 10)
                    {
                        lat += 0.01;
                        if (flag < 4)
                            flag = 4;
                        continue;
                    }
                    //else if (dto1 > 2)
                    //{
                    //    lat += 0.001;
                    //    if (flag < 5)
                    //        flag = 5;
                    //    continue;
                    //}
                    list.Add((lon, lat, dto1, dto2));
                }
                if (flag == 1)
                    lon += 1;
                else if (flag == 2)
                    lon += 0.5;
                else if (flag == 3)
                    lon += 0.2;
                else if (flag == 4)
                    lon += 0.01;
                else if (flag == 5)
                    lon += 0.001;
            }
            double[] posRes;
            var p1 = list.OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
            if (p1 == default)
            {
                posRes = new double[7] { 999, 999, 0, 999, 999, 0, -1 };
                return posRes;
            }
            var p2 = list.Where(p => PhysicsHelper.DistanceGeo((p1.Item1, p1.Item2, 0), (p.Item1, p.Item2, 0)) > 10000).OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
            if (p2 == default)
                posRes = new double[7] { p1.Item1, p1.Item2, 0, 999, 999, 0, -1 };
            else
                posRes = new double[7] { p1.Item1, p1.Item2, 0, p2.Item1, p2.Item2, 0, -1 };
            if (posRes[3] != 999)
            {
                var dis1 = PhysicsHelper.DistanceArcGeo((posRes[3], posRes[4]), (sRes.CdbTxLon.Value, sRes.CdbTxLat.Value));
                var dis2 = PhysicsHelper.DistanceArcGeo((posRes[0], posRes[1]), (sRes.CdbTxLon.Value, sRes.CdbTxLat.Value));
                if (dis1 < dis2)
                {
                    var tmp1 = posRes[3];
                    var tmp2 = posRes[4];
                    posRes[3] = posRes[0];
                    posRes[4] = posRes[1];
                    posRes[0] = tmp1;
                    posRes[1] = tmp2;
                }
            }
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                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 }; //定位区域
                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;
                    double[] res = new double[6];
                    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)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.Dto2.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            if (cgRes.Dto1.Value == 0 || cgRes.Dto2.Value == 0) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            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);
            var posRes = ConvertToGeoPoint(res);//精确搜索值
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                var posResGz = X3Dto_GzPos(cgRes, sRes);
                if (IsGeoPoint2(posResGz))
                {
                    int succeedCount = 0;
                    var cgResNew = cgRes.Clone();
                    for (int i = 0; i < confidenceCount; i++)
                    {
                        cgResNew.MainX = cgRes.MainX + RandomHelper.Normal(0, 500);
                        cgResNew.MainY = cgRes.MainY + RandomHelper.Normal(0, 500);
                        cgResNew.MainZ = cgRes.MainZ + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1X = cgRes.Adja1X + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1Y = cgRes.Adja1Y + RandomHelper.Normal(0, 500);
                        cgResNew.Adja1Z = cgRes.Adja1Z + RandomHelper.Normal(0, 500);
                        cgResNew.Adja2X = cgRes.Adja2X + RandomHelper.Normal(0, 500);
                        cgResNew.Adja2Y = cgRes.Adja2Y + RandomHelper.Normal(0, 500);
                        cgResNew.Adja2Z = cgRes.Adja2Z + RandomHelper.Normal(0, 500);
                        cgResNew.Dto1 = (cgRes.Dto1 * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.Dto2 = (cgRes.Dto2 * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.YbMainDto = (cgRes.YbMainDto * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.YbAdja1Dto = (cgRes.YbAdja1Dto * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        cgResNew.YbAdja2Dto = (cgRes.YbAdja2Dto * 1e6 + RandomHelper.Normal(0, 4)) / 1e6;
                        res = X3Dto_GzPos(cgResNew, sRes);
                        var posResNew = ConvertToGeoPoint(res);
                        if (IsGeoPoint2(posResNew))
                        {
                            var distance = PhysicsHelper.DistanceGeo((posResGz[0], posResGz[1], 0), (posResNew[0], posResNew[1], 0));
                            if (distance <= confidenceDistance)
                            {
                                succeedCount++;
                            }
                        }
                    }
                    posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
                }
                else
                {
                    posRes[6] = 0;
                }
            }
            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)
        {
            if (cgRes.Dto1.Value == 0 || cgRes.Dto2.Value == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            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)
        {
            if (cgRes.Dfo1.Value == 0 || cgRes.Dfo2.Value == 0) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            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)
        {
            if (cgRes.Dto1.Value == 0) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            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, -1 };
            }
            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, -1 }).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(-1);
                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;
        }
    }
}