|
@@ -5,6 +5,7 @@ using System.Runtime.InteropServices;
|
|
|
using System.Text;
|
|
|
using System.Threading.Tasks;
|
|
|
using XdCxRhDW.Entity;
|
|
|
+using XdCxRhDW.Framework;
|
|
|
|
|
|
namespace XdCxRhDW.Api
|
|
|
{
|
|
@@ -13,6 +14,12 @@ namespace XdCxRhDW.Api
|
|
|
/// </summary>
|
|
|
public static class PosApi
|
|
|
{
|
|
|
+ //置信度统计次数
|
|
|
+ private static int confidenceCount = 100;
|
|
|
+
|
|
|
+ //置信度有效距离(m)
|
|
|
+ private static int confidenceDistance = 10000;
|
|
|
+
|
|
|
#region cpp dll Interop
|
|
|
|
|
|
//一星一地测向带参定位
|
|
@@ -48,18 +55,19 @@ namespace XdCxRhDW.Api
|
|
|
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>
|
|
|
- /// 一星一地带参,返返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 一星一地带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -72,16 +80,45 @@ namespace XdCxRhDW.Api
|
|
|
double[] res = new double[6];
|
|
|
var ybDto1 = cgRes.YbMainDto.Value / 1e6;
|
|
|
X1D1_Pos20240305_Core(mainSat, satStation, cdbStation, cxStation, refStation, zone, theta, dtoCdb, ybDto1, res);
|
|
|
- return 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 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>
|
|
|
- /// 两星一地带参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 两星一地带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -96,16 +133,54 @@ namespace XdCxRhDW.Api
|
|
|
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);
|
|
|
- return ConvertToGeoPoint(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>
|
|
|
- /// 两星一地无参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 两星一地无参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -116,50 +191,88 @@ namespace XdCxRhDW.Api
|
|
|
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);
|
|
|
- return ConvertToGeoPoint(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>
|
|
|
- /// 融合定位带参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 融合定位带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ public static double[] RH_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes, bool CalcConfidence = false)
|
|
|
{
|
|
|
- var res1 = X1D1_Pos(cgRes, sRes, cxRes);
|
|
|
- var res2 = X2D1_Pos(cgRes, sRes);
|
|
|
- double[] res = new double[] { 999, 999, 0, 999, 999, 0 };
|
|
|
+ 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[6] {
|
|
|
+ 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[6] {
|
|
|
+ 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[6]
|
|
|
+ res = new double[7]
|
|
|
{
|
|
|
p1[0] + 0.003,
|
|
|
p1[1] - 0.002,
|
|
@@ -167,12 +280,12 @@ namespace XdCxRhDW.Api
|
|
|
p2[0] + 0.003,
|
|
|
p2[1] - 0.002,
|
|
|
0,
|
|
|
-
|
|
|
+ (res1[6]+res2[6])/2
|
|
|
};
|
|
|
}
|
|
|
else if (IsGeoPoint(p3) && IsGeoPoint(p4))
|
|
|
{
|
|
|
- res = new double[6]
|
|
|
+ res = new double[7]
|
|
|
{
|
|
|
p3[0] + 0.003,
|
|
|
p3[1] - 0.002,
|
|
@@ -180,19 +293,20 @@ namespace XdCxRhDW.Api
|
|
|
p4[0] + 0.003,
|
|
|
p4[1] - 0.002,
|
|
|
0,
|
|
|
-
|
|
|
+ (res1[6]+res2[6])/2
|
|
|
};
|
|
|
}
|
|
|
return res;
|
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
|
- /// 三星双时差带参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 三星双时差带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -207,16 +321,61 @@ namespace XdCxRhDW.Api
|
|
|
double[] res = new double[6];
|
|
|
X3_Pos20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, satStation,
|
|
|
satStation, satStation, refStation, zone, tarDto1, tarDto2, refDto1, refDto2, res);
|
|
|
- return ConvertToGeoPoint(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>
|
|
|
- /// 三星双时差无参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 三星双时差无参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -227,16 +386,59 @@ namespace XdCxRhDW.Api
|
|
|
var tarDto2 = cgRes.Dto2.Value / 1e6;
|
|
|
double[] res = new double[6];
|
|
|
X3_PosNoRef20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, zone, tarDto1, tarDto2, res);
|
|
|
- return ConvertToGeoPoint(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>
|
|
|
- /// 三星双频差带参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 三星双频差带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -255,16 +457,61 @@ namespace XdCxRhDW.Api
|
|
|
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);
|
|
|
- return ConvertToGeoPoint(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>
|
|
|
- /// 双星时频差带参,返回经度纬度高度及镜像点,数组长度为6
|
|
|
+ /// 双星时频差带参,返回经度、纬度、高度、镜像点、置信度,数组长度为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)
|
|
|
+ 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 };
|
|
@@ -281,14 +528,53 @@ namespace XdCxRhDW.Api
|
|
|
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);
|
|
|
- return ConvertToGeoPoint(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[6] { 999, 999, 0, 999, 999, 0 };
|
|
|
+ return new double[7] { 999, 999, 0, 999, 999, 0, 100 };
|
|
|
}
|
|
|
else if (res.Length != 3 && res.Length != 6)
|
|
|
{
|
|
@@ -298,17 +584,18 @@ namespace XdCxRhDW.Api
|
|
|
var list = res.Select(p => Math.Round(p, 4)).ToArray();
|
|
|
if (list.Length == 3)
|
|
|
{
|
|
|
- list = list.Concat(new double[3] { 999, 999, 0 }).ToArray();
|
|
|
+ list = list.Concat(new double[4] { 999, 999, 0, 100 }).ToArray();
|
|
|
}
|
|
|
list[2] = list[5] = 0;//高度
|
|
|
- if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
|
|
|
- {
|
|
|
- list[3] = list[4] = 999;
|
|
|
- }
|
|
|
+
|
|
|
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;
|
|
@@ -325,7 +612,12 @@ namespace XdCxRhDW.Api
|
|
|
{
|
|
|
list[3] = list[4] = 999;
|
|
|
}
|
|
|
-
|
|
|
+ if (list.Length == 6)
|
|
|
+ {
|
|
|
+ var listNew = list.ToList();
|
|
|
+ listNew.Add(100);
|
|
|
+ list = listNew.ToArray();
|
|
|
+ }
|
|
|
return list;
|
|
|
}
|
|
|
|
|
@@ -342,6 +634,18 @@ namespace XdCxRhDW.Api
|
|
|
}
|
|
|
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;
|
|
|
+ }
|
|
|
}
|
|
|
}
|