private AdjustResultMatrix GetRealTimeResult(AdjustObsMatrix input)
{
//构建不变参数的法方程
var newConstParamNe = input.BuildConstParamNormalEquation();
NormalEquationSuperposer.Add(newConstParamNe); //添加到法方程迭加器中
WeightedVector estY = NormalEquationSuperposer.GetEstimated();
AdjustResultMatrix result = Step3GetMutableX(estY, ObsMatrix); //求异变参数
return(result);
}
private static WeightedVector Step2GetConstY(List <MatrixEquation> normals)
{
NormalEquationSuperposer NormalEquationSuperPoser = new NormalEquationSuperposer();
//step 2： 求固定参数
foreach (var normal in normals)
{
NormalEquationSuperPoser.Add(normal);
}
return(NormalEquationSuperPoser.GetEstimated());
}
/// <summary>
/// 构造函数
/// </summary>
public RecursiveAdjuster()
{
NormalEquationSuperposer = new NormalEquationSuperposer();
MatrixAdjuster = new KalmanFilter();
this.StepOfRecursive = StepOfRecursive.SuperposOfConstNeq;
}
/// <summary>
/// 构建不变参数的计算结果。
/// 注意：需要控制参数的增减问题。
/// 基本思路：增加则插入，减少则删除，通过参数名称来控制。
/// </summary>
/// <param name="obsMatrix"></param>
/// <param name="NormalEquationSuperposer"></param>
/// <returns></returns>
public static AdjustResultMatrix GetConstParamResult(AdjustObsMatrix obsMatrix, NormalEquationSuperposer NormalEquationSuperposer)
{
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)obsMatrix.Observation);
Matrix QL = new Matrix(obsMatrix.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(obsMatrix.Coefficient);
Matrix AT = A.Trans;
Matrix B = new Matrix(obsMatrix.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = obsMatrix.HasApprox ? new Matrix(obsMatrix.ApproxVector, true) : null;
Matrix Y0 = obsMatrix.HasSecondApprox ? new Matrix(obsMatrix.SecondApproxVector, true) : null;
Matrix D = obsMatrix.HasFreeVector ? new Matrix(obsMatrix.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = obsMatrix.SecondParamNames.Count;// B.ColCount;
int freedom = obsCount - fixedParamCount;
//观测值更新
Matrix lxy = L - (A * X0 + B * Y0 + D); //采用估值计算的观测值小量
Matrix ATPL = AT * PL;
//法方程
Matrix Na = ATPL * A;
Matrix Nab = AT * PL * B;
Matrix InverNa = Na.Inversion;
Matrix J = A * InverNa * AT * PL;
Matrix I = Matrix.CreateIdentity(J.RowCount);
Matrix B2 = (I - J) * B; //新的系数阵 Ac, 原文中为 B波浪~
Matrix B2T = B2.Trans;
Matrix B2TPL = B2T * PL;
Matrix NofB2 = B2TPL * B2;
Matrix UofB2 = B2TPL * lxy;
NofB2.ColNames = obsMatrix.SecondParamNames;
NofB2.RowNames = obsMatrix.SecondParamNames;
UofB2.RowNames = obsMatrix.SecondParamNames;
UofB2.ColNames = new List <string>()
{
"ConstParam"
};
//生成法方程
var ne = new MatrixEquation(NofB2, UofB2);
//叠加法方程
NormalEquationSuperposer.Add(ne);//添加到法方程迭加器中
var acNe = NormalEquationSuperposer.CurrentAccumulated;
Matrix inverN = acNe.N.Inversion;
Matrix y = inverN * acNe.U;
y.RowNames = acNe.ParamNames;
Matrix Qy = inverN;
Qy.ColNames = acNe.ParamNames;
Qy.RowNames = acNe.ParamNames;
var estY = new WeightedVector(y, Qy)
{
ParamNames = acNe.ParamNames
};
var V = B2 * y - lxy;
Matrix Qv = QL - B2 * Qy * B2T;
Matrix Y = Y0 + y;
var vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差
WeightedVector CorrectedEstimate = new WeightedVector(Y, Qy);
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = obsMatrix.Observation.ParamNames
};
Matrix Lhat = L + V;
Matrix QLhat = B2 * Qy * B2T;
var correctedObs = new WeightedVector(Lhat, QLhat)
{
ParamNames = obsMatrix.Observation.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.递归最小二乘)
.SetEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(obsMatrix)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
