/// <summary>
/// 获取二次多项式加1个主要周期项模型的结果
/// </summary>
/// <param name="ModelData"></param>
/// <param name="PredictedLength"></param>
/// <param name="IntervalSecond"></param>
/// <returns></returns>
public static ArrayMatrix GetQuadraticPolynomialT1X(ArrayMatrix ModelData, int PredictedLength, int IntervalSecond)
{
double f1 = 1.0 / (12 * 3600);
int N = ModelData.Rows + PredictedLength;
ArrayMatrix QuadraticPolynomialX = new ArrayMatrix(N, 1);
ArrayMatrix A = new ArrayMatrix(ModelData.Rows, 5);
double t;
for (int i = 0; i < ModelData.Rows; i++)
{
t = i * IntervalSecond;
A[i, 0] = 1;
A[i, 1] = t;
A[i, 2] = t * t;
A[i, 3] = Math.Sin(2 * Math.PI * f1 * t);
A[i, 4] = Math.Cos(2 * Math.PI * f1 * t);
}
ArrayMatrix X = (A.Transpose() * A).Inverse * (A.Transpose() * ModelData);
double a1 = X[0, 0];
double a2 = X[1, 0];
double a3 = X[2, 0];
QuadraticPolynomialX[0, 0] = ModelData[0, 0];
for (int i = 0; i < N; i++)
{
t = i * IntervalSecond;
QuadraticPolynomialX[i, 0] = a1 + a2 * IntervalSecond * i + a3 * IntervalSecond * i * IntervalSecond * i + X[3, 0] * Math.Sin(2 * Math.PI * f1 * t) + X[4, 0] * Math.Cos(2 * Math.PI * f1 * t);
}
return(QuadraticPolynomialX);
}
/// <summary>
/// 获取二次多项式模型的结果
/// </summary>
/// <param name="ModelData"></param>
/// <param name="PredictedLength"></param>
/// <param name="IntervalSecond"></param>
/// <returns></returns>
public static ArrayMatrix GetQuadraticPolynomialX(ArrayMatrix ModelData, int PredictedLength, int IntervalSecond)
{
int N = ModelData.Rows + PredictedLength;
ArrayMatrix QuadraticPolynomialX = new ArrayMatrix(N, 1);
ArrayMatrix A = new ArrayMatrix(ModelData.Rows, 3);
double t;
for (int i = 0; i < ModelData.Rows; i++)
{
if (ModelData[i, 0] == 0)
{
continue;
}
t = i * IntervalSecond;
A[i, 0] = 1;
A[i, 1] = t;
A[i, 2] = t * t;
}
ArrayMatrix X = (A.Transpose() * A).Inverse * (A.Transpose() * ModelData);
double a1 = X[0, 0];
double a2 = X[1, 0];
double a3 = X[2, 0];
QuadraticPolynomialX[0, 0] = ModelData[0, 0];
for (int i = 0; i < N; i++)
{
QuadraticPolynomialX[i, 0] = a1 + a2 * IntervalSecond * i + a3 * IntervalSecond * i * IntervalSecond * i;
}
return(QuadraticPolynomialX);
}
/// <summary>
/// factorization of Q into L^TProduct D L
/// </summary>
/// <param name="D"></param>
/// <param name="L"></param>
private bool LTDL(double[] D, ArrayMatrix L)
{
for (int i = n - 1; i >= 0; i--)
{
D[i] = Q[i, i];
if (Q[i, i] <= 0.0)
{
return(false);
}
for (int j = 0; j <= i; j++)
{
L[i, j] = Q[i, j] / Math.Sqrt(Q[i, i]);
}
for (int j = 0; j <= i - 1; j++)
{
for (int k = 0; k <= j; k++)
{
Q[j, k] = Q[j, k] - L[i, k] * L[i, j];
}
}
for (int j = 0; j <= i; j++)
{
L[i, j] = L[i, j] / L[i, i];
}
}
return(true);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="ModelData"></param>
/// <param name="CompareData"></param>
/// <param name="IntervalSeconds"></param>
public void Calculate(ArrayMatrix ModelData, ArrayMatrix CompareData, int IntervalSeconds)
{
int ModelLength = ModelData.RowCount; //设定所用建模数据长度
int PredictedLength = CompareData.RowCount; //设定所用
ArrayMatrix RobustDLinearPolynomialX = GetRobustLinearPolynomialX(ModelData, PredictedLength, IntervalSeconds);
ArrayMatrix RobustLinearPolynomialX = new ArrayMatrix(ModelLength + PredictedLength + 1, 1);
RobustLinearPolynomialX[0, 0] = ModelData[0, 0];
for (int i = 0; i < ModelLength + PredictedLength; i++)
{
RobustLinearPolynomialX[i + 1, 0] = RobustLinearPolynomialX[i, 0] + RobustDLinearPolynomialX[i, 0];
}
ArrayMatrix PolyX = GetModelPolyX(RobustLinearPolynomialX, ModelLength);
PolyError = PolyX - ModelData;
PolyRms = GetRMS(PolyError, ModelLength);
PredictedData = GetModelPredictedX(RobustLinearPolynomialX, ModelLength);
GetPredictedError(CompareData, PredictedLength);
PredictedRms = GetRMS(PredictedError, PredictedLength) * 1E9;
Predicted3hRms = GetRMS(PredictedError, 3 * 3600 / IntervalSeconds) * 1E9;
Predicted6hRms = GetRMS(PredictedError, 6 * 3600 / IntervalSeconds) * 1E9;
Predicted12hRms = GetRMS(PredictedError, 12 * 3600 / IntervalSeconds) * 1E9;
Predicted24hRms = GetRMS(PredictedError, 24 * 3600 / IntervalSeconds) * 1E9;
}
/// <summary>
/// 最小二乘解算。返回参数向量平差值。
/// Delta X^ = inv（N） U
/// </summary>
/// <returns></returns>
public double[][] Solve_ToBeCheck()
{
ArrayMatrix InverNe = new ArrayMatrix(Normal_error).Inverse;
ArrayMatrix Ue = new ArrayMatrix(RightHandSide_error);
ArrayMatrix Coe_c = new ArrayMatrix(Coeff_condition);
ArrayMatrix TransCoe_c = Coe_c.Transpose();
ArrayMatrix InverNc = new ArrayMatrix(Normal_condition).Inverse;
ArrayMatrix Vc = new ArrayMatrix(ObsMinusApriori_condition);
ArrayMatrix K = InverNc * (Coe_c * InverNe * Ue - Vc);
ArrayMatrix X = -InverNe * (TransCoe_c * K - Ue);
//Matrix X2 = (InverNe - InverNe * TransCoe_c * InverNc * Coe_c * InverNe) * Ue - InverNe * TransCoe_c * InverNc * Vc;
//精度评定
if (A != null && L != null)
{
ArrayMatrix V = A * X - L;
this.ObsError = V.Array;
this.VarianceOfUnitWeight = (V.Transpose() * V)[0, 0] / (this.Freedom + ConditionCount);
//结果转化
this.CovaOfParams = (InverNe.Inverse * VarianceOfUnitWeight).Array;//(N.Inverse ).Array; //
}
this.Param = X.Array;
return(Param);
}
/// <summary>
/// 初始化。
/// </summary>
/// <param name="coeffOfParams">参数（误差方程）系数阵</param>
/// <param name="obsMinusApriori">误差方程右手边</param>
/// <param name="inverseWeight">观测方程权逆阵</param>
/// <param name="coeff_condition">条件方程系数阵</param>
/// <param name="obsMinusApriori_condition">条件方程常数项</param>
public void Init(double[][] coeffOfParams, double[][] obsMinusApriori, double[][] inverseWeight, double[][] coeff_condition, double[][] obsMinusApriori_condition)
{
ArrayMatrix Q, P;
if (inverseWeight == null)//若为空，则为单位阵
{
Q = new ArrayMatrix(MatrixUtil.CreateIdentity(coeffOfParams.Length));
P = Q;
}
else
{
Q = new ArrayMatrix(inverseWeight);
P = Q.Inverse;
}
this.Coeff_condition = coeff_condition;
this.ObsMinusApriori_condition = obsMinusApriori_condition;
this.A = new ArrayMatrix(coeffOfParams);
this.L = new ArrayMatrix(obsMinusApriori);
ArrayMatrix AT = A.Transpose();
this.Weight = P.Array;
this.ObsCount = coeffOfParams.Length;
this.Normal_error = (AT * P * A).Array;
this.RightHandSide_error = (AT * P * L).Array;
this.Normal_condition = (new ArrayMatrix(coeff_condition) * new ArrayMatrix(Normal_error) * new ArrayMatrix(coeff_condition).Transpose()).Array;
}
/// <summary>
/// 最小二乘解算。返回参数向量平差值。
/// </summary>
/// <returns></returns>
public double[][] Solve()
{
//组建大法矩阵
ArrayMatrix InverN_error = new ArrayMatrix(Normal_error).Inverse;
int row = Normal_error.Length + Coeff_condition.Length;
double[][] normalArray = MatrixUtil.Create(row);
MatrixUtil.SetSubMatrix(normalArray, Normal_error);
MatrixUtil.SetSubMatrix(normalArray, Coeff_condition, Normal_error.Length);
MatrixUtil.SetSubMatrix(normalArray, MatrixUtil.Transpose(Coeff_condition), 0, Normal_error.Length);
//法方程大右手边
double[][] rightHandSide = MatrixUtil.Create(row, 1);
MatrixUtil.SetSubMatrix(rightHandSide, RightHandSide_error);
MatrixUtil.SetSubMatrix(rightHandSide, ObsMinusApriori_condition, RightHandSide_error.Length);
//解
ArrayMatrix XK = new ArrayMatrix(normalArray).Inverse *new ArrayMatrix(rightHandSide);
ArrayMatrix X = new ArrayMatrix(MatrixUtil.GetSubMatrix(XK.Array, 6, 1, 0, 0));
//精度评定
if (A != null && L != null)
{
ArrayMatrix V = A * X - L;
this.VarianceOfUnitWeight = (V.Transpose() * V)[0, 0] / (this.Freedom + ConditionCount);
//结果转化
this.CovaOfParams = (InverN_error.Inverse * VarianceOfUnitWeight).Array;//(N.Inverse ).Array; //
}
this.Param = X.Array;
return(Param);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="ModelData"></param>
/// <param name="CompareData"></param>
/// <param name="IntervalSeconds"></param>
public void Calculate(ArrayMatrix ModelData, ArrayMatrix CompareData, int IntervalSeconds)
{
int ModelLength = ModelData.RowCount; //设定所用建模数据长度
int PredictedLength = CompareData.RowCount; //设定所用
ArrayMatrix PredictedData = new ArrayMatrix(PredictedLength, 1); //预报数据
ArrayMatrix QuadraticPolynomialT4X = GetQuadraticPolynomialT4X(ModelData, PredictedLength, IntervalSeconds);
ArrayMatrix QPT4_PolyX = GetModelPolyX(QuadraticPolynomialT4X, ModelLength);
ArrayMatrix QPT4_PolyError = QPT4_PolyX - ModelData;
ArrayMatrix GreyModelofDX = GetGreyModelX(QPT4_PolyError, ModelLength);
ArrayMatrix GreyModelPolyofDX = GetModelPolyX(GreyModelofDX, ModelLength);
ArrayMatrix QPT4GM = QuadraticPolynomialT4X + GreyModelofDX;
ArrayMatrix QPT4GM_PolyX = GetModelPolyX(QPT4GM, ModelLength);
PolyError = QPT4GM_PolyX - ModelData;
PolyRms = GetRMS(PolyError, ModelLength);
PredictedData = GetModelPredictedX(QPT4GM, ModelLength);
GetPredictedError(CompareData, PredictedLength);
PredictedRms = GetRMS(PredictedError, PredictedLength) * 1E9;
Predicted3hRms = GetRMS(PredictedError, 3 * 3600 / IntervalSeconds) * 1E9;
Predicted6hRms = GetRMS(PredictedError, 6 * 3600 / IntervalSeconds) * 1E9;
Predicted12hRms = GetRMS(PredictedError, 12 * 3600 / IntervalSeconds) * 1E9;
Predicted24hRms = GetRMS(PredictedError, 24 * 3600 / IntervalSeconds) * 1E9;
}
private double[] PrecessClasterByDimentions(List <double[]> claster, Func <STAT, double> geterResultValueFunc, Action <STAT> provessAction)
{
int n = claster[0].Length;
int N = claster.Count;
var arr = ArrayMatrix.GetJaggedArray(n, N);
for (int i = 0; i < N; i++)
{
for (int j = 0; j < n; i++)
{
arr[j][i] = claster[i][j];
}
}
double[] result = new double[n];
for (int i = 0; i < n; i++)
{
STAT s = new STAT();
s.Setd2Stat2d(arr[i]);
provessAction?.Invoke(s);
result[i] = geterResultValueFunc(s);
}
return(result);
}
public Matrix GetMatrixOfCovariations()
{
int n = Center.Length;
double[] averages = Dimentions.Select(dim => dim.Average()).ToArray();
double[][] cov = ArrayMatrix.GetJaggedArray(n, n);
for (int k = 0; k < n; k++)
{
for (int p = 0; p < n; p++)
{
double v = 0;
for (int l = 0; l < Nj; l++)
{
v += (Points[k][l] - averages[k]) * (Points[p][l] - averages[p]);
}
cov[k][p] = v / Nj;
}
}
return(new Matrix(cov));
}
/// <summary>
/// collexts integer vectors and corresponding squared distances
/// </summary>
/// <param name="MaxCan">number of minimum integer vectors required</param>
/// <param name="D">diagonal matrix</param>
/// <param name="Chic">Chi squared</param>
/// <param name="cands">2-dimensional array to store the candidates</param>
/// <param name="disal1">according squared norms \hat{a}-\check{a}</param>
/// <param name="tmax">the largest distance of the Min(ncan, MaxCan) vectors with minimum distance found until now</param>
/// <param name="imax">position in disall/cands of the vector with the largest distance of the Min (ncan, MaxCan) vectors with minimum distance found until now</param>
/// <param name="right">vector</param>
/// <param name="left">vector</param>
/// <param name="ncan">number of integer vectors found</param>
/// <param name="lef">vector</param>
/// <param name="dist">difference between the integer tried and \hat{a}_n</param>
/// <param name="endd">vector</param>
private void Collects(int MaxCan, double[] D, double Chic, ArrayMatrix cands, ArrayMatrix disal1,
ref double tmax, ref int imax, double[] right, double[] left, ref int ncan, double[] lef, double[] dist, double[] endd)
{
double t = Chic - (right[0] - left[0]) * D[0];
endd[0] = endd[0] + 1;
//The following loop should be run through at least once
while (dist[0] <= endd[0])
{
ncan = ncan + 1;
if (ncan <= MaxCan)
{
//
Stores(ncan, ncan, t, cands, disal1, ref tmax, ref imax, dist);
}
else
{
if (t < tmax)
{
Stores(MaxCan, imax, t, cands, disal1, ref tmax, ref imax, dist);
}
}
t = t + (2 * (dist[0] + lef[0]) + 1) * D[0];
dist[0] = dist[0] + 1;
}
}
/// <summary>
/// 计算的简化版本
/// </summary>
/// <param name="coeff"></param>
/// <param name="obs"></param>
/// <param name="inverseWeight_obs"></param>
/// <param name="lastInverseWeight"></param>
/// <param name="inverseWeight_model"></param>
/// <param name="trans"></param>
/// <param name="lastParams"></param>
public void Init1(
double[][] coeff, //A
double[][] obs, //L
double[][] trans, //状态转移矩阵
double[][] lastParams, //X0上历元的状态向量
double[][] inverseWeight_obs, //Q 本次观测噪声权逆阵
double[][] lastInverseWeight, //Q0上历元的权逆阵
double[][] inverseWeight_model //动力模型噪声权逆阵
)
{
//0表示上一个，1表示预测，无数字表示当次， kk0表示转移逆阵
ArrayMatrix A, AT, D_o, D_m, L, D0, X0, Trans;//
//Matrix N, U;//middle
A = new ArrayMatrix(coeff); //误差方程系数阵
AT = A.Transpose(); //A 的转置
L = new ArrayMatrix(obs); //观测值，或 观测值 - 估计值
D_o = new ArrayMatrix(inverseWeight_obs); //观测噪声权逆阵
D_m = new ArrayMatrix(inverseWeight_model); //动态噪声权逆阵
D0 = new ArrayMatrix(lastInverseWeight); //上一次估计误差方差矩阵
X0 = new ArrayMatrix(lastParams); //上一次估值
Trans = new ArrayMatrix(trans); //状态转移矩阵
//状态一步预测
ArrayMatrix X1 = Trans * X0;
//一步预测误差方差矩阵
ArrayMatrix D1 = Trans * D0 * Trans.Transpose() + D_m;
//计算增益矩阵J
ArrayMatrix J = X1 * AT * (A * D1 * AT + D_o).Inverse;
//状态估计
ArrayMatrix X = X1 + J * (L - A * X1);
//状态估计误差方差矩阵
ArrayMatrix D = D1 - J * A * D1;
}
public void Add(ArrayMatrix <int, IntOperator> mat1, ArrayMatrix <int, IntOperator> mat2, ArrayMatrix <int, IntOperator> expected)
{
(mat1 + mat2).Value.Should().BeEquivalentTo(expected.Value);
(mat2 + mat1).Value.Should().BeEquivalentTo(expected.Value);
default(ArrayMatrixOperator <int, IntOperator>).Add(mat1, mat2).Value.Should().BeEquivalentTo(expected.Value);
default(ArrayMatrixOperator <int, IntOperator>).Add(mat2, mat1).Value.Should().BeEquivalentTo(expected.Value);
}
/// <summary>
/// 获取一次多项式模型的结果
/// </summary>
/// <param name="ModelData"></param>
/// <param name="PredictedLength"></param>
/// <param name="IntervalSecond"></param>
/// <returns></returns>
public static ArrayMatrix GetLinearPolynomialX(ArrayMatrix ModelData, int PredictedLength, int IntervalSecond)
{
int N = ModelData.Rows + PredictedLength;
ArrayMatrix LinearPolynomialX = new ArrayMatrix(N, 1);
ArrayMatrix A = new ArrayMatrix(ModelData.Rows, 2);
double t;
for (int i = 0; i < ModelData.Rows; i++)
{
t = i * IntervalSecond;
A[i, 0] = 1;
A[i, 1] = t;
}
ArrayMatrix X = (A.Transpose() * A).Inverse * (A.Transpose() * ModelData);
double a1 = X[0, 0];
double a2 = X[1, 0];
LinearPolynomialX[0, 0] = ModelData[0, 0];
for (int i = 0; i < N; i++)
{
LinearPolynomialX[i, 0] = a1 + a2 * IntervalSecond * i;
}
return(LinearPolynomialX);
}
/// <summary>
/// 因式分解Q为LTDL
/// factorization of Q into L^TProduct D L
/// </summary>
/// <param name="D"></param>
/// <param name="L"></param>
private static void FactorizationQToLTDL(IMatrix Cova, ref double[] D, ref ArrayMatrix L)
{
int Dimension = Cova.RowCount;
for (int i = Dimension - 1; i >= 0; i--)
{
D[i] = Cova[i, i];
for (int j = 0; j <= i; j++)
{
L[i, j] = Cova[i, j] / Math.Sqrt(Cova[i, i]);
}
for (int j = 0; j <= i - 1; j++)
{
for (int k = 0; k <= j; k++)
{
Cova[j, k] = Cova[j, k] - L[i, k] * L[i, j];
}
}
for (int j = 0; j <= i; j++)
{
L[i, j] = L[i, j] / L[i, i];
}
}
}
protected double[] PrecessClasterByDimentions(List <double[]> claster, Func <STAT, double> geterResultValueFunc, Action <STAT> provessAction = null)
{
int n = claster[0].Length;
int N = claster.Count;
var arr = ArrayMatrix.TransposeArr(claster.ToArray());
//for (int i = 0; i < N; i++)
// for (int j = 0; j < n; i++)
// arr[j][i] = claster[i][j];
double[] result = new double[n];
for (int i = 0; i < n; i++)
{
STAT s = new STAT();
s.Setd2Stat2d(arr[i]);
provessAction?.Invoke(s);
result[i] = geterResultValueFunc(s);
}
return(result);
}
/// <summary>
/// 计算
/// </summary>
/// <param name="ModelData"></param>
/// <param name="CompareData"></param>
/// <param name="IntervalSeconds"></param>
public void Calculate(ArrayMatrix ModelData, ArrayMatrix CompareData, int IntervalSeconds)
{
int ModelLength = ModelData.RowCount; //设定所用建模数据长度
int PredictedLength = CompareData.RowCount; //设定所用
#region 取建模数据最后5个历元，对最后一个历元的钟差值进行拟合
ArrayMatrix QPModelData = new ArrayMatrix(5, 1);
for (int i = 0; i < 5; i++)
{
QPModelData[4 - i, 0] = ModelData[ModelLength - i, 0];
}
ArrayMatrix QPModelDataX = GetQuadraticPolynomialX(QPModelData, 1, IntervalSeconds);
ModelData[ModelLength, 0] = QPModelDataX[4, 0];
#endregion
//Matrix PredictedData = new Matrix(PredictedLength, 1);//预报数据
ArrayMatrix GreyModelX = GetGreyModelX(ModelData, PredictedLength);
ArrayMatrix GreyModelPolyX = GetModelPolyX(GreyModelX, ModelLength);
PolyError = GreyModelPolyX - ModelData;
PolyRms = GetRMS(PolyError, ModelLength);
PredictedData = GetModelPredictedX(GreyModelX, ModelLength);
PredictedError = PredictedData - CompareData;
PredictedRms = GetRMS(PredictedError, PredictedLength) * 1E9;
Predicted3hRms = GetRMS(PredictedError, 3 * 3600 / IntervalSeconds) * 1E9;
Predicted6hRms = GetRMS(PredictedError, 6 * 3600 / IntervalSeconds) * 1E9;
Predicted12hRms = GetRMS(PredictedError, 12 * 3600 / IntervalSeconds) * 1E9;
Predicted24hRms = GetRMS(PredictedError, 24 * 3600 / IntervalSeconds) * 1E9;
}
//returns true if abs of difference of all between new and old centers smaller then epsilon
public bool RecalcCenter(double eps)
{
if (Points.Count == 0)
{
return(true); //значение, по факту, ничего не меняет.
}
int n = Center.Length;
double[] newCenter = new double[n];
double[][] pnts = ArrayMatrix.TransposeArr(Points.ToArray());
for (int i = 0; i < n; i++)
{
newCenter[i] = pnts[i].Average();
}
//epsilon cheking
bool status = true;
for (int i = 0; i < n; i++)
{
status = status && (Center[i] - newCenter[i]).Abs() < eps;
}
Center = newCenter;
return(status);
}
/// <summary>
/// 第二步，由公共参数计算分区参数。
/// 求 ObsError， Params。
/// </summary>
public void Solve_step2()
{
ArrayMatrix A = new ArrayMatrix(CoeffOfParams);
ArrayMatrix Q = new ArrayMatrix(InverseWeightOfObs);
ArrayMatrix P = Q.Inverse;
ArrayMatrix L = new ArrayMatrix(Obs);
ArrayMatrix B = new ArrayMatrix(CoeffOfCommonParams);
ArrayMatrix Xb = new ArrayMatrix(CommonParams);
ArrayMatrix InverNa = new ArrayMatrix(InverseNormal);
ArrayMatrix Ua = new ArrayMatrix(RightHandA);
ArrayMatrix Nb = new ArrayMatrix(NormalAB);
ArrayMatrix X = InverNa * (Ua - Nb * Xb);
ArrayMatrix V = A * X + B * Xb - L;
ArrayMatrix vtpv = V.Transpose() * P * V;
this.ObsError = V.Array;
this.EstimatedParam = X.Array;
this.VTPV = vtpv[0, 0];
this.VarianceOfUnitWeight = this.VTPV / Freedom;
this.StdDev = Math.Sqrt(VarianceOfUnitWeight); //标准差
ArrayMatrix Dx = InverNa * VarianceOfUnitWeight; //协方差阵
double[] ParamCovaVector = MatrixUtil.GetDiagonal(Dx.Array);
this.ParamRmsVector = MatrixUtil.GetPow(ParamCovaVector, 0.5);
//结果转化
this.WeightOfParam = InverNa.Array;
this.CovaOfParams = Dx.Array;
this.ObsError = V.Array;
}
/// <summary>
/// 第一步计算。如，在多核上执行。
/// 求 RightHand，NormalAB， etc.
/// </summary>
public void Solve_step1()
{
//输入参数
ArrayMatrix A = new ArrayMatrix(CoeffOfParams);
ArrayMatrix Q = new ArrayMatrix(InverseWeightOfObs);
ArrayMatrix P = Q.Inverse;
ArrayMatrix L = new ArrayMatrix(Obs);
ArrayMatrix B = new ArrayMatrix(CoeffOfCommonParams);
//间接变量
ArrayMatrix AT = A.Transpose();
ArrayMatrix BT = B.Transpose();
ArrayMatrix Na = AT * P * A;
ArrayMatrix InverNa = Na.Inverse;
ArrayMatrix Ua = AT * P * L;
ArrayMatrix Nb = AT * P * B;//不一定为法矩阵Normal
ArrayMatrix NbT = Nb.Transpose();
ArrayMatrix Ub = BT * P * L;
ArrayMatrix Nbb = BT * P * B;
//结果
ArrayMatrix N = Nbb - NbT * InverNa * Nb;
ArrayMatrix U = Ub - NbT * InverNa * Ua;
this.InverseNormal = InverNa.Array;
this.Normal = N.Array;
this.RightHand = U.Array;
this.RightHandA = Ua.Array;
this.NormalAB = Nb.Array;
this.InverseWeightOfParam = N.Array;
}
public void block()
{
int CoreNum = Environment.ProcessorCount;//获取本机处理器个数
blockMatrix = new IMatrix[CoreNum][];
// this.arows = CoreNum;
//this.columns = CoreNum;//按核分行、列
int rows = OriginA.RowCount; //.Rows;
int columns = OriginA.ColCount; //.Columns;
arowBlock = averageArowBlock(rows);
columnBlock = averageClowBlock(columns);
firstarowBlock = firstArowBlock(rows);
firstcolumnBlock = firstClowBlock(columns);
////////////////串行分块
//for (int i = 0; i < CoreNum; i++)
//{
// blockMatrix[i] = new Matrix[CoreNum];
// for (int j = 0; j < CoreNum; j++)
// {
// int countarow = arowBlock[i * CoreNum + j];
// int countcolumn = columnBlock[i * CoreNum + j];
// int firstarow = firstarowBlock[i * CoreNum + j];
// int firstcolumn = firstcolumnBlock[i * CoreNum + j];
// double[][] data = new double[countarow][];
// for (int ii = 0; ii < countarow; ii++)
// {
// data[ii] = new double[countcolumn];
// for (int jj = 0; jj < countcolumn; jj++)
// {
// data[ii][jj] = OriginA.Datas[firstarow + ii][firstcolumn + jj];
// }
// }
// blockMatrix[i][j] = new Matrix(data);
// }
//}
////////////////并行分块
Parallel.For(0, CoreNum, (int i) =>
{
blockMatrix[i] = new IMatrix[CoreNum];
for (int j = 0; j < CoreNum; j++)
{
int countarow = arowBlock[i * CoreNum + j];
int countcolumn = columnBlock[i * CoreNum + j];
int firstarow = firstarowBlock[i * CoreNum + j];
int firstcolumn = firstcolumnBlock[i * CoreNum + j];
double[][] data = new double[countarow][];
for (int ii = 0; ii < countarow; ii++)
{
data[ii] = new double[countcolumn];
for (int jj = 0; jj < countcolumn; jj++)
{
data[ii][jj] = OriginA.Array[firstarow + ii][firstcolumn + jj];
}
}
blockMatrix[i][j] = new ArrayMatrix(data);
}
});
}
/// <summary>
/// 在 GNSS 动态导航中的应用
/// </summary>
protected void Motion()
{
List <string> paramNames = new List <string>()
{
"X", "Y", "Z", "dX", "dY", "dZ"
};
///参数
List <Vector> satXyzs = new List <Vector>();
Vector siteXyz = new Vector(3);
Vector L = new Vector(6); //观测值
double interval = 30; //时间间隔（单位：秒）
//将瞬时加速度作为随机干扰，
//状态向量， 位置和速度 X = trans([x, y, z, vx, vy, vz])
//噪声向量， 为加速度 Noise = trans(ax, ay, az])
//纯量形式， y = y0 + t * v0 + 0.5 * a0 * a0 * t
// v = v0 + a0 * t
/// 状态方程
/// ┌I Δt┐ ┌0.5*Δt*Δt┐
/// X(k) = │ │X(k-1) + │ │Ω(k-1)
/// └0 I ┘ └ Δt ┘
/// 观测方程
/// L(k) = A(k) X(k) + Ω(k)
/// 其中，L(k)为 k 时刻系统的 C/A 码观测向量
//常用量
double[][] identity3x3 = MatrixUtil.CreateIdentity(3);
int satCount = satXyzs.Count;
//状态方程状态转移矩阵 Φ（k,k-1）
// double[][] trans = MatrixUtil.CreateIdentity(6);
ArrayMatrix trans = new ArrayMatrix(6, 6);
MatrixUtil.SetSubMatrix(trans.Array, MatrixUtil.GetMultiply(identity3x3, interval), 0, 3);
//状态方程噪声向量Ω(k-1)的系数阵
double[][] coeef_noise = MatrixUtil.Create(6, 3);
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, 0.5 * interval * interval));
MatrixUtil.SetSubMatrix(coeef_noise, MatrixUtil.GetMultiply(identity3x3, interval), 3);
//观测方程系数矩阵 A,每个时刻不同
ArrayMatrix coeef_obs = new ArrayMatrix(satCount, 6);
for (int i = 0; i < satCount; i++)
{
IVector differXyz = (satXyzs[i] - siteXyz);
coeef_obs[i, 0] = differXyz.GetCos(0);//.CosX;
coeef_obs[i, 1] = differXyz.GetCos(1);
coeef_obs[i, 2] = differXyz.GetCos(2);
}
KalmanFilter lastKf = null;
//Vector lastParams = lastKf.Corrected ?? null;
//IMatrix lastParamCova = lastKf.Corrected.InverseWeight ?? null;
//KalmanFilter kf = new KalmanFilter(coeef_obs, L, trans, lastParams, lastParamCova);
//kf.Process();
}
/// <summary>
/// 灰色模型
/// </summary>
/// <param name="ModelData"></param>
/// <param name="PredictedLength"></param>
/// <returns></returns>
public static ArrayMatrix GetGreyModelX(ArrayMatrix ModelData, int PredictedLength)
{
int N = ModelData.Rows + PredictedLength;
ArrayMatrix GreyModelX = new ArrayMatrix(N, 1);
ArrayMatrix GreyModel1 = new ArrayMatrix(ModelData.Rows, 1);
ArrayMatrix L1 = new ArrayMatrix(ModelData.Rows, 1);
ArrayMatrix L = new ArrayMatrix(ModelData.Rows - 1, 1);
double MaxValueofModelData = Math.Abs(ModelData.MaxValue);
if (ModelData.MinValue < 0)
{
for (int i = 0; i < ModelData.Rows; i++)
{
GreyModel1[i, 0] = ModelData[i, 0] + MaxValueofModelData * 10;
} //;
}
else
{
GreyModel1 = ModelData;
}
L1[0, 0] = GreyModel1[0, 0];
for (int i = 1; i < ModelData.Rows; i++)
{
L1[i, 0] = L1[i - 1, 0] + GreyModel1[i, 0];
L[i - 1, 0] = GreyModel1[i, 0];
}
ArrayMatrix A = new ArrayMatrix(ModelData.Rows - 1, 2);
for (int i = 0; i < ModelData.Rows - 1; i++)
{
A[i, 0] = -(L1[i, 0] + L1[i + 1, 0]) / 2.0;
A[i, 1] = 1;
}
if (A[0, 0] * A[1, 1] - A[0, 1] * A[1, 0] == 0)
{
return(GreyModelX);
}
ArrayMatrix X = (A.Transpose() * A).Inverse * (A.Transpose() * L);
double a = X[0, 0];
double u = X[1, 0];
GreyModelX[0, 0] = GreyModel1[0, 0];
for (int i = 1; i < N; i++)
{
GreyModelX[i, 0] = (1.0 - Math.Exp(a)) * (GreyModelX[0, 0] - u / a) * Math.Exp(-a * i);
}
if (ModelData.MinValue < 0)
{
for (int i = 0; i < GreyModelX.Rows; i++)
{
GreyModelX[i, 0] = (GreyModelX[i, 0] - MaxValueofModelData * 10);
} //
}
return(GreyModelX);
}
/// <summary>
/// 求解残差的中误差值
/// </summary>
/// <param name="error"></param>
/// <param name="Length"></param>
/// <returns></returns>
public static double GetRMS(ArrayMatrix error, int Length)
{
double RMS = 0;
for (int i = 0; i < Length; i++)
{
RMS += error[i, 0] * error[i, 0];
}
return(Math.Sqrt(RMS / Length));
}
/// <summary>
/// 获取模型的预报值
/// </summary>
/// <param name="ModelX"></param>
/// <param name="N"></param>
/// <returns></returns>
public static ArrayMatrix GetModelPredictedX(ArrayMatrix ModelX, int N)
{
ArrayMatrix PredictedX = new ArrayMatrix(ModelX.Rows - N, 1);
for (int i = 0; i < ModelX.Rows - N; i++)
{
PredictedX[i, 0] = ModelX[N + i, 0];
}
return(PredictedX);
}
/// <summary>
/// 获取模型的拟合值
/// </summary>
/// <param name="ModelX"></param>
/// <param name="N"></param>
/// <returns></returns>
public static ArrayMatrix GetModelPolyX(ArrayMatrix ModelX, int N)
{
ArrayMatrix PolyX = new ArrayMatrix(N, 1);
for (int i = 0; i < N; i++)
{
PolyX[i, 0] = ModelX[i, 0];
}
return(PolyX);
}
/// <summary>
/// 获取模型的预报值
/// </summary>
/// <param name="x0"></param>
/// <param name="x1"></param>
/// <param name="x2"></param>
/// <param name="IntervalSecond"></param>
/// <param name="PredictedLength"></param>
/// <returns></returns>
public static ArrayMatrix GetPredictedData(double x0, double x1, double x2, int IntervalSecond, int PredictedLength)
{
ArrayMatrix PredictedData = new ArrayMatrix(PredictedLength, 1);
for (int i = 0; i < PredictedLength; i++)
{
PredictedData[i, 0] = x0 + x1 * (i + 1) * IntervalSecond + x2 * (i + 1) * IntervalSecond * (i + 1) * IntervalSecond;
}
return(PredictedData);
}
/// <summary>
/// 获取预报值对应的真值
/// </summary>
/// <param name="CompareData"></param>
/// <param name="PredictedLength"></param>
/// <returns></returns>
public static ArrayMatrix GetPredictedRealData(ArrayMatrix CompareData, int PredictedLength)
{
ArrayMatrix PredictedRealData = new ArrayMatrix(PredictedLength, 1);
for (int i = 0; i < PredictedLength; i++)
{
PredictedRealData[i, 0] = CompareData[i, 0];
}
return(PredictedRealData);
}
/// <summary>
/// 获得钟差对应的时间矩阵
/// </summary>
/// <param name="N"></param>
/// <param name="IntervalSecond"></param>
/// <returns></returns>
public static ArrayMatrix GetTimeMatrix(int N, int IntervalSecond)
{
ArrayMatrix T = new ArrayMatrix((int)((N - 1.0) / 2.0 + 1), 1);
for (int i = 0; i < (N - 1) / 2.0 - 1; i++)
{
T[i, 0] = IntervalSecond * (i + 1);
}
return(T);
}
public void MultiplyModInt(ArrayMatrix <int, IntOperator> matInt1, ArrayMatrix <int, IntOperator> matInt2, ArrayMatrix <int, IntOperator> expectedInt)
{
var mat1 = Int2ModInt(matInt1);
var mat2 = Int2ModInt(matInt2);
var expected = Int2ModInt(expectedInt);
(mat1 * mat2).Value.Should().BeEquivalentTo(expected.Value);
mat1.Strassen(mat2).Value.Should().BeEquivalentTo(expected.Value);
default(ArrayMatrixOperator <StaticModInt <Mod1000000007>, StaticModIntOperator <Mod1000000007> >)
.Multiply(mat1, mat2).Value.Should().BeEquivalentTo(expected.Value);
}
private static void LoadMatriz(Options type)
{
if (type == Options.Arreglos)
{
matrixA = new ArrayMatrix(matrixAFile);
matrixB = new ArrayMatrix(matrixBFile);
}
else
{
matrixA = new PolynMatrix(matrixAFile);
matrixB = new PolynMatrix(matrixBFile);
}
}
