/// <summary>
/// Kalmam滤波。
/// </summary>
/// <param name="material">接收信息</param>
/// <param name="last">上次解算结果（用于 Kalman 滤波）,若为null则使用初始值计算</param>
/// <returns></returns>
public virtual TProduct CaculateKalmanFilter(TMaterial material, TProduct last = default(TProduct))
{
AdjustObsMatrix obsMatrix = BuildAdjustObsMatrix(material);
this.Adjustment = this.RunAdjuster(obsMatrix);
if (Adjustment.Estimated == null)
{
return(default(TProduct));
}
// return BuildResult();
return(BuildResult());
}
/// <summary>
/// 模糊度固定解，条件平差。
/// </summary>
/// <param name="result"></param>
/// <param name="fixedPppAmbi"></param>
private PppResult FixPppResult(PppResult result, Dictionary <SatelliteNumber, double> fixedPppAmbi)
{
if (fixedPppAmbi.Count < 2)
{
return(result);
}
var floadParams = result.ResultMatrix.Estimated;
var paramCount = floadParams.Count;
int fixedCount = fixedPppAmbi.Count;
Vector fixedVector = new Vector();//fixedCount + 1
foreach (var item in fixedPppAmbi)
{
fixedVector.Add(item.Value, item.Key.ToString());
}
//构建控制阵
Matrix constraintMatrix = new Matrix(fixedCount, paramCount);
int baseColIndex = -1;// floadParams.ParamNames.IndexOf(this.CurrentBasePrn.ToString());
for (int i = 5; i < paramCount; i++)
{
var sat = SatelliteNumber.Parse(floadParams.ParamNames[i]);
if (sat == CurrentBasePrn)
{
baseColIndex = i;
break;
}
}
for (int colIndex = 5; colIndex < paramCount; colIndex++)
{
var sat = SatelliteNumber.Parse(floadParams.ParamNames[colIndex]);
int rowIndex = fixedVector.ParamNames.IndexOf(sat.ToString());//列编号与固定值对应
if (rowIndex == -1)
{
continue;//没有，略过
}
constraintMatrix[rowIndex, baseColIndex] = -1;
constraintMatrix[rowIndex, colIndex] = 1;
}
//条件平差
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(constraintMatrix).SetObservation(floadParams).SetFreeVector(fixedVector);
ConditionalAdjuster adjuster = new ConditionalAdjuster();
var resultMatrix = adjuster.Run(obsMatrix);
result.ResultMatrix.Estimated = resultMatrix.CorrectedObs;
return(result);
}
/// <summary>
/// 运行计算器
/// </summary>
/// <param name="obsMatrix"></param>
/// <returns></returns>
protected virtual AdjustResultMatrix RunAdjuster(AdjustObsMatrix obsMatrix)
{
AdjustResultMatrix result = null;
if (this.Option.IsAdjustEnabled)
{
result = this.MatrixAdjuster.Run(obsMatrix);
}
else
{
result = new AdjustResultMatrix()
{
ObsMatrix = obsMatrix
};
}
return(result);
}
/// <summary>
/// 参数估计
/// </summary>
/// <returns></returns>
public PlainXyConvertParam Estimate()
{
var obsVector = new Vector(ObsCount);
//构建观测向量
for (int i = 0; i < CoordCount; i++)
{
int index = 2 * i;
var oldXy = OldOrTobeConvertingXys[i];
var newXy = NewOrTargedXys[i];
obsVector[index + 0] = newXy.X - oldXy.X;
obsVector[index + 1] = newXy.Y - oldXy.Y;
}
//构建系数阵
Matrix coeffOfParam = new Matrix(ObsCount, 4);
for (int i = 0; i < CoordCount; i++)
{
int index = 2 * i;
var oldXy = OldOrTobeConvertingXys[i];
//尺度因子，
coeffOfParam[index + 0, 0] = 1;
coeffOfParam[index + 1, 1] = 1;
//转换参数，弧度
coeffOfParam[index + 0, 2] = oldXy.X;
coeffOfParam[index + 0, 3] = oldXy.Y;
coeffOfParam[index + 1, 2] = oldXy.Y;
coeffOfParam[index + 1, 3] = -oldXy.X;
}
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(obsVector);
var adjuster = new ParamAdjuster();
this.ResultMatrix = adjuster.Run(obsMatrix);
var ested = ResultMatrix.Estimated;
var result = new PlainXyConvertParam(new XY(ested[0], ested[1]), ested[2] * 1e6, ested[3] * CoordConsts.RadToSecMultiplier);
return(result);
}
/// <summary>
/// 参数估计
/// </summary>
/// <returns></returns>
public XyzFrameConvertParam Estimate()
{
var obsVector = new Vector(ObsCount);
//构建观测向量
for (int i = 0; i < CoordCount; i++)
{
int index = 3 * i;
var oldXy = OldOrTobeConvertingXys[i];
var newXy = NewOrTargedXys[i];
obsVector[index + 0] = newXy.X - oldXy.X;
obsVector[index + 1] = newXy.Y - oldXy.Y;
obsVector[index + 2] = newXy.Z - oldXy.Z;
}
//构建系数阵
Matrix coeffOfParam = new Matrix(ObsCount, 7);
for (int i = 0; i < CoordCount; i++)
{
int index = 3 * i;
var oldXy = OldOrTobeConvertingXys[i];
//平移参数XYZ，0-2，单位：m
coeffOfParam[index + 0, 0] = 1;
coeffOfParam[index + 1, 1] = 1;
coeffOfParam[index + 2, 2] = 1;
//尺度参数，3，单位：ppm，求出的直接为 ppm
double factor = 1.0;// 1e-6;
coeffOfParam[index + 0, 3] = oldXy.X * factor;
coeffOfParam[index + 1, 3] = oldXy.Y * factor;
coeffOfParam[index + 2, 3] = oldXy.Z * factor;
//旋转参数，4-6, 单位：s,需要转换为rad
factor = 1.0;// CoordConsts.SecToRadMultiplier;
coeffOfParam[index + 0, 5] = -oldXy.Z * factor;
coeffOfParam[index + 0, 6] = oldXy.Y * factor;
coeffOfParam[index + 1, 4] = oldXy.Z * factor;
coeffOfParam[index + 1, 6] = -oldXy.X * factor;
coeffOfParam[index + 2, 4] = -oldXy.Y * factor;
coeffOfParam[index + 2, 5] = oldXy.X * factor;
}
AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
obsMatrix.SetCoefficient(coeffOfParam).SetObservation(obsVector);
var adjuster = new ParamAdjuster();
this.ResultMatrix = adjuster.Run(obsMatrix);
var ested = ResultMatrix.Estimated;
var result = new XyzFrameConvertParam(
new XYZ(ested[0], ested[1], ested[2]), //meter
new XYZ(ested[4], ested[5], ested[6]) * CoordConsts.RadToSecMultiplier, //second
ested[3] * 1e6 //ppm
);
//输出精度信息
var RMSVector = ResultMatrix.StdOfEstimatedParam;
StringBuilder sb = new StringBuilder();
sb.AppendLine("单位权中误差：" + this.ResultMatrix.StdDev.ToString("G6"));
sb.Append("平移参数(m)：" + result.Offset);
sb.AppendLine(", 标准差：" + RMSVector[0].ToString("G6") + ", " + RMSVector[1].ToString("G6") + ", " + RMSVector[2].ToString("G6"));
sb.Append("旋转参数(s)：" + result.RotationAngleSecond);
sb.AppendLine(", 标准差：" + RMSVector[4].ToString("G6") + ", " + RMSVector[5].ToString("G6") + ", " + RMSVector[6].ToString("G6"));
sb.Append("尺度参数(ppm)：" + result.ScaleFactorPpm.ToString("G6"));
sb.AppendLine(", 标准差：" + RMSVector[3].ToString("G6"));
log.Info(sb.ToString());
return(result);
}
