private void FirstPhaseHalfPivot(MatrixEquation <double> eq)
{
//Pierwsza faza eliminacji Gaussa, z częściowym wyborem elementu podstawowego
//Tworzy z macierzy A macierz trójkątną górną oraz dzieli wiersze tak, aby uzyskać jedynki wiodące
//Przenosi operacje na macierz B
for (int i = 0; i < eq.A.ColCount; i++)
{
//Wybór wiodącego elementu
int max = FindMaxInColumn(eq.A, i, i, eq.A.RowCount);
eq.A.SwapRows(i, max);
eq.B.SwapRows(i, max);
if (eq.A.ValueMatrix[i][i].CompareTo(MatrixDouble.ONE) != 0) //Pierwsza optymalizacja - jeśli wiodąca liczba jest jedynką, to wiersz nie jest już mnożony
{
eq.B.ValueMatrix[i] = Matrix <double> .MultiplyRow(eq.B.ValueMatrix[i], (eq.A.ValueMatrix[i][i].GetInverse()));
eq.A.ValueMatrix[i] = Matrix <double> .MultiplyRow(eq.A.ValueMatrix[i], (eq.A.ValueMatrix[i][i].GetInverse()));
}
for (int j = i + 1; j < eq.A.RowCount; j++)
{
if (eq.A.ValueMatrix[j][i].CompareTo(MatrixDouble.ZERO) != 0)
{
eq.B.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.B.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.B.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
eq.A.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.A.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.A.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
}
}
}
}
/// <summary>
/// 用MatrixEquation得到的预测值
/// </summary>
/// <param name="str">实验号</param>
/// <returns>预测值</returns>
public static List <DataValue> getAllPredictValue(String str)
{
List <DataValue> lists = new List <DataValue>();
List <double> a = SqlData.sqlGetData("dbo.小麦质量特性参数数据表", "CanNumberValue", str);
List <DateTime> dateTime = SqlData.sqlDataDate("dbo.小麦质量特性参数数据表", "Date", str);
List <double> grainTemp = SqlData.sqlDataDoubleValue("dbo.小麦质量特性参数数据表", "GrainTemp", str);
List <double> WaterValue = SqlData.sqlDataDoubleValue("dbo.小麦质量特性参数数据表", "Water", str);
String canShuNumber = SqlData.getCanShuNumber("dbo.小麦参数实验总概括表", "CanShuNumber", str);
String canShuName = SqlData.getCanShuName("dbo.小麦参数名表", "CanShuName", canShuNumber);
double[] douTemp = grainTemp.ToArray();
double[] col0 = a.ToArray();
DateTime[] date = dateTime.ToArray();
double[] water = WaterValue.ToArray();
List <double> dayDou = new List <double>();//将有数据的日期转换为double类型
for (int i = 0; i < date.Length; i++)
{
dayDou.Add(date[i].ToOADate());
}
List <double> doubleDay = new List <double>();
for (double j = dayDou[0]; j <= dayDou[dayDou.Count - 1]; j++)
{
doubleDay.Add(j);
}
double[] ceshi = spline.setSplineData(dayDou.ToArray(), col0, doubleDay.ToArray());//实际值
MatrixEquation mart = new MatrixEquation();
double[] ceshi2 = mart.MultiLine(dayDou.ToArray(), col0, col0.Length, 3);
int index = -1;
DateTime dDay = date[0];
for (int i = 0; i < doubleDay.Count; i++)
{
DataValue datapoint = new DataValue();
double y1 = ceshi2[0] + ceshi2[1] * doubleDay[i] + ceshi2[2] * doubleDay[i] * doubleDay[i] + ceshi2[3] * doubleDay[i] * doubleDay[i] * doubleDay[i];
datapoint.PredictData = y1;
if (i % 30 == 0)
{
index++;
}
datapoint.GrainTemp = douTemp[index];
datapoint.Water = water[index];
datapoint.date = dDay.AddDays(1);
datapoint.CanShuNumber = canShuNumber;
datapoint.yLable = canShuName;
dDay = datapoint.date;
datapoint.ChaValue = ceshi[i] - y1;
datapoint.Y = ceshi[i];
lists.Add(datapoint);
}
return(lists);
}
public void Perform(MatrixEquation <double> eq)
{
Matrix <double> oldVector = new Matrix <double>(eq.B);
for (int i = 0; i < oldVector.RowCount; i++)
{
oldVector.ValueMatrix[i][0] = MatrixDouble.ZERO;
}
Matrix <double> newVector = new Matrix <double>(eq.B);
int iterations = 0;
while (Matrix <double> .GetNormOfDiffrence(oldVector, newVector) > this.precision)
{
oldVector = new Matrix <double>(newVector);
iterations++;
for (int i = 0; i < eq.A.RowCount; i++)
{
GaussSeidelIteration(eq, newVector, i);
}
}
eq.X = newVector;
return;
}
public void Perform(MatrixEquation <double> eq)
{
FirstPhaseHalfPivot(eq);
SecondPhase(eq);
eq.X = eq.B;
return;
}
public void Perform(MatrixEquation <double> eq)
// Eliminacja Gaussa z częściowym wyborem elementu
// Brak optymalizacji
{
FirstPhaseHalfPivot(eq);
SecondPhase(eq);
eq.X = eq.B;
return;
}
public LinearFunction(double[] arguments, double[] values)
{
Matrix <double> A = GetAMatrix(arguments);
Matrix <double> B = GetBMatrix(arguments, values);
MatrixEquation <double> Eq = new MatrixEquation <double>(A, null, B);
Eq.Evaluate(new Matrix.MatrixEquasionEvaluator.GaussianHalfPivot());
b = Eq.X.ValueMatrix[0][0].Evaluate();
a = Eq.X.ValueMatrix[1][0].Evaluate();
}
/// <summary>
/// 第一步：组固定参数的方法方程
/// </summary>
/// <param name="inputs"></param>
/// <returns></returns>
private static List <MatrixEquation> Setp1ComposeConstNormaEqualtion(List <AdjustObsMatrix> inputs)
{
List <MatrixEquation> normals = new List <MatrixEquation>();
//step 1： 分离易变参数
foreach (var obsMatrix in inputs)
{
MatrixEquation ne = obsMatrix.BuildConstParamNormalEquation();
normals.Add(ne);
}
return(normals);
}
private void SecondPhase(MatrixEquation <double> eq)
{
//Druga faza eliminacji Gaussa
//Sprowadza macierz A do macierzy jednostkowej
//Po tej operacji macierz B to wyliczony X, błąd to | ||B|| - ||X|| |
for (int i = eq.A.ColCount - 1; i >= 0; i--)
{
for (int j = i - 1; j >= 0; j--)
{
eq.B.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.B.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.B.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
eq.A.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.A.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.A.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
}
}
}
/// <summary>
/// 构建不变参数的法方程。不变参数由第二参数构成
/// 注意：需要控制不变参数的增减问题。
/// 基本思路：增加则插入，减少则删除，通过参数名称来控制。
/// </summary>
/// <returns></returns>
public MatrixEquation BuildConstParamNormalEquation()
{
//观测值权阵设置,对已知量赋值
Matrix L = new Matrix((IMatrix)this.Observation);
Matrix QL = new Matrix(this.Observation.InverseWeight);
Matrix PL = new Matrix(QL.GetInverse());
Matrix A = new Matrix(this.Coefficient);
Matrix AT = A.Trans;
Matrix B = new Matrix(this.SecondCoefficient);
Matrix BT = B.Trans;
Matrix X0 = this.HasApprox ? new Matrix(this.ApproxVector, true) : null;
Matrix Y0 = this.HasSecondApprox ? new Matrix(this.SecondApproxVector, true) : null;
Matrix D = this.HasFreeVector ? new Matrix(this.FreeVector, true) : null;
int obsCount = L.RowCount;
int fixedParamCount = 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 = this.SecondParamNames;
NofB2.RowNames = this.SecondParamNames;
UofB2.RowNames = this.SecondParamNames;
UofB2.ColNames = new List <string>()
{
"ConstParam"
};
var ne = new MatrixEquation(NofB2, UofB2);
return(ne);
}
private void GaussSeidelIteration(MatrixEquation <Double> eq, Matrix <Double> newVector, int i)
{
IMatrixDataType <double> x = new MatrixDouble(0);
for (int j = 0; j < eq.A.ColCount; j++)
{
if (i != j)
{
x = (x.Add(eq.A.ValueMatrix[i][j].Multiply(newVector.ValueMatrix[j][0])));
}
}
x = x.Multiply(MatrixDouble.MINUSONE);
x = x.Add(eq.B.ValueMatrix[i][0]);
x = x.Divide(eq.A.ValueMatrix[i][i]);
newVector.ValueMatrix[i][0] = x;
}
private void SecondPhase(MatrixEquation <double> eq)
{
//Druga faza eliminacji Gaussa
//Sprowadza macierz A do macierzy jednostkowej
for (int i = eq.A.ColCount - 1; i >= 0; i--)
{
for (int j = i - 1; j >= 0; j--)
{
if (eq.A.ValueMatrix[j][i].CompareTo(MatrixDouble.ZERO) != 0)
{
eq.B.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.B.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.B.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
eq.A.ValueMatrix[j] = Matrix <double> .SubtractRows(eq.A.ValueMatrix[j], Matrix <double> .MultiplyRow(eq.A.ValueMatrix[i], eq.A.ValueMatrix[j][i]));
}
}
}
}
public List <DaoGuZhiLiangTeXingModel> getAllPredictData(List <DaoGuZhiLiangTeXingModel> models)
{
string xLabel = models[0].xLabel;
string yLabel = models[0].canShuName;
List <DaoGuZhiLiangTeXingModel> allModels = getAllData(models);
double[] douY = new double[allModels.Count];
double[] douX = new double[allModels.Count];
List <double> douAllX = new List <double>();
//double[] douAllY;
for (int i = 0; i < allModels.Count; i++)
{
douX[i] = Convert.ToDouble(allModels[i].xData);
douY[i] = Convert.ToDouble(allModels[i].yData);
}
for (double i = douX[0]; i <= douX[allModels.Count - 1]; i++)
{
douAllX.Add(i);
}
MatrixEquation mart = new MatrixEquation();
double[] xiShu = mart.MultiLine(douX, douY, douY.Length, 3);
List <DaoGuZhiLiangTeXingModel> allPredictModels = new List <DaoGuZhiLiangTeXingModel>();
for (int i = 0; i < douAllX.Count; i++)
{
DaoGuZhiLiangTeXingModel model = new DaoGuZhiLiangTeXingModel();
double y = xiShu[0] + xiShu[1] * douAllX[i] + xiShu[2] * douAllX[i] * douAllX[i] + xiShu[3] * douAllX[i] * douAllX[i] * douAllX[i];
model.xData = douAllX[i].ToString();
model.yData = y.ToString();
allPredictModels.Add(model);
}
for (int i = 0; i < allPredictModels.Count; i++)
{
allPredictModels[i].high = getMax(convert(allPredictModels));
allPredictModels[i].low = getMin(convert(allPredictModels));
allPredictModels[i].predictChaValue = Convert.ToDouble(allModels[i].yData) - Convert.ToDouble(allPredictModels[i].yData);
allPredictModels[i].xLabel = xLabel;
allPredictModels[i].canShuName = yLabel;
}
return(allPredictModels);
}
private void button_randomGen_Click(object sender, EventArgs e)
{
DateTime from = DateTime.Now;
int order = 100;
int row = this.namedIntControl_row.GetValue();
int col = this.namedIntControl_col.GetValue();
var name = namedStringControl_name.GetValue();
double[][] matrix = MatrixUtil.CreateRandom(row, col);
Matrix mat = new Matrix(matrix);
var prefix = namedStringControl_prefName.GetValue();
List <string> rowNames = new List <string>();
for (int i = 0; i < row; i++)
{
rowNames.Add(prefix + i.ToString("000"));
}
List <string> colNames = new List <string>();
for (int i = 0; i < col; i++)
{
colNames.Add(prefix + i.ToString("000"));
}
mat.RowNames = rowNames;
mat.ColNames = colNames;
double[][] rigth = MatrixUtil.CreateRandom(row, 1);
Matrix rmat = new Matrix(rigth);
rmat.RowNames = rowNames;
Matrix QofU = Matrix.CreateIdentity(row);
QofU.ColNames = rowNames;
QofU.RowNames = rowNames;
var equa = new MatrixEquation(mat, rmat)
{
Name = name, QofU = QofU
};
DataBind(equa);
}
private void DataBind(MatrixEquation mat)
{
Entity = mat;
this.richTextBoxControl_left.Text = Entity.LeftSide.ToReadableText();
StringBuilder sb = new StringBuilder();
sb.AppendLine(Entity.RightSide.ToReadableText());
if (Entity.HasWeightOfRightSide && Entity.QofU.RowCount < 100)
{
sb.AppendLine("右边权逆阵");
sb.AppendLine(Entity.QofU.ToReadableText());
}
this.richTextBoxControl_right.Text = sb.ToString();
var path = Path.Combine(OutputDirectory, "Combied" + Setting.TextMatrixEquationFileExtension);
WriteToTextFile(path);
Geo.Utils.FormUtil.ShowOkAndOpenDirectory(Path.GetDirectoryName(path));
}
public void DataBind(MatrixEquation mat)
{
Entity = (mat);
//具有固定参数，则生成打开
if (this.HasOrEnableFixedParam)
{
var fixedStorage = this.GetFixedParamStorage();
if (fixedStorage != null)
{
ParamFixedMatrixEquationBuilder equationBuilder = new ParamFixedMatrixEquationBuilder(fixedStorage);
var newEq = equationBuilder.Build(Entity);
MatrixEquationForm form = new MatrixEquationForm();
form.DataBind(newEq);
form.Show();
}
}
StringBuilder sb = new StringBuilder();
if (Entity.MaxSize > MaxOrderToShow)
{
sb.Append("超出最大显示阶数：" + Entity.MaxSize);
}
else
{
//左右方程
this.richTextBoxControl_left.Text = Entity.LeftSide.ToReadableText();
this.richTextBoxControl_right.Text = Entity.RightSide.ToReadableText();
if (Entity.HasWeightOfRightSide)
{
richTextBoxControl_weightOfU.Text = Entity.QofU.ToReadableText();
}
sb.AppendLine("方程文本");
sb.AppendLine(Entity.ToReadableText());
}
//法方程与观测残差
var normal = Entity.NormalEquation;
sb.AppendLine("法方程文本");
sb.AppendLine(normal.ToReadableText());
sb.AppendLine("参数计算结果");
var est = normal.GetEstimated();
sb.AppendLine(est.ToReadableText());
var table = est.GetObjectTable();
this.objectTableControl_result.DataBind(table);
//写结果
var paramPath = Path.Combine(OutputDirectory, "EstimatedParamOf" + Entity.Name + Setting.ParamFileExtension);
ObjectTableWriter.Write(table, paramPath);
sb.AppendLine("观测残差");
sb.AppendLine(normal.GetResidual().ToReadableText());
this.richTextBoxControl_result.Text = sb.ToString();
}
/// <summary>
/// 用MatrixEquation得到的预测值
/// </summary>
/// <param name="str">实验号</param>
/// <returns>预测值</returns>
public static List <DataValue> getAllPredictValue(String str)
{
List <DataValue> lists = new List <DataValue>();
List <DateTime> dateTime;
List <double> a;
List <string> inTemp;
List <double> grainTemp;
try
{
dateTime = SqlData.sqlDataDate("dbo.小麦害虫种群变化数据表", "Date", str);
a = SqlData.sqlGetData("dbo.小麦害虫种群变化数据表", "CanShuValue", str);
inTemp = SqlData.sqlDataInTemp("dbo.小麦害虫种群变化数据表", "InTemp", str);
grainTemp = SqlData.sqlDataGrainTemp("dbo.小麦害虫种群变化数据表", "GrainTemp", str);
}
catch (Exception e)
{
throw e;
}
string[] strTemp = inTemp.ToArray();
double[] douTemp = grainTemp.ToArray();
double[] col0 = a.ToArray();
DateTime[] date = dateTime.ToArray();
List <double> dayDou = new List <double>();//将有数据的日期转换为double类型
for (int i = 0; i < date.Length; i++)
{
dayDou.Add(date[i].ToOADate());
}
List <double> doubleDay = new List <double>();
for (double j = dayDou[0]; j <= dayDou[dayDou.Count - 1]; j++)
{
doubleDay.Add(j);
}
double[] ceshi = spline.setSplineData(dayDou.ToArray(), col0, doubleDay.ToArray());//实际值
MatrixEquation mart = new MatrixEquation();
double[] ceshi2 = mart.MultiLine(dayDou.ToArray(), col0, col0.Length, 3);
int index = -1;
DateTime dDay = date[0];
for (int i = 0; i < doubleDay.Count; i++)
{
DataValue datapoint = new DataValue();
double y1 = ceshi2[0] + ceshi2[1] * doubleDay[i] + ceshi2[2] * doubleDay[i] * doubleDay[i] + ceshi2[3] * doubleDay[i] * doubleDay[i] * doubleDay[i];
datapoint.PredictData = y1;
if (i % 7 == 0)
{
index++;
}
datapoint.InTemp = strTemp[index];
datapoint.GrainTemp = douTemp[index];
datapoint.date = dDay.AddDays(1);
dDay = datapoint.date;
datapoint.ChaValue = ceshi[i] - y1;
if (str == "000051")
{
datapoint.Y = ceshi[i];
}
else if (str == "000052")
{
datapoint.Y = ceshi[i];
}
else if (str == "000053")
{
datapoint.Y = ceshi[i];
}
else if (str == "000054")
{
datapoint.Y = ceshi[i];
}
lists.Add(datapoint);
}
return(lists);
}
/// <summary>
/// 具有约束条件的参数平差的无限权解法解算模糊度固定。
/// </summary>
/// <param name="totalFloat"></param>
/// <param name="fixedAmbiguities"></param>
/// <returns></returns>
public WeightedVector GetResultByWeighedParamAdjust(WeightedVector totalFloat, WeightedVector fixedAmbiguities)
{
if (fixedAmbiguities.Count == 0)
{
return(totalFloat);
}
int totalParamCount = totalFloat.Count; //待估参数个数
int fixedAmbiCount = fixedAmbiguities.Count;
//平差系数阵
var PA = new Matrix(totalFloat.Weights);
var LA = new Matrix((IVector)totalFloat, true)
{
RowNames = totalFloat.ParamNames
};
var A = DiagonalMatrix.GetIdentity(totalParamCount);//A x = l
var LB = new Matrix((IVector)fixedAmbiguities, true)
{
RowNames = fixedAmbiguities.ParamNames
};
var PB = new Matrix(fixedAmbiguities.Weights);
Matrix B = BuildCoeefOfFixedToFloat(totalFloat, fixedAmbiguities);
var BTBP = B.Trans * PB;
//A 为单位阵，不需要计算
var NA = PA; //AT * P * A
var UA = PA * LA; //AT * Ｐ * Ｌ
UA.ColNames = new List <string>()
{
"Names"
};
NA.ColNames = totalFloat.ParamNames;
UA.RowNames = totalFloat.ParamNames;
NA.RowNames = totalFloat.ParamNames;
var NB = BTBP * B;
var UB = BTBP * LB;
UB.ColNames = new List <string>()
{
"Names"
};
NB.ColNames = totalFloat.ParamNames;
UB.RowNames = totalFloat.ParamNames;
NB.RowNames = totalFloat.ParamNames;
MatrixEquation neA = new MatrixEquation(NA, UA);
MatrixEquation neB = new MatrixEquation(NB, UB);
var eq = neA + neB;
var result = eq.GetEstimated();
//整体矩阵验证, 2018.10.20, czs, in hmx, 已经验证与整体平差一致，但是如果权太大如1e40，则可能出现舍入误差，而失真！！
if (false)
{
Matrix AB = new Matrix(A.RowCount + B.RowCount, A.ColCount);
AB.SetSub(A);
AB.SetSub(B, A.ColCount);
Matrix PAB = new Matrix(PA.RowCount + PB.RowCount);
PAB.SetSub(PA);
PAB.SetSub(PB, PA.RowCount, PA.ColCount);
Vector LAB = new Vector(totalFloat);
LAB.AddRange(fixedAmbiguities);
ParamAdjuster paramAdjuster = new ParamAdjuster();
var result2 = paramAdjuster.Run(new AdjustObsMatrix(new WeightedVector(LAB, PAB.Inversion), AB));
}
return(result);
}
private void DataBind(MatrixEquation mat)
{
Entity = mat;
//具有固定参数，则生成打开
if (this.HasOrEnableFixedParam)
{
var fixedStorage = this.GetFixedParamStorage();
if (fixedStorage != null)
{
ParamFixedMatrixEquationBuilder equationBuilder = new ParamFixedMatrixEquationBuilder(fixedStorage);
var newEq = equationBuilder.Build(Entity);
MatrixEquationForm form = new MatrixEquationForm();
form.DataBind(newEq);
form.Show();
}
}
this.richTextBoxControl_left.Text = Entity.LeftSide.ToReadableText();
StringBuilder sb = new StringBuilder();
sb.AppendLine(Entity.RightSide.ToReadableText());
if (Entity.HasWeightOfRightSide && Entity.QofU.RowCount < 100)
{
sb.AppendLine("右边权逆阵");
sb.AppendLine(Entity.QofU.ToReadableText());
}
this.richTextBoxControl_right.Text = sb.ToString();
//输出
var path = Path.Combine(OutputDirectory, mat.Name + Setting.TextMatrixEquationFileExtension);
WriteToTextFile(path);
//法方程与观测残差
sb = new StringBuilder();
var normal = Entity.NormalEquation;
sb.AppendLine("法方程文本");
sb.AppendLine(normal.ToReadableText());
sb.AppendLine("参数计算结果");
var est = normal.GetEstimated();
sb.AppendLine(est.ToReadableText());
var table = est.GetObjectTable();
this.objectTableControl1.DataBind(table);
//写结果
var paramPath = Path.Combine(OutputDirectory, "EstimatedParamOf" + Entity.Name + Setting.ParamFileExtension);
ObjectTableWriter.Write(table, paramPath);
sb.AppendLine("观测残差");
sb.AppendLine(normal.GetResidual().ToReadableText());
richTextBoxControl_normal.Text = sb.ToString();
}
/// <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>
/// <returns></returns>
public MatrixEquation GetObsMatrixEquation(string name)
{
MatrixEquation eq = new MatrixEquation(this.Coefficient, this.Observation, name);
return(eq);
}
