/// <summary>
/// 生产平差器
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
public static MatrixAdjuster Create(AdjustmentType type)
{
MatrixAdjuster Adjustment = null;
switch (type)
{
case AdjustmentType.参数平差:
Adjustment = new ParamAdjuster();
break;
case AdjustmentType.条件平差:
Adjustment = new ConditionalAdjuster();
break;
case AdjustmentType.具有参数的条件平差:
Adjustment = new ConditionalAdjusterWithParam();
break;
case AdjustmentType.具有条件的参数平差:
Adjustment = new ParamAdjusterWithCondition();
break;
case AdjustmentType.卡尔曼滤波:
Adjustment = new SimpleKalmanFilter();
// Adjustment = new KalmanFilter();
break;
case AdjustmentType.均方根滤波:
Adjustment = new SquareRootInformationFilter();
break;
case AdjustmentType.序贯平差:
Adjustment = new SequentialAdjuster();
break;
case AdjustmentType.参数加权平差:
Adjustment = new WeightedParamAdjuster();
break;
case AdjustmentType.递归最小二乘:
Adjustment = new RecursiveAdjuster();
break;
case AdjustmentType.单期递归最小二乘:
Adjustment = new SingleRecursiveAdjuster();
break;
default:
//Adjustment = new ParamAdjuster();
break;
}
if (Adjustment == null)
{
throw new Exception("尚未实现 " + type);
}
return(Adjustment);
}
/// <summary>
/// 参数估计
/// </summary>
/// <returns></returns>
public PlainXyConvertParam Estimate()
{
var obsVector = new Vector(ObsCount);
//构建观测向量
for (int i = 0; i < CoordCount; i++)
{
int index = 2 * i;
var oldXy = OldOrTobeConvertingXys[i];
var newXy = NewOrTargedXys[i];
obsVector[index + 0] = newXy.X - oldXy.X;
obsVector[index + 1] = newXy.Y - oldXy.Y;
}
//构建系数阵
Matrix coeffOfParam = new Matrix(ObsCount, 4);
for (int i = 0; i < CoordCount; i++)
{
int index = 2 * i;
var oldXy = OldOrTobeConvertingXys[i];
//尺度因子，
coeffOfParam[index + 0, 0] = 1;
coeffOfParam[index + 1, 1] = 1;
//转换参数，弧度
coeffOfParam[index + 0, 2] = oldXy.X;
coeffOfParam[index + 0, 3] = oldXy.Y;
coeffOfParam[index + 1, 2] = oldXy.Y;
coeffOfParam[index + 1, 3] = -oldXy.X;
}
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(obsVector);
var adjuster = new ParamAdjuster();
this.ResultMatrix = adjuster.Run(obsMatrix);
var ested = ResultMatrix.Estimated;
var result = new PlainXyConvertParam(new XY(ested[0], ested[1]), ested[2] * 1e6, ested[3] * CoordConsts.RadToSecMultiplier);
return(result);
}
/// <summary>
/// 构建参数平差观测方程，并进行平差计算。
/// </summary>
/// <param name="fileA"></param>
/// <param name="fileB"></param>
/// <returns></returns>
private double[][] Adjust(SinexFile fileA, SinexFile fileB)
{
//检查一下是否只包含坐标，如否，则清理。
if (!fileA.IsOnlyEstimateCoordValue)
{
fileA.CleanNonCoordSolutionValue();
}
if (!fileB.IsOnlyEstimateCoordValue)
{
fileB.CleanNonCoordSolutionValue();
}
double[][] A = SinexSubNetsUnion.GetCoeffMatrixOfParams(fileA, fileB);
double[][] Q = SinexSubNetsUnion.GetCovaMatrixOfObs(fileA, fileB);
double[][] obsMinusApriori = SinexSubNetsUnion.GetObsMinusApriori(fileA, fileB);
var pa = new ParamAdjuster();
p = pa.Run(new AdjustObsMatrix(A, obsMinusApriori, Q));
double[][] apriori = GetApriori(fileA, fileB);
//MatrixUtil.SaveToText(A, @"C:\A.txt");
//MatrixUtil.SaveToText(Q, @"C:\Q.txt");
//MatrixUtil.SaveToText(obsMinusApriori, @"C:\l.txt");
//MatrixUtil.SaveToText(apriori, @"C:\D.txt");
xyzs = GetXyzs(MatrixUtil.GetPlus(p.Estimated.OneDimArray, apriori));
geoCoords = new List <GeoCoord>();
foreach (var item in xyzs)
{
geoCoords.Add(CoordTransformer.XyzToGeoCoord(item));
}
result = SinexMerger.EmergeBasic(fileA, fileB);
return(apriori);
}
/// <summary>
/// 参数估计
/// </summary>
/// <returns></returns>
public XyzFrameConvertParam Estimate()
{
var obsVector = new Vector(ObsCount);
//构建观测向量
for (int i = 0; i < CoordCount; i++)
{
int index = 3 * i;
var oldXy = OldOrTobeConvertingXys[i];
var newXy = NewOrTargedXys[i];
obsVector[index + 0] = newXy.X - oldXy.X;
obsVector[index + 1] = newXy.Y - oldXy.Y;
obsVector[index + 2] = newXy.Z - oldXy.Z;
}
//构建系数阵
Matrix coeffOfParam = new Matrix(ObsCount, 7);
for (int i = 0; i < CoordCount; i++)
{
int index = 3 * i;
var oldXy = OldOrTobeConvertingXys[i];
//平移参数XYZ，0-2，单位：m
coeffOfParam[index + 0, 0] = 1;
coeffOfParam[index + 1, 1] = 1;
coeffOfParam[index + 2, 2] = 1;
//尺度参数，3，单位：ppm，求出的直接为 ppm
double factor = 1.0;// 1e-6;
coeffOfParam[index + 0, 3] = oldXy.X * factor;
coeffOfParam[index + 1, 3] = oldXy.Y * factor;
coeffOfParam[index + 2, 3] = oldXy.Z * factor;
//旋转参数，4-6, 单位：s,需要转换为rad
factor = 1.0;// CoordConsts.SecToRadMultiplier;
coeffOfParam[index + 0, 5] = -oldXy.Z * factor;
coeffOfParam[index + 0, 6] = oldXy.Y * factor;
coeffOfParam[index + 1, 4] = oldXy.Z * factor;
coeffOfParam[index + 1, 6] = -oldXy.X * factor;
coeffOfParam[index + 2, 4] = -oldXy.Y * factor;
coeffOfParam[index + 2, 5] = oldXy.X * factor;
}
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(obsVector);
var adjuster = new ParamAdjuster();
this.ResultMatrix = adjuster.Run(obsMatrix);
var ested = ResultMatrix.Estimated;
var result = new XyzFrameConvertParam(
new XYZ(ested[0], ested[1], ested[2]), //meter
new XYZ(ested[4], ested[5], ested[6]) * CoordConsts.RadToSecMultiplier, //second
ested[3] * 1e6 //ppm
);
//输出精度信息
var RMSVector = ResultMatrix.StdOfEstimatedParam;
StringBuilder sb = new StringBuilder();
sb.AppendLine("单位权中误差：" + this.ResultMatrix.StdDev.ToString("G6"));
sb.Append("平移参数(m)：" + result.Offset);
sb.AppendLine(", 标准差：" + RMSVector[0].ToString("G6") + ", " + RMSVector[1].ToString("G6") + ", " + RMSVector[2].ToString("G6"));
sb.Append("旋转参数(s)：" + result.RotationAngleSecond);
sb.AppendLine(", 标准差：" + RMSVector[4].ToString("G6") + ", " + RMSVector[5].ToString("G6") + ", " + RMSVector[6].ToString("G6"));
sb.Append("尺度参数(ppm)：" + result.ScaleFactorPpm.ToString("G6"));
sb.AppendLine(", 标准差：" + RMSVector[3].ToString("G6"));
log.Info(sb.ToString());
return(result);
}
