/// <summary>
/// 预报
/// </summary>
/// <param name="TransferMatrix"></param>
/// <param name="InverseWeightOfTransfer"></param>
private void Predict(IMatrix TransferMatrix, IMatrix InverseWeightOfTransfer)
{
// int stateRow = AprioriParam.RowCount;
//这两个矩阵的作用是什么？？？？ 2014.12.14,czs, namu shaungliao,
//IMatrix controlMatrix = new Matrix(stateRow, 1, 0.0);
//IMatrix controlInputVector = new Matrix(1, 1, 0.0);
// Compute the a priori state vector
IMatrix PredictParam = TransferMatrix.Multiply(AprioriParam);//.Plus(controlMatrix.Multiply(controlInputVector));
//IMatrix tranposOfTranfer = TransferMatrix.Transposition;
// Compute the a priori estimate error covariance matrix
// IMatrix CovaOfPredictParam1 = TransferMatrix.Multiply(CovaOfAprioriParam).Multiply(tranposOfTranfer).Plus(InverseWeightOfTransfer);
//此处解算结果完全一致，但是 CovaOfPredictParam1 计算的是 对角阵，BQBT 计算的是三角阵。BQBT该判断是否对角阵后计算，更能体现优势。
//应该进一步测试比较内存消耗和执行效率，最后决定采用方法。??? 2016.10.21, czs noted.
IMatrix CovaOfPredictParam1 = AdjustmentUtil.BQBT(TransferMatrix, CovaOfAprioriParam).Plus(InverseWeightOfTransfer);
//IMatrix q = CovaOfPredictParam1.Minus(CovaOfPredictParam);
this.Predicted = new Geo.Algorithm.Adjust.WeightedVector(PredictParam, CovaOfPredictParam1)
{
ParamNames = ObsMatrix.ParamNames
};
}
/// <summary>
/// 快捷计算方法，返回 SymmetricMatrix
/// </summary>
/// <param name="A">系数阵</param>
/// <param name="P">对角阵</param>
/// <returns></returns>
public static Matrix ATPA(IMatrix A, IMatrix P)
{
return(new Matrix(AdjustmentUtil.ATPA(A, P)));
}
/// <summary>
/// 实用方法，Q为对称阵,速度较慢？摘抄自 宋力杰测量平差程序设计 P11
/// </summary>
/// <param name="B"></param>
/// <param name="Q">可能非对称</param>
/// <returns></returns>
public static IMatrix BQBT(IMatrix B, IMatrix Q)
{
return(AdjustmentUtil.BQBT(B, Q));
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//命名规则：0表示上一个（先验信息），1表示预测，无数字表示当次
//上次次观测设置
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
var X0 = new Matrix((IMatrix)input.Apriori);
//本次观测设置
var Qo = new Matrix(input.Observation.InverseWeight);
var Po = new Matrix(Qo.GetInverse());
var A = new Matrix(input.Coefficient);
var AT = A.Trans;
var L = new Matrix((IMatrix)input.Observation);
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有状态转移的序贯平差
//if (input.Transfer != null && input.InverseWeightOfTransfer!=null)
//{
// Matrix Trans = new Matrix(input.Transfer.Array); //状态转移矩阵
// Matrix TransT = Trans.Transpose();
// Matrix Q_m = new Matrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
// //计算参数预测值，可以看做序贯平差中的第一组数据
// //ArrayMatrix X1 = Trans * X0;
// //ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
// X0 = Trans * X0;
// Qx0 = Trans * Qx0 * TransT + Q_m;
// Px0 = Qx0.Inversion;
//}
//1.预测残差
//计算预测残差
var V1 = L - A * X0; //观测值 - 估计近似值
var Qv1 = Qo + A * Qx0 * AT; //预测残差方差
//2.计算增益矩阵
var J = Qx0 * AT * Qv1.Inversion; // 增益矩阵
//3.平差结果
var dX = J * V1; //参数改正
var X = X0 + dX;
//4.精度评定
var Qx = Qx0 - J * A * Qx0;
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;
// var V = A * dX - V1;//估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) = A * deltaX - l.
// this.VarianceOfUnitWeight = (V.Trans * Po * V).FirstValue / Freedom;//单位权方差
Matrix V = A * X - L;
Matrix Vx = X - X0;
Matrix VTPV = null;
if (Po.IsDiagonal)
{
VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
}
else
{
VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
}
var vtpv = VTPV[0, 0];
this.SumOfVptv += vtpv;
this.SumOfObsCount += A.RowCount;
var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 估计，改正。通常采用的方法。From 崔阳、2017.06.22
/// </summary>
/// <param name="observation"></param>
/// <param name="control"></param>
/// <param name="covaOfObs"></param>
/// <returns></returns>
private WeightedVector NewCorrect(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);
//计算新息向量
Matrix Vk1 = A * X1 - L;
Matrix PXk = null;
if (Q1.IsDiagonal)
{
var atpa = new Matrix(AdjustmentUtil.ATPA(AT, Q1));
//IMatrix at = AT.Multiply(P_o).Multiply(control);
PXk = new Matrix(atpa + Qo);
}
else
{
PXk = A * Q1 * AT + Qo;//平差值Xk的权阵
}
//计算平差值的权逆阵
Matrix CovaOfP = PXk.Inversion;//.GetInverse();
//计算增益矩阵
//IMatrix J = Q1.Multiply(AT).Multiply(CovaOfP);
Matrix J = Q1 * AT * CovaOfP;
//计算平差值
//IMatrix X = PredictParam.Minus(J.Multiply(Vk1));
Matrix X = X1 - J * Vk1;
Matrix I = Matrix.CreateIdentity(Q1.ColCount);
#region 理论公式
Matrix t2 = J * A; // (J.Multiply(A));
Matrix t3 = I - t2; // I.Minus(t2);
Matrix Qx = t3 * Q1; // (t3).Multiply(Q1);
#endregion
#region 阮论文公式
//IMatrix t21 = I.Minus(J.Multiply(control));
//IMatrix t22 = I.Minus(controlT.Multiply(J.Transposition));
//IMatrix t3 = J.Multiply(covaOfObs.Multiply(J.Transposition));
//IMatrix CovaOfEstParam = ((t21).Multiply(CovaOfPredictParam).Multiply(t22)).Plus(t3);
#endregion
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
BuildCovaFactor(L, A, Po, P1, X1, X);
return(Estimated);
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
this.ObsMatrix = input;
//命名规则：0表示上一个（先验信息），1表示预测，无数字表示当次
//上次次观测设置
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
var X0 = new Matrix((IMatrix)input.Apriori);
//本次观测设置
var Qo = new Matrix(input.Observation.InverseWeight);
var Po = new Matrix(Qo.GetInverse());
var A = new Matrix(input.Coefficient);
var AT = A.Trans;
var L = new Matrix((IMatrix)input.Observation);
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有状态转移的序贯平差
if (input.Transfer != null && input.InverseWeightOfTransfer != null)
{
Matrix Trans = new Matrix(input.Transfer.Array); //状态转移矩阵
Matrix TransT = Trans.Transpose();
Matrix Q_m = new Matrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
//计算参数预测值，可以看做序贯平差中的第一组数据
//ArrayMatrix X1 = Trans * X0;
//ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
//更新先验值
X0 = Trans * X0;
Qx0 = Trans * Qx0 * TransT + Q_m;
Px0 = Qx0.Inversion;
}
var ATP = AT * Po;
var N = ATP * A;
var U = ATP * L;
var Px = (N + Px0);
var Qx = Px.Inversion;
var X = Qx * (U + Px0 * X0);
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;
Matrix V = A * X - L;
Matrix Vx = X - X0;
Matrix VTPV = null;
if (Po.IsDiagonal)
{
VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
}
else
{
VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
}
var vtpv = VTPV[0, 0];
this.SumOfObsCount += A.RowCount;
var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
