/// <summary>
/// 拟合参数
/// </summary>
public double [] FitParameters()
{
int rowA = ArrayY.Length;//行
int colA = ParamCount;
//coeffOfParams(m,n)
double[][] A = Geo.Utils.MatrixUtil.Create(rowA, colA);
//让 X 不变，如输出相同。！！！！2018.05.20，czs
for (int i = 0; i < rowA; i++)
{
for (int j = 0; j < colA; j++)
{
var x = ArrayX[i];
A[i][j] = Math.Pow(x, j);
}
}
double[][] L = Geo.Utils.MatrixUtil.Create(rowA, 1);
for (int i = 0; i < rowA; i++)
{
L[i][0] = ArrayY[i];
}
Adjust.ParamAdjuster ParamAdjuster = new Adjust.ParamAdjuster();
var ad = ParamAdjuster.Run(new Adjust.AdjustObsMatrix(A, L));
Parameters = ad.Estimated.OneDimArray;
StdDev = ad.StdDev;
return(Parameters);
}
/// <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>
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="x"></param>
/// <param name="xs"></param>
/// <param name="ys"></param>
/// <param name="order">参数数量=最高阶次+1</param>
/// <returns></returns>
public static double ChebyshevFit(double x, double[] xs, double[] ys, int order = 2)
{
int xLen = xs.Length;
double min = Math.Min(xs[0], xs[xLen - 1]);
double max = Math.Max(xs[0], xs[xLen - 1]);
double[][] A = MatrixUtil.Create(xLen, order);
for (int i = 0; i < xLen; i++)
{
double scopeX = ToScope(xs[i], min, max);
for (int j = 0; j < order; j++)
{
A[i][j] = GetT(scopeX, j);
}
}
double[] L = new double[xLen];
for (int i = 0; i < xLen; i++)
{
L[i] = ys[i];
}
Adjust.ParamAdjuster pa = new Adjust.ParamAdjuster();
var ad = pa.Run(new Adjust.AdjustObsMatrix(A, L));
double[] param = ad.Estimated.OneDimArray;
double result = 0;
double smallX = ToScope(x, min, max);
for (int i = 0; i < param.Length; i++)
{
result += param[i] * GetT(smallX, i);
}
return(result);
}
/// <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);
}
