/// 初始化并计算。
/// </summary>
/// <param name="coeff_param">参数的系数阵</param>
/// <param name="obsMinusApriori">观测值减去估值</param>
/// <param name="inverseWeight_obs">观测值的权逆阵</param>
/// <param name="inverseWeight_signal">含下面最后三个变量</param>
public void Init(
double[][] coeff_param, //参数的系数阵
double[][] obsMinusApriori, //观测值减去估值
double[][] inverseWeight_obs, //观测值的权逆阵
double[][] inverseWeight_signal // 含下面最后三个变量
)
{
int paramCount = coeff_param[0].Length;
int allSignalCount = inverseWeight_signal[0].Length;
int measuredSignalCount = paramCount;
int notMeasuredSignalCount = allSignalCount - measuredSignalCount;
//生成信号的系数阵
double[][] coeff_signal = MatrixUtil.Create(paramCount, allSignalCount);//是否应该自己生成
MatrixUtil.SetSubMatrix(coeff_signal, MatrixUtil.CreateIdentity(paramCount));
//提取方差、协方差阵
double[][] inverseWeight_mesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的左上角
double[][] inverseWeight_notMesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, notMeasuredSignalCount, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右下角
double[][] covaSignals_Ss = MatrixUtil.GetSubMatrix(inverseWeight_signal, 0, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右上角
Init(
coeff_param, //参数的系数阵
obsMinusApriori, //观测值减去估值
inverseWeight_obs, //观测值的权逆阵
inverseWeight_signal, // 含下面最后三个变量
coeff_signal, //是否应该自己生成
inverseWeight_mesuredSignal, //inverseWeight_signal的左上角
inverseWeight_notMesuredSignal, //inverseWeight_signal的右下角
covaSignals_Ss //inverseWeight_signal的右上角
);
}
/// <summary>
/// 提取观测量的权逆阵。是否只提取对角阵？？
/// </summary>
/// <param name="fileA"></param>
/// <param name="fileB"></param>
/// <returns></returns>
public static double[][] GetCovaMatrixOfObs(SinexFile fileA, SinexFile fileB)
{
int paramCountA = fileA.EstimateParamCount;
int row = paramCountA + fileB.EstimateParamCount;
double[][] array = MatrixUtil.Create(row);
//fileA直接设置。
//这里应该定权！
SinexStatistic statisticA = fileA.GetStatistic();
SinexStatistic statisticB = fileB.GetStatistic();
SinexStatistic statisticNew = SinexStatistic.Merge(statisticA, statisticB);
double varFactorA = statisticNew.VarianceOfUnitWeight / statisticA.VarianceOfUnitWeight;
double varFactorB = statisticNew.VarianceOfUnitWeight / statisticB.VarianceOfUnitWeight;
double[][] matrixA = fileA.GetEstimateCovaMatrix();
double[][] matrixB = fileB.GetEstimateCovaMatrix();
MatrixUtil.Multiply(matrixA, varFactorA);
MatrixUtil.Multiply(matrixB, varFactorB);
MatrixUtil.SetSubMatrix(array, matrixA);
MatrixUtil.SetSubMatrix(array, matrixB, paramCountA, paramCountA);
return(array);
}
/// <summary>
/// 最小二乘解算。返回参数向量平差值。
/// </summary>
/// <returns></returns>
public double[][] Solve()
{
//组建大法矩阵
ArrayMatrix InverN_error = new ArrayMatrix(Normal_error).Inverse;
int row = Normal_error.Length + Coeff_condition.Length;
double[][] normalArray = MatrixUtil.Create(row);
MatrixUtil.SetSubMatrix(normalArray, Normal_error);
MatrixUtil.SetSubMatrix(normalArray, Coeff_condition, Normal_error.Length);
MatrixUtil.SetSubMatrix(normalArray, MatrixUtil.Transpose(Coeff_condition), 0, Normal_error.Length);
//法方程大右手边
double[][] rightHandSide = MatrixUtil.Create(row, 1);
MatrixUtil.SetSubMatrix(rightHandSide, RightHandSide_error);
MatrixUtil.SetSubMatrix(rightHandSide, ObsMinusApriori_condition, RightHandSide_error.Length);
//解
ArrayMatrix XK = new ArrayMatrix(normalArray).Inverse *new ArrayMatrix(rightHandSide);
ArrayMatrix X = new ArrayMatrix(MatrixUtil.GetSubMatrix(XK.Array, 6, 1, 0, 0));
//精度评定
if (A != null && L != null)
{
ArrayMatrix V = A * X - L;
this.VarianceOfUnitWeight = (V.Transpose() * V)[0, 0] / (this.Freedom + ConditionCount);
//结果转化
this.CovaOfParams = (InverN_error.Inverse * VarianceOfUnitWeight).Array;//(N.Inverse ).Array; //
}
this.Param = X.Array;
return(Param);
}
/// <summary>
/// 拟合参数
/// </summary>
public double [] FitParameters()
{
int rowA = ArrayY.Length;//行
int colA = ParamCount;
//coeffOfParams(m,n)
double[][] A = MatrixUtil.Create(rowA, colA);
//让 X 不变，如输出相同。！！！！2018.05.20，czs
for (int i = 0; i < rowA; i++)
{
for (int j = 0; j < colA; j++)
{
var x = ArrayX[i];
A[i][j] = Math.Pow(x, j);
}
}
double[][] L = MatrixUtil.Create(rowA, 1);
for (int i = 0; i < rowA; i++)
{
L[i][0] = ArrayY[i];
}
ParamAdjuster ParamAdjuster = new ParamAdjuster();
var ad = ParamAdjuster.Run(new Matrix(A), new Matrix(L));
Parameters = ad.OneDimArray;
StdDev = ParamAdjuster.StdDev;
return(Parameters);
}
/// <summary>
/// 在 GNSS 动态导航中的应用
/// </summary>
protected void Motion()
{
List <string> paramNames = new List <string>()
{
"X", "Y", "Z", "dX", "dY", "dZ"
};
///参数
List <Vector> satXyzs = new List <Vector>();
Vector siteXyz = new Vector(3);
Vector L = new Vector(6); //观测值
double interval = 30; //时间间隔（单位：秒）
//将瞬时加速度作为随机干扰，
//状态向量， 位置和速度 X = trans([x, y, z, vx, vy, vz])
//噪声向量， 为加速度 Noise = trans(ax, ay, az])
//纯量形式， y = y0 + t * v0 + 0.5 * a0 * a0 * t
// v = v0 + a0 * t
/// 状态方程
/// ┌I Δt┐ ┌0.5*Δt*Δt┐
/// X(k) = │ │X(k-1) + │ │Ω(k-1)
/// └0 I ┘ └ Δt ┘
/// 观测方程
/// L(k) = A(k) X(k) + Ω(k)
/// 其中，L(k)为 k 时刻系统的 C/A 码观测向量
//常用量
double[][] identity3x3 = MatrixUtil.CreateIdentity(3);
int satCount = satXyzs.Count;
//状态方程状态转移矩阵 Φ（k,k-1）
// double[][] trans = MatrixUtil.CreateIdentity(6);
ArrayMatrix trans = new ArrayMatrix(6, 6);
MatrixUtil.SetSubMatrix(trans.Array, MatrixUtil.GetMultiply(identity3x3, interval), 0, 3);
//状态方程噪声向量Ω(k-1)的系数阵
double[][] coeef_noise = MatrixUtil.Create(6, 3);
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, 0.5 * interval * interval));
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, interval), 3);
//观测方程系数矩阵 A,每个时刻不同
ArrayMatrix coeef_obs = new ArrayMatrix(satCount, 6);
for (int i = 0; i < satCount; i++)
{
IVector differXyz = (satXyzs[i] - siteXyz);
coeef_obs[i, 0] = differXyz.GetCos(0);//.CosX;
coeef_obs[i, 1] = differXyz.GetCos(1);
coeef_obs[i, 2] = differXyz.GetCos(2);
}
KalmanFilter lastKf = null;
//Vector lastParams = lastKf.Corrected ?? null;
//IMatrix lastParamCova = lastKf.Corrected.InverseWeight ?? null;
//KalmanFilter kf = new KalmanFilter(coeef_obs, L, trans, lastParams, lastParamCova);
//kf.Process();
}
/// <summary>
/// 获取二维数组矩阵。
/// </summary>
/// <returns></returns>
public double[][] GetMatrix()
{
double[][] matrix = MatrixUtil.Create(RowCount, ColCount);
foreach (var item in MatrixItems)
{
matrix[item.Row][item.Col] = item.Value;
}
return(matrix);
}
private static double[][] GenCoeffA(int obsCount, int paraCount)
{
double[][] coeffA = MatrixUtil.Create(obsCount, paraCount);
for (int i = 0; i < obsCount; i++)
{
int col = i % paraCount;
coeffA[i][col] = 1.0;
}
return(coeffA);
}
/// <summary>
/// 测试分区平差
/// </summary>
public static void Test(int allObsCount = 1000, bool useParalell = true, int core = 8, int blockCount = 8)
{
DateTime from = DateTime.Now;
//观测噪声
Random random = new Random();
//设有blockObsCount个观测量，paramCount个区内参数，commonParamCount个公共参数
int blockObsCount = allObsCount / blockCount;
int paramCount = blockObsCount / 2 + 1;
int commonParamCount = blockObsCount / 4 + 1;
List <BlockAdjustItem> items = new List <BlockAdjustItem>();
for (int m = 0; m < blockCount; m++)
{
//分区内参数系数阵 obsCount x paramCount
double[][] coeffA = GenCoeffA(blockObsCount, paramCount);
//观测值 obsCount x 1
double[][] obs = MatrixUtil.Create(blockObsCount, 1);
for (int i = 0; i < blockObsCount; i++)
{
obs[i][0] = i; // +random.NextDouble() - 0.5;
}
//观测值权逆阵 obsCount x obsCount
double[][] inverseOfObs = MatrixUtil.CreateIdentity(blockObsCount);
//分区内对公共参数的系数阵 obsCount x commonParamCount
double[][] coeffB = MatrixUtil.Create(blockObsCount, commonParamCount);
for (int i = 0; i < commonParamCount; i++)
{
coeffB[i][i] = 1;
}
//System.IO.File.WriteAllText(
// "C:\\Block"+m+".txt",
// "A" + "\r\n" + MatrixUtil.GetFormatedText(coeffA) + "\r\n"
// + "L" + "\r\n" + MatrixUtil.GetFormatedText(obs) + "\r\n"
// + "Q" + "\r\n" + MatrixUtil.GetFormatedText(inverseOfObs) + "\r\n"
// + "Ab" + "\r\n" + MatrixUtil.GetFormatedText(coeffB) + "\r\n"
// );
BlockAdjustItem item = new BlockAdjustItem(coeffA, obs, inverseOfObs, coeffB);
items.Add(item);
}
BlockAdjustment ba = new BlockAdjustment(items.ToArray(), useParalell, core);
TimeSpan span = DateTime.Now - from;
StringBuilder sb = new StringBuilder();
sb.AppendLine("耗时:" + span + " = " + span.TotalMinutes + " 分");
System.IO.File.WriteAllText("C:\\IsP" + useParalell.ToString() + "obs" + +allObsCount + "_core" + core + "_block" + blockCount + ".txt", sb.ToString() + ba.ToString());
}
/// <summary>
/// 根据两个Sinex文件，提取系数阵。就是参数平差的参数系数阵。
/// Get coe Of Params Matrix Of Params
/// </summary>
/// <param name="fileA"></param>
/// <param name="fileB"></param>
/// <returns></returns>
public static double[][] GetCoeffMatrixOfParams(SinexFile fileA, SinexFile fileB)
{
List <string> paramCodes = SinexFile.GetDistinctSiteCodes(fileA, fileB);
List <string> codesA = fileA.GetSiteCods();
List <string> codesB = fileB.GetSiteCods();
List <int> sameParamIndexesInB = new List <int>();//B文件中具有相同测站的代码的索引号。
for (int i = 0; i < codesB.Count; i++)
{
if (codesA.Contains(codesB[i]))
{
sameParamIndexesInB.Add(i);
}
}
int paramCountA = fileA.EstimateParamCount; //文件A的参数，一个测站有三个参数X,Y,Z
int row = paramCountA + fileB.EstimateParamCount; //观测数量包含A和B
int col = paramCodes.Count * 3; //新参数数量（除去了A,B共有参数）
double[][] array = MatrixUtil.Create(row, col);
int indentCount = 0;//有多少参数相同，则缩进多少位置
for (int i = 0; i < paramCountA; i++)
{
array[i][i] = 1; //先A
}
for (int i = paramCountA; i < row; i++) //再B
{
int siteIndexInB = (i - paramCountA) / 3; //测站名在B中的索引
if (sameParamIndexesInB.Contains(siteIndexInB))
{
int colInA = codesA.IndexOf(codesB[siteIndexInB]) * 3;//在A中的索引
array[i][colInA] = 1;
array[++i][colInA + 1] = 1;
array[++i][colInA + 2] = 1;
indentCount += 3;//一个测站缩进三单位
}
else
{
array[i][i - indentCount] = 1;
}
}
return(array);
}
private void button_exportNormaRightHand_Click(object sender, EventArgs e)
{
if (!file.HasNormalEquationVectorMatrix)
{
MessageBox.Show("法方程右手边为空！"); return;
}
SaveFileDialog saveFileDialog1 = new SaveFileDialog();
saveFileDialog1.FileName = "NormaRightHand.txt";
saveFileDialog1.Filter = "文本文档(*.txt)|*.txt";
if (saveFileDialog1.ShowDialog() == System.Windows.Forms.DialogResult.OK)
{
MatrixUtil.Print(new FileStream(saveFileDialog1.FileName, FileMode.Create),
MatrixUtil.Create(file.GetNormalEquationVector()));
FormUtil.ShowIfOpenDirMessageBox(saveFileDialog1.FileName);
}
}
/// <summary>
/// 测试参数加权平差
/// </summary>
public static void Test()
{
Random random = new Random();//观测噪声
double[][] A = MatrixUtil.CreateIdentity(6);
double[][] L = MatrixUtil.Create(6, 1);
for (int i = 0; i < L.Length; i++)
{
L[i][0] = i + 1 + random.NextDouble() - 0.5;
}
double[][] QL = MatrixUtil.CreateDiagonal(6, 1);
double[][] Xa0 = MatrixUtil.Create(3, 1);
for (int i = 0; i < Xa0.Length; i++)
{
Xa0[i][0] = i + 1;
}
double[][] Qx0 = MatrixUtil.CreateDiagonal(3, 0.001);
// WeightedParamAdjustment adjust = new WeightedParamAdjustment(A, L, QL, Xa0, Qx0);
}
/// <summary>
/// 未知参数的先验值。是一个列向量.
/// </summary>
/// <param name="fileA"></param>
/// <param name="fileB"></param>
/// <returns></returns>
public static double[][] GetApriori(SinexFile fileA, SinexFile fileB)
{
int row = SinexFile.GetDistinctSiteCodes(fileA, fileB).Count * 3;
double[][] array = MatrixUtil.Create(row, 1);
int j = 0;
foreach (var item in fileA.SolutionAprioriBlock.Items)
{
array[j++][0] = item.ParameterValue;
}
List <string> sames = SinexFile.GetSameSiteCodes(fileA, fileB);
foreach (var item in fileB.SolutionAprioriBlock.Items)
{
if (!sames.Contains(item.SiteCode))
{
array[j++][0] = item.ParameterValue;
}
}
return(array);
}
/// <summary>
/// 切比雪夫多项式拟合
/// </summary>
/// <param name="x"></param>
/// <param name="xs"></param>
/// <param name="ys"></param>
/// <param name="order">参数数量=最高阶次+1</param>
/// <returns></returns>
public static double ChebyshevFit(double x, double[] xs, double[] ys, int order = 2)
{
int xLen = xs.Length;
double min = Math.Min(xs[0], xs[xLen - 1]);
double max = Math.Max(xs[0], xs[xLen - 1]);
double[][] A = MatrixUtil.Create(xLen, order);
for (int i = 0; i < xLen; i++)
{
double scopeX = ToScope(xs[i], min, max);
for (int j = 0; j < order; j++)
{
A[i][j] = GetT(scopeX, j);
}
}
double[] L = new double[xLen];
for (int i = 0; i < xLen; i++)
{
L[i] = ys[i];
}
Adjust.ParamAdjuster pa = new Adjust.ParamAdjuster();
var ad = pa.Run(new Adjust.AdjustObsMatrix(A, L));
double[] param = ad.Estimated.OneDimArray;
double result = 0;
double smallX = ToScope(x, min, max);
for (int i = 0; i < param.Length; i++)
{
result += param[i] * GetT(smallX, i);
}
return(result);
}
/// <summary>
/// 获取矩阵l，为观测值减去先验值，是一个列向量。
/// </summary>
/// <param name="fileA"></param>
/// <param name="fileB"></param>
/// <returns></returns>
public static double[][] GetObsMinusApriori(SinexFile fileA, SinexFile fileB)
{
int paraCountA = fileA.EstimateParamCount;
int row = paraCountA + fileB.EstimateParamCount;
double[][] array = MatrixUtil.Create(row, 1);
double[] estiamtedA = fileA.GetEstimateVector();
double[] aprioriA = fileA.GetAprioriVector();
double[] estiamtedB = fileB.GetEstimateVector();
double[] aprioriB = fileB.GetAprioriVector();
for (int i = 0; i < paraCountA; i++)
{
array[i][0] = estiamtedA[i] - aprioriA[i];
}
for (int i = paraCountA; i < paraCountA + estiamtedB.Length; i++)
{
array[i][0] = estiamtedB[i - paraCountA] - aprioriB[i - paraCountA];
}
return(array);
}
/// <summary>
/// 参数平差构造函数。
/// </summary>
/// <param name="coeffOfParams">参数系数阵</param>
/// <param name="obsMinusApriori">观测值 - 估值，观测方程常数项</param>
/// <param name="inervseWeight">权逆阵</param>
public AdjustObsMatrix(double coeffOfParams, double obsMinusApriori, double inervseWeight = 1)
: this(MatrixUtil.Create(coeffOfParams), MatrixUtil.Create(obsMinusApriori), MatrixUtil.Create(inervseWeight))
{
}
/// <summary>
/// 参数平差构造函数。
/// </summary>
/// <param name="coeffOfParams">参数系数阵</param>
/// <param name="obsMinusApriori">观测值 - 估值，观测方程常数项</param>
/// <param name="inervseWeight">权逆阵</param>
public AdjustObsMatrix(double[][] coeffOfParams, double[] obsMinusApriori, double[][] inervseWeight = null, Vector approx = null)
: this(coeffOfParams, MatrixUtil.Create(obsMinusApriori), inervseWeight, approx)
{
}
/// <summary>
/// Ambizap算法的实现。
/// 选择两个点的PPP计算结果，及其基线计算结果。
/// </summary>
private void Read()
{
string pointNameA = null, pointNameB = null;
//---------------------------------------------------数据读取//---------------------------------------------------
//只提取两个点的位置信息及其基线。
//读取PPP结果
string[] pppfilePathes = this.textBox_PathesOfPPP.Lines;
List <SinexFile> pppsinexFiles = SinexReader.Read(pppfilePathes);
List <SinexSiteDetail> pppSitesA = new List <SinexSiteDetail>();
List <SinexSiteDetail> pppSitesB = new List <SinexSiteDetail>();
foreach (var file in pppsinexFiles)
{
List <SinexSiteDetail> sites = file.GetSinexSites();
foreach (var item2 in sites)
{
if (pointNameA == null)
{
pointNameA = item2.Name;
}
if (pointNameB == null && pointNameA != item2.Name)
{
pointNameB = item2.Name;
}
if (item2.Name == pointNameA)
{
pppSitesA.Add(item2);
}
if (item2.Name == pointNameB)
{
pppSitesB.Add(item2);
}
}
}
if (pointNameA == null || pointNameB == null)
{
throw new ArgumentException("起算数据不足。");
}
//读取基线结果
//只需要一条基线就行了。
string[] baseLineFilePathes = this.textBox_pathesOfBaseLine.Lines;
List <SinexFile> baseLineSinexFiles = SinexReader.Read(baseLineFilePathes);
List <SinexSiteDetail> baseLineSites = new List <SinexSiteDetail>();
foreach (var file in baseLineSinexFiles)
{
List <SinexSiteDetail> sites = file.GetSinexSites();
foreach (var item2 in sites)
{
if ((item2.Name == pointNameA || item2.Name == pointNameB) && (baseLineSites.Find(m => m.Name == item2.Name) == null))
{
baseLineSites.Add(item2);
}
if (baseLineSites.Count == 2)
{
break;
}
}
if (baseLineSites.Count == 2)
{
break;
}
}
//组建基线
string name = baseLineSites[0].Name + "-" + baseLineSites[1].Name;
BaseLine line = new BaseLine(name, baseLineSites[1].EstimateXYZ - baseLineSites[0].EstimateXYZ);
//---------------------------------------------------------平差计算--------------------------------------------
//以第一个坐标为近似值
XYZ aprioriA = pppSitesA[0].EstimateXYZ;
XYZ aprioriB = pppSitesB[0].EstimateXYZ;
//构建矩阵，具有约束条件的参数平差中，误差方程和约束方程的未知数相同。
//参数有6个x0,y0,z0,x1,y1,z1,条件有三个 x1-x0 = a, y1-y0 = b, z1-z0 = c
double[][] coeff_error = MatrixUtil.Create((pppSitesA.Count + pppSitesB.Count) * 3, 6);
double[][] indentity3x3 = MatrixUtil.CreateIdentity(3);
for (int i = 0; i < pppSitesA.Count; i++)
{
MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, i * 3, 0);
}
for (int i = 0; i < pppSitesB.Count; i++)
{
MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, (pppSitesA.Count + i) * 3, 3);
}
//obsMinusApriori
double[] obsMinusApriori_error = new double[(pppSitesA.Count + pppSitesB.Count) * 3];
for (int i = 0; i < pppSitesA.Count; i++)
{
int rowIndex = i * 3;
obsMinusApriori_error[rowIndex + 0] = pppSitesA[i].EstimateXYZ.X - aprioriA.X;
obsMinusApriori_error[rowIndex + 1] = pppSitesA[i].EstimateXYZ.Y - aprioriA.Y;
obsMinusApriori_error[rowIndex + 2] = pppSitesA[i].EstimateXYZ.Z - aprioriA.Z;
}
for (int i = 0; i < pppSitesB.Count; i++)
{
int rowIndex = (pppSitesA.Count + i) * 3;
obsMinusApriori_error[rowIndex + 0] = pppSitesB[i].EstimateXYZ.X - aprioriB.X;
obsMinusApriori_error[rowIndex + 1] = pppSitesB[i].EstimateXYZ.Y - aprioriB.Y;
obsMinusApriori_error[rowIndex + 2] = pppSitesB[i].EstimateXYZ.Z - aprioriB.Z;
}
//Coeff_Condition,条件有三个 x0-x1 - a = 0,
//条件方程：c x + w = 0. => c deltaX + c X0 + w = 0
double[][] coeff_condition = MatrixUtil.Create(3, 6);
for (int index = 0; index < 3; index++)
{
coeff_condition[index][index + 0] = -1;
coeff_condition[index][index + 3] = 1;
}
//ConstVector_Condition c deltaX - W = 0, W = w - c X0
double[] obsMinusApriori_condition = new double[3];
XYZ aprioriVector = aprioriB - aprioriA;
obsMinusApriori_condition[0] = line.Vector.X - aprioriVector.X;
obsMinusApriori_condition[1] = line.Vector.Y - aprioriVector.Y;
obsMinusApriori_condition[2] = line.Vector.Z - aprioriVector.Z;
ParamAdjustmentWithCondition adjust = new ParamAdjustmentWithCondition(coeff_error, obsMinusApriori_error, coeff_condition, obsMinusApriori_condition);
double[][] param = adjust.Solve();
//---------------------------------------------------------结果输出--------------------------------------------
StringBuilder sb = new StringBuilder();
sb.AppendLine("计算完毕");
XYZ estA = aprioriA + new XYZ(adjust.ParamVector);
XYZ estB = aprioriB + new XYZ(adjust.ParamVector[3], adjust.ParamVector[4], adjust.ParamVector[5]);
sb.AppendLine("近似值A：" + aprioriA);
sb.AppendLine("平差值A：" + estA);
sb.AppendLine("近似值B：" + aprioriB);
sb.AppendLine("平差值B：" + estB);
sb.AppendLine("条件向量 A->B：" + line.Vector);
sb.AppendLine("平差向量 A->B：" + (estB - estA));
sb.AppendLine("------------------------------------参数------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(param));
double[][] param2 = adjust.Solve_ToBeCheck();
sb.AppendLine("param2");
sb.AppendLine(MatrixUtil.GetFormatedText(param2));
sb.AppendLine("误差方程系数阵");
sb.AppendLine(MatrixUtil.GetFormatedText(coeff_error));
sb.AppendLine("------------------------------------obsMinusApriori------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_error)));
sb.AppendLine("------------------------------------Coeff_Condition------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(coeff_condition));
sb.AppendLine("------------------------------------ConstVector_Condition------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_condition)));
sb.AppendLine("------------------------------------误差方程法方程------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_error));
sb.AppendLine("------------------------------------条件方程法方程------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_condition));
sb.AppendLine("------------------------------------误差方程右手边------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.RightHandSide_error));
this.textBox_info.Text = sb.ToString();
//----------------------------------------------------地图准备//----------------------------------------------------
points.Clear();
GeoCoord c1 = CoordTransformer.XyzToGeoCoord(aprioriA);
GeoCoord c2 = CoordTransformer.XyzToGeoCoord(aprioriB);
GeoCoord c3 = CoordTransformer.XyzToGeoCoord(estA);
GeoCoord c4 = CoordTransformer.XyzToGeoCoord(estB);
points.Add(new AnyInfo.Geometries.Point(c1.Lon, c1.Lat, "aprA"));
points.Add(new AnyInfo.Geometries.Point(c2.Lon, c2.Lat, "aprB"));
points.Add(new AnyInfo.Geometries.Point(c3.Lon, c3.Lat, "estA"));
points.Add(new AnyInfo.Geometries.Point(c4.Lon, c4.Lat, "estB"));
}
/// <summary>
/// 构造函数。
/// </summary>
/// <param name="coeff_error">参数（误差方程）系数阵</param>
/// <param name="obsMinusApriori_error">误差方程右手边</param>
/// <param name="inverseWeight">观测方程权逆阵</param>
/// <param name="coeff_condition">条件方程系数阵</param>
/// <param name="obsMinusApriori_condition">条件方程常数项</param>
public ParamAdjustmentWithCondition(double[][] coeff_error, double[] obsMinusApriori_error, double[][] coeff_condition, double[] obsMinusApriori_condition, double[][] inverseWeight = null)
{
Init(coeff_error, MatrixUtil.Create(obsMinusApriori_error), inverseWeight, coeff_condition, MatrixUtil.Create(obsMinusApriori_condition));
}
/// <summary>
/// 基于 Sinex 文件的分区平差。
/// </summary>
/// <param name="files">具有公共参数的分区</param>
public SinexBlockAdjust(params SinexFile[] files)
{
if (files.Length <= 1)
{
throw new ArgumentException("输入的SINEX文件数量不可少于2！");
}
this.files = files;
//检查一下是否只包含坐标，如否，则清理。
foreach (var item in files)
{
if (!item.IsOnlyEstimateCoordValue)
{
item.CleanNonCoordSolutionValue();
}
}
//提取公共测站名称
List <string> sameSites = null; new List <string>();
foreach (var item in files)
{
if (sameSites == null)
{
sameSites = new List <string>(); sameSites.AddRange(item.GetSiteCods());
}
else
{
sameSites = SinexFile.GetSameSiteCodes(item, sameSites);
}
}
if (sameSites.Count == 0)
{
throw new ArgumentException("分区没有公共点，不可执行分区平差！");
}
//计算统一单位权中误差
double commonVarFactor = GetVarianceFactor(files);
//组建分区
DateTime from = DateTime.Now;
List <BlockAdjustItem> items = new List <BlockAdjustItem>();
foreach (var file in files)
{
//获取公共参数所在的编号,都转化为X，Y，Z分别的参数
List <int> commonParamIndexes = file.GetParamIndexes(sameSites); //公共参数的索引
List <int> blockParamIndexes = file.GetParamIndexesExcept(sameSites); //区内参数的索引
List <int> newIndexes = new List <int>(blockParamIndexes); //新索引顺序
newIndexes.AddRange(commonParamIndexes);
int blockParamCount = blockParamIndexes.Count;
int obsCount = file.EstimateParamCount;
int commonParamCount = commonParamIndexes.Count;
//分区内参数系数阵 obsCount x paramCount
double[][] coeffA = MatrixUtil.Create(obsCount, blockParamCount);
for (int i = 0; i < blockParamCount; i++)
{
coeffA[i][i] = 1.0;
}
//观测值 obsCount x 1
double[][] obs = MatrixUtil.Create(obsCount, 1);
for (int i = 0; i < obsCount; i++)
{
if (i < blockParamCount)//区内参数
{
obs[i][0] = file.SolutionEstimateBlock.Items[blockParamIndexes[i]].ParameterValue;
}
else//公共参数
{
obs[i][0] = file.SolutionEstimateBlock.Items[commonParamIndexes[i - blockParamCount]].ParameterValue;
}
}
//观测值权逆阵 obsCount x obsCount
//注意：此处应该统一化权阵
double[][] inverseOfObs = null;
if (file.HasEstimateCovaMatrix)
{
inverseOfObs = file.GetEstimateCovaMatrix();
MatrixUtil.SymmetricExchange(inverseOfObs, newIndexes);
MatrixUtil.Multiply(inverseOfObs, commonVarFactor / file.GetStatistic().VarianceOfUnitWeight);
}
//else if (file.HasNormalEquationMatrix && file.HasNormalEquationVectorMatrix)
//{
// //方法待验证
// double[][] normal = file.GetNormalEquationMatrix();
// double[] righHand = file.GetNormalEquationVector();
// Geo.Algorithm.Matrix n = new Geo.Algorithm.Matrix(normal);
// Geo.Algorithm.Matrix u = new Geo.Algorithm.Matrix(MatrixUtil.Create( righHand));
// inverseOfObs = n.Inverse.Array;
// double lTpl = file.GetStatistic().WeightedSqureSumOfOMinusC;
// double vTpv = lTpl - (u.Transpose() * n * u)[0,0];
// double varFactor = vTpv / file.GetStatistic().NumberOfDegreesOfFreedom;
// MatrixUtil.Multiply(inverseOfObs, varFactor);
// MatrixUtil.SymmetricExchange(inverseOfObs, newIndexes);
// MatrixUtil.Multiply(inverseOfObs, commonVarFactor / file.GetStatistic().VarianceOfUnitWeight);
//}
else//去相关
{
inverseOfObs = MatrixUtil.Create(obsCount);
for (int i = 0; i < obsCount; i++)
{
if (i < blockParamCount)//区内参数
{
inverseOfObs[i][i] = Math.Pow(file.SolutionEstimateBlock.Items[blockParamIndexes[i]].StdDev, 2);
}
else//公共参数
{
inverseOfObs[i][i] = Math.Pow(file.SolutionEstimateBlock.Items[commonParamIndexes[i - blockParamCount]].StdDev, 2);
}
}
MatrixUtil.Multiply(inverseOfObs, commonVarFactor / file.GetStatistic().VarianceOfUnitWeight);
}
//分区内对公共参数的系数阵 obsCount x commonParamCount
double[][] coeffB = MatrixUtil.Create(obsCount, commonParamCount);
for (int i = blockParamCount; i < obsCount; i++)
{
coeffB[i][i - blockParamCount] = 1;
}
BlockAdjustItem item = new BlockAdjustItem(coeffA, obs, inverseOfObs, coeffB);
items.Add(item);
}
ba = new BlockAdjustment(items.ToArray(), false);
//创建结果SINEX文件，文件只包含公共参数的内容
ResultSinexFile = new SinexFile("Gnsser");
//测站名称
int commonSiteCount = ba.CommonParamCount / 3;
List <SiteId> siteIds = new List <SiteId>();
for (int i = 0; i < commonSiteCount; i++)
{
siteIds.Add(files[0].GetSiteId(sameSites[i]));
}
ResultSinexFile.SiteIdBlock = SinexFactory.CreateSiteIdBlock(siteIds);
//统计信息
SinexStatistic statistic = new SinexStatistic()
{
NumberOfUnknown = ba.CommonParamCount,
NumberOfDegreesOfFreedom = ba.Freedom,
VarianceOfUnitWeight = ba.VarianceOfUnitWeight,
SquareSumOfResidualsVTPV = ba.SquareSumOfResidualsVTPV,
NumberOfObservations = ba.ObsCount
};
ResultSinexFile.SolutionStattisticsBlock =
SinexFactory.CreateSolutionStattisticsBlock(statistic.GetSolutionStatistics());
//估值
List <SolutionValue> estList = new List <SolutionValue>();
Time refEchop = files[0].SolutionEstimateBlock.Items[0].RefEpoch;
for (int i = 0; i < commonSiteCount; i++)
{
int paramIndex = i * 3;
SolutionValue svX = new SolutionValue(paramIndex + 1, siteIds[i].SiteCode, "STAX", ba.CommonParams[paramIndex + 0][0], refEchop, ba.CommonParamRmsVector[paramIndex + 0]);
SolutionValue svY = new SolutionValue(paramIndex + 2, siteIds[i].SiteCode, "STAY", ba.CommonParams[paramIndex + 1][0], refEchop, ba.CommonParamRmsVector[paramIndex + 1]);
SolutionValue svZ = new SolutionValue(paramIndex + 3, siteIds[i].SiteCode, "STAZ", ba.CommonParams[paramIndex + 2][0], refEchop, ba.CommonParamRmsVector[paramIndex + 2]);
estList.AddRange(new SolutionValue[] {
svX, svY, svZ
});
}
ResultSinexFile.SolutionEstimateBlock = SinexFactory.CreateSolutionEstimateBlock(estList);
//协方差
ResultSinexFile.SolutionMatrixEstimateCova = SinexFactory.CreateSolutionMatrixEstimateCova(ba.CovaOfCommonParams);
this.TimeSpan = DateTime.Now - from;
}
