/// <summary>
/// Kalman滤波计算不变参数。
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
private AdjustResultMatrix GetSequentialConst(AdjustObsMatrix input)
{
WeightedVector appri = null;
Matrix B2 = input.BuildCoeefOfConstParam();
if (LastConstResult != null)
{
var IsEqual = Geo.Utils.ListUtil.IsEqual(LastConstResult.ParamNames, input.SecondParamNames);
if (IsEqual)
{
appri = LastConstResult.Estimated;
}
else
{
appri = SimpleAdjustMatrixBuilder.GetNewWeighedVectorInOrder(input.SecondParamNames, LastConstResult.Estimated);
}
}
if (appri == null)//第一次，使用参数平差结果
{
ParamAdjuster paramAdjuster = new ParamAdjuster();
var paramResult = paramAdjuster.Run(new AdjustObsMatrix(input.Observation, B2, null, input.SecondParamNames));
appri = paramResult.Estimated;
}
AdjustObsMatrix obsMatrix1 = new AdjustObsMatrix(appri, input.Observation, B2, input.SecondTransfer);
obsMatrix1.ParamNames = input.SecondParamNames;
//var kalmanFilter = new SimpleKalmanFilter();
var res = MatrixAdjuster.Run(obsMatrix1);
return(res);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//try
//{
this.ObsMatrix = input;
int paramCount = input.Coefficient.ColCount;
Predict(input.Transfer, InverseWeightOfTransfer);
var est1 = CorrectSimple(input.Observation - input.FreeVector, input.Coefficient, input.Observation.InverseWeight);
// var est2 = CorrectNormal(this.Observation, CoeffOfParam, Observation.InverseWeight);
// var est3 = NewCorrect(this.Observation, CoeffOfParam, Observation.InverseWeight);
//var differ12 = est1 - est2;
//var differ23 = est2 - est3;
//var differ13 = est1 - est3;
var Estimated = est1;
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetObsMatrix(input)
.SetFreedom(Freedom)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv)
;
return(result);
//}
//catch (Exception ex)
//{
// log.Error("SimpleKalmanFilter 滤波出错了", ex);
//}
}
public static AdjustObsMatrix BuildATtest(int paramCount, int obsCount, AdjustmentType adjustmentType = AdjustmentType.参数平差)
{
bool isParamOrCondition = adjustmentType == AdjustmentType.参数平差;
int restCount = obsCount - paramCount;
int rowOfCoeef = isParamOrCondition ? obsCount : restCount;
int colOfCoeef = isParamOrCondition ? paramCount : obsCount;
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.Observation = WeightedVector.GenerateATest(obsCount);
obsMatrix.Apriori = WeightedVector.GenerateATest(paramCount);
obsMatrix.Coefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
obsMatrix.Transfer = new WeightedMatrix(Matrix.CreateIdentity(paramCount), Matrix.CreateIdentity(paramCount));
obsMatrix.FreeVector = new Vector(rowOfCoeef, 1);
obsMatrix.SecondFreeVector = new Vector(paramCount, 1);
obsMatrix.ApproxVector = new Vector(paramCount, 1);
obsMatrix.SecondApproxVector = new Vector(paramCount, 1);
if (adjustmentType == AdjustmentType.具有参数的条件平差)
{
obsMatrix.SecondCoefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
}
return(obsMatrix);
}
/// <summary>
/// 实时计算，数据计算
/// </summary>
/// <param name="input">观测矩阵</param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
if (input == this.ObsMatrix)
{
return(this.LastResult);
}
this.ObsMatrix = input;
switch (StepOfRecursive)
{
case StepOfRecursive.SuperposOfConstNeq:
return(GetConstParamResult(input, NormalEquationSuperposer));
//case StepOfRecursive.ComputeMutableParam: break;
case StepOfRecursive.SequentialConst:
AdjustResultMatrix res = GetSequentialConst(input);
this.LastConstResult = res;
return(res);
case StepOfRecursive.RealTime: //实时计算，参数变化可能带来错误
AdjustResultMatrix result = GetRealTimeResult(input);
this.LastResult = result;
return(result);
case StepOfRecursive.ParamAdjust: //参数逐历元平差
return(GetSimpleParamAdjustResult(input)); //参数平差验证,2018.10.15, czs, hmx, 验证第一个结果是一样的。
default:
break;
}
return(null);
}
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
var result = ConditionalAdjuster.Run(input);
throw new NotImplementedException();
}
private AdjustResultMatrix GetRealTimeResult(AdjustObsMatrix input)
{
//构建不变参数的法方程
var newConstParamNe = input.BuildConstParamNormalEquation();
NormalEquationSuperposer.Add(newConstParamNe); //添加到法方程迭加器中
WeightedVector estY = NormalEquationSuperposer.GetEstimated();
AdjustResultMatrix result = Step3GetMutableX(estY, ObsMatrix); //求异变参数
return(result);
}
/// <summary>
/// 运行
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//原始输入
Matrix B = new Matrix(input.Coefficient);
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight);
Matrix B0 = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;//B0
Matrix PL = QL.Inversion;
int freedom = B.RowCount;
Matrix BT = B.Trans;
int obsCount = L.RowCount;
int paramCount = 0;
Matrix W = -(B * L - B0);
Matrix N = B * QL * BT;
Matrix inverN = N.Inversion;
Matrix K = inverN * W;
Matrix Vhat = (QL * BT * K);
Matrix Qvhat = QL * BT * inverN * B * QL;
WeightedVector estLW = new WeightedVector(Vhat, Qvhat)
{
ParamNames = input.Observation.ParamNames
};
Matrix Lhat = L + Vhat;
Matrix QhatL = QL - Qvhat;
WeightedVector correctedObs = new WeightedVector(Lhat, QhatL)
{
ParamNames = input.Observation.ParamNames
};
double vtpv = (Vhat.Trans * PL * Vhat).FirstValue;
double s0 = vtpv / freedom;//单位权中误差估值
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.条件平差)
.SetEstimated(estLW)
.SetCorrectedObs(correctedObs)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 计算
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//命名规则：0表示上一个，1表示预测，无数字表示当次
//上次次观测设置
var X0 = new Matrix((IMatrix)input.Apriori);
var Qx0 = new Matrix(input.Apriori.InverseWeight);
var Px0 = new Matrix(Qx0.GetInverse());
//本次观测设置
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 paramCount = A.ColCount;
int obsCount = A.RowCount;
//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 Freedom = input.Observation.Count - input.ParamCount + input.Apriori.Count;
var V = A * dX - V1; //估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) = A * deltaX - l.
var vtpv = (V.Trans * Po * V)[0, 0];
var VarianceOfUnitWeight = vtpv / Freedom; //单位权方差
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv)
;
return(result);
}
/// <summary>
/// 恢复参数平差。
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
private static AdjustResultMatrix GetSimpleParamAdjustResult(AdjustObsMatrix input)
{
#region 合并系数阵
Matrix A = new Matrix(input.Coefficient.RowCount, input.Coefficient.ColCount + input.SecondCoefficient.ColCount);
A.SetSub(input.Coefficient);
A.SetSub(input.SecondCoefficient, 0, input.Coefficient.ColCount);
var names = new List <string>();
names.AddRange(input.ParamNames);
names.AddRange(input.SecondParamNames);
#endregion
ParamAdjuster paramAdjuster = new ParamAdjuster();
AdjustObsMatrix adjustObs = new AdjustObsMatrix(input.Observation, A, null, names);
var resultMatrix = paramAdjuster.Run(adjustObs);
return(resultMatrix);
}
/// <summary>
/// 解析
/// </summary>
/// <param name="text"></param>
/// <param name="splitterOfMatrixItems">分隔矩阵元素的分隔符</param>
/// <returns></returns>
public static AdjustObsMatrix Parse(string text, string[] splitterOfMatrixItems = null)
{
AdjustObsMatrix obs = new AdjustObsMatrix();
if (splitterOfMatrixItems == null)
{
splitterOfMatrixItems = new string[] { ",", ";", "\t", " " };
}
string[] lines = text.Split(new char[] { '\r', '\n' }, StringSplitOptions.None); //保留空格以分隔
//解析本对象属性
ParseHeader(obs, lines);
int len = lines.Length;
var typeNames = Enum.GetNames(typeof(ObsMatrixType));
StringBuilder currentBlockSb = new StringBuilder();
ObsMatrixType currentType = ObsMatrixType.Apriori;
ObsMatrixType nextType = ObsMatrixType.Apriori;
for (int i = 0; i < len; i++)
{
string line = lines[i].Trim(); //当前行
if (typeNames.Contains(line)) //新区
{
currentType = nextType; //
nextType = Geo.Utils.EnumUtil.Parse <ObsMatrixType>(line); //这是解析的下一个区的名称
if (currentBlockSb.Length > 2)
{
ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType);
}
continue;
}
currentBlockSb.AppendLine(line);
}
if (currentBlockSb.Length > 2)
{
currentType = nextType;//
//解析最后一个
ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType);
}
return(obs);
}
/// <summary>
/// 解析为对象
/// </summary>
/// <param name="splitterOfMatrixItems"></param>
/// <param name="obs"></param>
/// <param name="currentBlockSb"></param>
/// <param name="currentType"></param>
private static void ParseBufferText(string[] splitterOfMatrixItems, AdjustObsMatrix obs, StringBuilder currentBlockSb, ObsMatrixType currentType)
{
switch (currentType)
{
case ObsMatrixType.Apriori:
obs.Apriori = WeightedVector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.Observation:
obs.Observation = WeightedVector.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.Coefficient:
obs.Coefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.SecondCoefficient:
obs.SecondCoefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.Transfer:
obs.Transfer = WeightedMatrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
break;
case ObsMatrixType.ApproxVector:
obs.ApproxVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.SecondApproxVector:
obs.SecondApproxVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.FreeVector:
obs.FreeVector = Vector.Parse(currentBlockSb.ToString());
break;
case ObsMatrixType.SecondFreeVector:
obs.SecondFreeVector = Vector.Parse(currentBlockSb.ToString());
break;
default:
break;
}
currentBlockSb.Clear();
}
/// <summary>
/// 条件平差法解算固定解，将固定解当成虚拟观测量,对原浮点解进行约束，条件平差。
/// </summary>
/// <param name="coeffOfParam">系数阵,条件方程构造</param>
/// <param name="totalFloat">原浮点解</param>
/// <param name="fixedObs">已经固定的参数，固定解当成虚拟观测量</param>
/// <returns></returns>
public static WeightedVector SolveAmbiFixedResultByConditionAdjust(WeightedVector totalFloat, Vector fixedObs, IMatrix coeffOfParam)
{
//以下算法已经验证等价！！2018.09.02， czs, hmx
bool isSong = false;
WeightedVector NewEstimated = null;
if (isSong)
{
#region 求固定解 宋力杰方法
IMatrix X_old = totalFloat;
IMatrix QX_old = totalFloat.InverseWeight;
IMatrix coeffOfParamT = coeffOfParam.Transposition;
IMatrix W = coeffOfParam.Multiply(X_old).Minus(new VectorMatrix(fixedObs));
IMatrix tmp = coeffOfParam.Multiply(QX_old).Multiply(coeffOfParamT);
IMatrix Nadd = (QX_old.Multiply(coeffOfParamT)).Multiply(tmp.GetInverse());
IMatrix X_new = X_old.Minus(Nadd.Multiply(W));
IMatrix tmp2 = Nadd.Multiply(coeffOfParam);
IMatrix QX_new = QX_old.Minus(tmp2.Multiply(QX_old));
NewEstimated = new WeightedVector(X_new, QX_new)
{
ParamNames = coeffOfParam.ColNames
};
#endregion
}
else
{
//条件平差
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(totalFloat).SetFreeVector(fixedObs);
ConditionalAdjuster adjuster = new ConditionalAdjuster();
var resultMatrix = adjuster.Run(obsMatrix);
NewEstimated = resultMatrix.CorrectedObs;
}
return(NewEstimated);
}
/// <summary>
/// 批量总体技术。参数不要改变，否则达不到预期效果。
/// </summary>
/// <param name="inputs"></param>
/// <returns></returns>
public List <AdjustResultMatrix> Run(List <AdjustObsMatrix> inputs)
{
AdjustObsMatrix firstMatrix = inputs[0];
Matrix Y0all = firstMatrix.HasSecondApprox ? new Matrix(firstMatrix.SecondApproxVector, true) : null;
WeightedVector estY = GetConstY(inputs);
//Matrix Y = Y0all + constY;
//step 3：求易变参数
List <AdjustResultMatrix> results = new List <AdjustResultMatrix>();
foreach (var obsMatrix in inputs)
{
this.ObsMatrix = obsMatrix;
AdjustResultMatrix result = Step3GetMutableX(estY, obsMatrix);
results.Add(result);
}
return(results);
}
/// <summary>
/// 解析本对象属性
/// </summary>
/// <param name="obs"></param>
/// <param name="lines"></param>
private static void ParseHeader(AdjustObsMatrix obs, string[] lines)
{
var headerSpliter = new char[] { ':', '：' };
var headerItemSpliter = new char[] { ',', ' ', '\t', ';' };
foreach (var line in lines)
{
if (String.IsNullOrEmpty(line))
{
continue;
}
if (line.Contains("×"))
{
break;
}
var items = line.Split(headerSpliter, StringSplitOptions.RemoveEmptyEntries);
if (items.Length == 1)
{
continue;
}
var header = items[0];
var content = items[1];
switch (header)
{
case "ParamNames":
obs.ParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries));
break;
case "SecondParamNames":
obs.SecondParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries));
break;
default: break;
}
}
}
/// <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);
}
/// <summary>
/// 参数加权平差
/// </summary>
/// <param name="input"></param>
/// <returns></returns>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//下标 o 表示观测值，x表示估计值，xa0表示具有先验信息的随机参数,
//xa为包含先验信息xa0的矩阵，是在整个误差方程中计算的矩阵
var Freedom = input.ObsCount - (input.ParamCount - input.Apriori.RowCount); // n -tb
Matrix A = new Matrix(input.Coefficient);
Matrix AT = A.Trans;
Matrix L = new Matrix((IMatrix)input.Observation);
Matrix Qo = new Matrix(input.Observation.InverseWeight);
Matrix Po = Qo.Inversion;
int obsCount = L.RowCount;
int paramCount = A.ColCount;
//具有先验信息的随机参数
Matrix Xa0 = new Matrix((IMatrix)input.Apriori);
Matrix Qxa0 = new Matrix(input.Apriori.InverseWeight);
Matrix Pxa0 = Qxa0.Inversion;
//计算先验信息的平差矩阵部分
Matrix Nxa0 = new Matrix(input.ParamCount);
Nxa0.SetSub(Pxa0);
Matrix Uxa0 = Pxa0 * Xa0;
Matrix Uxa = new Matrix(input.ParamCount, 1);
Uxa.SetSub(Uxa0);
//法方程系数阵
Matrix N = AT * Po * A + Nxa0;
Matrix InverN = N.Inversion;
//法方程右手边
Matrix U = AT * Po * L + Uxa;
//计算估值
Matrix X = InverN * U;
//精度估计
Matrix Xa = X.GetSub(0, 0, Xa0.RowCount);
Matrix dXa = Xa - Xa0;
Matrix V = A * X - L;
Matrix VT = V.Trans;
var vtpv = (VT * Po * V + dXa.Trans * Pxa0 * dXa).FirstValue;
var VarianceOfUnitWeight = vtpv / Freedom;
var Estimated = new WeightedVector(X, InverN)
{
ParamNames = input.ParamNames
};
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetObsMatrix(input)
.SetFreedom(Freedom)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <summary>
/// 数据处理。全部转化为计算偏移量，即，各种参数采用近似值，此处需要考虑初值情况。
/// </summary>
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//参数命名规则：下标 0 表示上一个，1 表示预测，无数字表示当次
#region 参数预测
WeightedVector apriori = input.Apriori;
if (apriori == null)
{
throw new ArgumentException("必须具有先验参数值。");
}
//先验值赋值
ArrayMatrix X0 = new ArrayMatrix(apriori); //上一次参数估值
ArrayMatrix Qx0 = new ArrayMatrix(apriori.InverseWeight.Array); //上一次估计误差方差权逆阵
ArrayMatrix Trans = new ArrayMatrix(input.Transfer.Array); //状态转移矩阵
ArrayMatrix TransT = Trans.Transpose();
ArrayMatrix Q_m = new ArrayMatrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声
//计算参数预测值，可以看做序贯平差中的第一组数据
ArrayMatrix X1 = Trans * X0;
ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
var Predicted = new WeightedVector(X1, Qx1)
{
ParamNames = input.ParamNames
}; //结果为残差
#endregion
//System.IO.File.WriteAllText(saveDir + @"\Predicted.txt", Predicted.ToFormatedText());
// System.IO.File.WriteAllText(saveDir + @"\Apriori.txt", Apriori.ToFormatedText());
#region 参数估计
ArrayMatrix A = new ArrayMatrix(input.Coefficient.Array); //误差方程系数阵
ArrayMatrix AT = A.Transpose(); //A 的转置
//估计值才需要观测值，而预测值不需要
//观测值赋值
WeightedVector obs = input.Observation - input.FreeVector;
if (obs == null)
{
throw new ArgumentException("必须具有观测向量。");
}
ArrayMatrix L = new ArrayMatrix(obs); //观测值，或 观测值 - 估计值，！！
ArrayMatrix Q_o = new ArrayMatrix(obs.InverseWeight.Array); //观测噪声权逆阵
ArrayMatrix P_o = Q_o.Inverse;
//计算预测的观测残差 自由项
//由 V = A X - L, 得 V = A x - l, l = L - A X0, X = X0 + x
ArrayMatrix dL = L - A * X1;//此处注意符号
ArrayMatrix QdL = Q_o + A * Qx1 * AT;
//计算平差值的权阵
ArrayMatrix PXk = AT * P_o * A + Qx1.Inverse;
//计算平差值的权逆阵
ArrayMatrix Qx = PXk.Inverse;
//计算增益矩阵
ArrayMatrix J = Qx * AT * P_o;
//计算参数改正值和估值
ArrayMatrix deltaX = J * dL;
ArrayMatrix X = X1 + deltaX;//改 X0 为 X1
//精度估计
ArrayMatrix UnitMatrix = ArrayMatrix.EyeMatrix(J.RowCount, 1.0);
ArrayMatrix B = UnitMatrix - J * A;
Qx = B * Qx1 * B.Transposition + J * Q_o * J.Transposition; //参数权逆阵
// Matrix Qx = (AT * P_o * A + Qx1.Inverse).Inverse;
var Estimated = new WeightedVector(X, Qx)
{
ParamNames = input.ParamNames
};
#endregion
#region 验后观测残差
// Matrix Px = Qx.Inverse;
ArrayMatrix V = L - A * X;
this.PostfitObservation = new Vector(MatrixUtil.GetColVector(V.Array))
{
ParamNames = input.ParamNames
};
#endregion
#region 精度估计
this.SumOfObsCount += input.ObsCount;
var Freedom = SumOfObsCount - input.ParamCount;// input.Freedom;
//观测噪声权阵
ArrayMatrix V1TPV1 = deltaX.Transpose() * Qx1.Inverse * deltaX;
ArrayMatrix VTPV = V.Transpose() * P_o * V;
var vtpv = VTPV[0, 0];
double upper = (V1TPV1 + VTPV)[0, 0];
if (!DoubleUtil.IsValid(upper) || upper > 1e10 || upper < 0)
{
log.Debug("方差值无效！" + upper);
}
//赋值
var VarianceOfUnitWeight = Math.Abs(upper) / Freedom;
//System.IO.File.WriteAllText(saveDir + @"\Estimated.txt", Estimated.ToFormatedText());
//System.IO.File.WriteAllText(saveDir + @"\Observation.txt", Observation.ToFormatedText());
#endregion
AdjustResultMatrix result = new AdjustResultMatrix()
.SetEstimated(Estimated)
.SetFreedom(Freedom)
.SetObsMatrix(input)
.SetVarianceFactor(VarianceOfUnitWeight)
.SetVtpv(vtpv);
return(result);
}
/// <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>
/// 构建不变参数的计算结果。
/// 注意：需要控制参数的增减问题。
/// 基本思路：增加则插入，减少则删除，通过参数名称来控制。
/// </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);
}
/// <summary>
/// 第三步：计算易变参数
/// </summary>
/// <param name="estY"></param>
/// <param name="obsMatrix"></param>
/// <returns></returns>
private AdjustResultMatrix Step3GetMutableX(WeightedVector estY, AdjustObsMatrix obsMatrix)
{
Matrix constY = estY.GetVectorMatrix();
Matrix constQy = estY.InverseWeight;
//观测值权阵设置,对已知量赋值
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 = B.ColCount;
int mutableParamCount = A.ColCount;
int paramCount = fixedParamCount + mutableParamCount;
int freedom = obsCount - paramCount;
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;
//求x
//观测值更新,采用估值进行计算
Matrix lx = lxy - B * constY; // = L - (A * X0 + B * Y + D);
/* Matrix x = InverNa * ATPL * lx;*/
Matrix x = InverNa * (ATPL * lxy - Nab * constY); //这两个计算是等价的
Matrix X = X0 + x;
Matrix Ntmp = Na.Inversion * Nab;
Matrix Qx = InverNa + Ntmp * constQy * Ntmp.Trans;
//Matrix Qxy = AT * PL * B;
//Matrix Qtemp = Qx * Qxy;
//Matrix Dx = Qx + Qtemp * Dy * Qtemp.Trans;
//精度评定
Matrix V = A * x - lx;
Matrix Qv = QL - A * Qx * AT - B * constQy * BT;
// Matrix lT = l.Trans;
double vtpv = (V.Trans * PL * V).FirstValue; //(lT * PL * l - lT * PL * Ac * y).FirstValue;//
double s0 = vtpv / (freedom == 0 ? 0.1 : freedom); //单位权方差
WeightedVector estX = new WeightedVector(x, Qx)
{
ParamNames = obsMatrix.ParamNames
};
WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
{
ParamNames = obsMatrix.ParamNames
};
WeightedVector estV = new WeightedVector(V, Qv)
{
ParamNames = 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)
.SetSecondEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(obsMatrix)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <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 B = new Matrix(input.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
Matrix Y0 = input.HasSecondApprox ? new Matrix(input.SecondApproxVector, true) : null;
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = B.ColCount;
//观测值更新
Matrix l = L - (A * X0 + B * Y0 + D); //如果null，则是本身
Matrix ATPL = AT * PL;
//法方程
Matrix Na = ATPL * A;
Matrix Nab = AT * PL * B;
Matrix InverNa = Na.Inversion;
Matrix J = A * InverNa * AT * PL;
Matrix B2 = (Matrix.CreateIdentity(J.RowCount) - J) * B;
Matrix AcT = B2.Trans;
Matrix Nc = AcT * PL * B2;;
Matrix InverNc = Nc.Inversion;
//平差结果
Matrix y = InverNc * AcT * PL * l;
Matrix Y = Y0 + y;
Matrix Qy = InverNc;
//只针对y的精度评定
Matrix W = B2 * y - l;
double ys0 = (W.Trans * PL * W).FirstValue / (obsCount - fixedParamCount);
Matrix Dy = ys0 * Qy;
//求x
Matrix lx = L - (A * X0 + B * Y + D); //如果null，则是本身
Matrix x = InverNa * ATPL * lx; //这两个计算是等价的
// Matrix x = InverNa * (ATPL * l - Nab * y);//这两个计算是等价的
Matrix X = X0 + x;
Matrix Ntmp = Na.Inversion * Nab;
Matrix Qx = InverNa + Ntmp * Qy * Ntmp.Trans;
//Matrix Qxy = AT * PL * B;
//Matrix Qtemp = Qx * Qxy;
//Matrix QtempT = Qtemp.Trans;
//Matrix Dx = Qx + Qtemp * Dy * QtempT;
//精度评定
Matrix V = A * x + B * y - l;
Matrix Qv = QL - A * Qx * AT - B * Qy * BT;
// Matrix lT = l.Trans;
double vtpv = (V.Trans * PL * V)[0, 0];//(lT * PL * l - lT * PL * Ac * y).FirstValue;//
int paramCount = A.ColCount + B.ColCount;
int freedom = A.RowCount - paramCount;
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 estY = new WeightedVector(y, Qy)
{
ParamNames = input.SecondParamNames
};
WeightedVector estV = new WeightedVector(V, Qv);
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)
.SetSecondEstimated(estY)
.SetCorrection(estV)
.SetCorrectedObs(correctedObs)
.SetCorrectedEstimate(CorrectedEstimate)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
/// <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);
}
public override AdjustResultMatrix Run(AdjustObsMatrix input)
{
//u(0<u<t)个独立量为参数,多余观测数r=n-t
//原始输入 m=r+u
Matrix A = input.Coefficient; //m×t
Matrix B = input.SecondCoefficient; //m×n
Matrix L = new Matrix((IMatrix)input.Observation); //n×1
Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight); //m×m
Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null; //u×1
Matrix D = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null; //B0//m×1
Matrix B0 = input.HasSecondFreeVector ? new Matrix(input.SecondFreeVector, true) : null; //B0//m×1
Matrix AT = A.Trans;
Matrix PL = QL.Inversion;
Matrix BT = B.Trans;
//多余观测数为r=m-u=n-t
int obsCount = L.RowCount;
int totalConditionCount = B.RowCount;
int paramCount = A.ColCount;//u
int freedom = obsCount - paramCount;
//观测值更新
Matrix l = L - (A * X0 + D); //如果null，则是本身
Matrix W = -(B * X0 - B0);
Matrix N = AT * PL * A;
Matrix U = AT * PL * l;
Matrix inverN = N.Inversion;
// ************************
//采用分块矩阵直接计算
int dimOfX = N.ColCount; //X的维度
int dimOfK = B.RowCount; //K的维度
int dimOfBigN = dimOfK + dimOfX;
Matrix BigN = new Matrix(dimOfBigN, dimOfBigN);
BigN.SetSub(N);
BigN.SetSub(BT, 0, dimOfX);
BigN.SetSub(B, dimOfX, 0);
Matrix inverBigN = BigN.Inversion;
Matrix bigW = new Matrix(dimOfBigN, 1);
bigW.SetSub(U);
bigW.SetSub(W, U.RowCount);
Matrix bigX = inverBigN * bigW;
Matrix x = bigX.GetSub(0, 0, dimOfX);
Matrix K = bigX.GetSub(dimOfX, 0);
Matrix X = X0 + x;
Matrix V = A * x - l;
Matrix EstL = L + V;
//**************精度估计---------------
Matrix Q11 = inverBigN.GetSub(0, 0, dimOfX, dimOfX);
Matrix Q12 = inverBigN.GetSub(0, dimOfX, dimOfK);
Matrix Q21 = inverBigN.GetSub(dimOfX, 0, dimOfK, dimOfK);
Matrix Q22 = inverBigN.GetSub(dimOfX, dimOfX);
Matrix Qx = Q11;
Matrix QestL = A * Q11 * AT;
var coWeightedX = new WeightedVector(X, Qx);
var weightedX = new WeightedVector(x, Qx);
var weightedL = new WeightedVector(EstL, QestL);
double vtpv = (V.Trans * PL * V).FirstValue;
double s0 = vtpv / freedom;//单位权中误差估值
if (!DoubleUtil.IsValid(s0))
{
log.Error("方差值无效！" + s0);
}
AdjustResultMatrix result = new AdjustResultMatrix()
.SetAdjustmentType(AdjustmentType.具有参数的条件平差)
.SetEstimated(weightedX)
.SetCorrectedEstimate(coWeightedX)
.SetCorrectedObs(weightedL)
.SetObsMatrix(input)
.SetFreedom(freedom)
.SetVarianceFactor(s0)
.SetVtpv(vtpv);
return(result);
}
