/// <summary>
/// 估计，改正。一个更健壮的方法。
/// </summary>
/// <param name="observation">观测值信息</param>
/// <param name="control">控制矩阵，有时为非对称阵，如PPP</param>
/// <param name="covaOfObs">观测值协方差</param>
private WeightedVector CorrectSimple(IMatrix observation, IMatrix control, IMatrix covaOfObs)
{
//简化字母表示
Matrix Q1 = new Matrix(CovaOfPredictParam);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix X1 = new Matrix(PredictParam);
Matrix L = new Matrix(observation);
Matrix A = new Matrix(control);
Matrix AT = new Matrix(A.Transposition);
Matrix Po = new Matrix(covaOfObs.GetInverse());
Matrix Qo = new Matrix(covaOfObs);
//平差值Xk的权阵
Matrix PXk = null;
Matrix Atpa = null;
if (Po.IsDiagonal)
{
Atpa = ATPA(A, Po);
}
else
{
Atpa = AT * Po * A;
}
PXk = new Matrix(SymmetricMatrix.Parse(Atpa)) + P1;
//计算平差值的权逆阵
Matrix Qx = PXk.Inversion;
Matrix J = Qx * AT * Po;
//计算平差值
Matrix Vk1 = A * X1 - L;//计算新息向量
Matrix X = X1 - J * Vk1;
X.RowNames = ObsMatrix.ParamNames;
BuildCovaFactor(L, A, Po, P1, X1, X);
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
return(Estimated);
}
/// <summary>
/// 数据计算
/// </summary>
/// <param name="input">观测矩阵</param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix QL = new Matrix(input.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(input.Coefficient);
Matrix AT = A.Trans;
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
Matrix dN = input.CoeffIncrementOfNormalEquation;
int obsCount = A.RowCount;
int paramCount = A.ColCount;
int freedom = obsCount - paramCount;
//观测值更新
Matrix l = L - (A * X0 + D); //如果null，则是本身
//法方程
Matrix N = new Matrix(SymmetricMatrix.Parse(AT * PL * A + dN));
Matrix U = AT * PL * l;
Matrix InverN = N.Inversion;
//平差结果
Matrix x = InverN * U;
Matrix Qx = InverN - dN;
//精度评定
Matrix V = A * x - l;
Matrix Qv = QL - A * Qx * AT;
Matrix X = X0 + x;
double vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差
WeightedVector estX = new WeightedVector(x, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
Matrix Lhat = L + V;
Matrix QLhat = A * Qx * AT;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = this.ObsMatrix.Observation.ParamNames
};
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.参数平差)
.SetEstimated(estX)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
