/// <summary>
/// 观测量的权逆阵，一个对角阵。
/// </summary>
/// <returns></returns>
private IMatrix BulidInverseWeightOfObs()
{
int row = this.ObsCount;
DiagonalMatrix inverseWeight = DiagonalMatrix.GetIdentity(row);
return(inverseWeight.Multiply(0.1 * 0.1));
}
/// <summary>
/// 具有约束条件的参数平差的无限权解法解算模糊度固定。
/// </summary>
/// <param name="totalFloat"></param>
/// <param name="fixedAmbiguities"></param>
/// <returns></returns>
public WeightedVector GetResultByWeighedParamAdjust(WeightedVector totalFloat, WeightedVector fixedAmbiguities)
{
if (fixedAmbiguities.Count == 0)
{
return(totalFloat);
}
int totalParamCount = totalFloat.Count; //待估参数个数
int fixedAmbiCount = fixedAmbiguities.Count;
//平差系数阵
var PA = new Matrix(totalFloat.Weights);
var LA = new Matrix((IVector)totalFloat, true)
{
RowNames = totalFloat.ParamNames
};
var A = DiagonalMatrix.GetIdentity(totalParamCount);//A x = l
var LB = new Matrix((IVector)fixedAmbiguities, true)
{
RowNames = fixedAmbiguities.ParamNames
};
var PB = new Matrix(fixedAmbiguities.Weights);
Matrix B = BuildCoeefOfFixedToFloat(totalFloat, fixedAmbiguities);
var BTBP = B.Trans * PB;
//A 为单位阵，不需要计算
var NA = PA; //AT * P * A
var UA = PA * LA; //AT * Ｐ * Ｌ
UA.ColNames = new List <string>()
{
"Names"
};
NA.ColNames = totalFloat.ParamNames;
UA.RowNames = totalFloat.ParamNames;
NA.RowNames = totalFloat.ParamNames;
var NB = BTBP * B;
var UB = BTBP * LB;
UB.ColNames = new List <string>()
{
"Names"
};
NB.ColNames = totalFloat.ParamNames;
UB.RowNames = totalFloat.ParamNames;
NB.RowNames = totalFloat.ParamNames;
MatrixEquation neA = new MatrixEquation(NA, UA);
MatrixEquation neB = new MatrixEquation(NB, UB);
var eq = neA + neB;
var result = eq.GetEstimated();
//整体矩阵验证, 2018.10.20, czs, in hmx, 已经验证与整体平差一致，但是如果权太大如1e40，则可能出现舍入误差，而失真！！
if (false)
{
Matrix AB = new Matrix(A.RowCount + B.RowCount, A.ColCount);
AB.SetSub(A);
AB.SetSub(B, A.ColCount);
Matrix PAB = new Matrix(PA.RowCount + PB.RowCount);
PAB.SetSub(PA);
PAB.SetSub(PB, PA.RowCount, PA.ColCount);
Vector LAB = new Vector(totalFloat);
LAB.AddRange(fixedAmbiguities);
ParamAdjuster paramAdjuster = new ParamAdjuster();
var result2 = paramAdjuster.Run(new AdjustObsMatrix(new WeightedVector(LAB, PAB.Inversion), AB));
}
return(result);
}
