/// <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>
/// </summary>
/// <param name="observation">观测值信息</param>
/// <param name="control">控制矩阵，有时为非对称阵，如PPP</param>
/// <param name="covaOfObs">观测值协方差</param>
private WeightedVector CorrectNormal(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);
/******* Normal method Start ********/
//计算增益矩阵
var temp = (Qo + (A * Q1 * AT)).Inversion; // (Qo.Plus(A.Multiply(Q1).Multiply(AT))).GetInverse();
var J = Q1 * AT * temp; //Q1.Multiply(AT).Multiply(temp);
//计算估计值
Matrix Vk1 = A * X1 - L; //计算新息向量A.Multiply(X1).Minus(L);
Matrix X = X1 - J * Vk1; //X1.Minus(J.Multiply(Vk1));
//计算平差值的权逆阵
var JA = J * A; // J.Multiply(A);
var temp2 = JA * Q1; //JA.Multiply(Q1);
var Qx = Q1 - temp2; //Q1.Minus(temp2);
//var maxtrix = new Matrix(Q1.Array) - new Matrix(temp2.Array);
//var differ2 = maxtrix.Minus(Qx);
//var I = DiagonalMatrix.GetIdentity(Q1.ColCount);
//var Qx2 = I.Minus(JA).Multiply(Q1);
//var differ = Qx2 .Minus(Qx);
/******* Normal method End ********/
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = ObsMatrix.ParamNames
};
//观测残差
BuildCovaFactor(L, A, Po, P1, X1, X);
return(Estimated);
}
/// <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 X1 = new Matrix(PredictParam);
//观测信息
Matrix L = new Matrix(observation);
Matrix B = new Matrix(control);
Matrix Qo = new Matrix(covaOfObs);
Matrix P1 = new Matrix(CovaOfPredictParam.GetInverse());
Matrix Po = new Matrix(covaOfObs.GetInverse());
//通过Cholesky分解，单位化
CholeskyDecomposition Cholesky1 = new CholeskyDecomposition(P1);
Matrix R0 = new Matrix((Cholesky1.LeftTriangularFactor.Transpose()));//
Matrix z0 = R0 * X1;
CholeskyDecomposition Cholesky2 = new CholeskyDecomposition(Po);
Matrix R = new Matrix((Cholesky2.LeftTriangularFactor.Transpose()));//
Matrix z = R * L;
Matrix A = R * B;
if (R0.ColCount != A.ColCount)
{
//
throw new Exception("What is wrong in SquareRootInformationFilter!");
}
//合并矩阵
Matrix ConbRA = ConbineMatrixByCol(R0, A);
Matrix ConbZ0Z = ConbineMatrixByCol(z0, z);
//正交化
HouseholderTransform HouseholderTransform = new HouseholderTransform(ConbRA);
Matrix T = HouseholderTransform.T;
//更新
Matrix newConbRA = T * ConbRA;
Matrix newConbZ0Z = T * ConbZ0Z;
//
//提取
Matrix newR0 = newConbRA.GetSub(0, 0, R0.RowCount, R0.ColCount);
Matrix newZ0 = newConbZ0Z.GetSub(0, 0, z0.RowCount, z0.ColCount);
//解算
Matrix newR0_Cov = new Matrix(newR0.GetInverse());
Matrix X = newR0_Cov * newZ0;
Matrix QX = newR0_Cov * newR0_Cov.Transpose();
BuildCovaFactor(L, B, Po, P1, X1, X);
if (false)
{
var III = newR0_Cov * newR0_Cov;
int i = 0;
i = 0;
}
var Estimated = new WeightedVector(X, QX)
{
ParamNames = ObsMatrix.ParamNames
};
return(Estimated);
}
/// <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 = 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);
}
