/// <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>
/// <param name="coeff_param">参数的系数阵</param>
/// <param name="obsMinusApriori">观测值减去估值</param>
/// <param name="inverseWeight_obs">观测值的权逆阵</param>
/// <param name="inverseWeight_signal">含下面最后三个变量</param>
public void Init(
double[][] coeff_param, //参数的系数阵
double[][] obsMinusApriori, //观测值减去估值
double[][] inverseWeight_obs, //观测值的权逆阵
double[][] inverseWeight_signal // 含下面最后三个变量
)
{
int paramCount = coeff_param[0].Length;
int allSignalCount = inverseWeight_signal[0].Length;
int measuredSignalCount = paramCount;
int notMeasuredSignalCount = allSignalCount - measuredSignalCount;
//生成信号的系数阵
double[][] coeff_signal = MatrixUtil.Create(paramCount, allSignalCount);//是否应该自己生成
MatrixUtil.SetSubMatrix(coeff_signal, MatrixUtil.CreateIdentity(paramCount));
//提取方差、协方差阵
double[][] inverseWeight_mesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的左上角
double[][] inverseWeight_notMesuredSignal = MatrixUtil.GetSubMatrix(inverseWeight_signal, notMeasuredSignalCount, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右下角
double[][] covaSignals_Ss = MatrixUtil.GetSubMatrix(inverseWeight_signal, 0, notMeasuredSignalCount, measuredSignalCount, measuredSignalCount); //inverseWeight_signal的右上角
Init(
coeff_param, //参数的系数阵
obsMinusApriori, //观测值减去估值
inverseWeight_obs, //观测值的权逆阵
inverseWeight_signal, // 含下面最后三个变量
coeff_signal, //是否应该自己生成
inverseWeight_mesuredSignal, //inverseWeight_signal的左上角
inverseWeight_notMesuredSignal, //inverseWeight_signal的右下角
covaSignals_Ss //inverseWeight_signal的右上角
);
}
/// <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>
public static void Test(int allObsCount = 1000, bool useParalell = true, int core = 8, int blockCount = 8)
{
DateTime from = DateTime.Now;
//观测噪声
Random random = new Random();
//设有blockObsCount个观测量，paramCount个区内参数，commonParamCount个公共参数
int blockObsCount = allObsCount / blockCount;
int paramCount = blockObsCount / 2 + 1;
int commonParamCount = blockObsCount / 4 + 1;
List <BlockAdjustItem> items = new List <BlockAdjustItem>();
for (int m = 0; m < blockCount; m++)
{
//分区内参数系数阵 obsCount x paramCount
double[][] coeffA = GenCoeffA(blockObsCount, paramCount);
//观测值 obsCount x 1
double[][] obs = MatrixUtil.Create(blockObsCount, 1);
for (int i = 0; i < blockObsCount; i++)
{
obs[i][0] = i; // +random.NextDouble() - 0.5;
}
//观测值权逆阵 obsCount x obsCount
double[][] inverseOfObs = MatrixUtil.CreateIdentity(blockObsCount);
//分区内对公共参数的系数阵 obsCount x commonParamCount
double[][] coeffB = MatrixUtil.Create(blockObsCount, commonParamCount);
for (int i = 0; i < commonParamCount; i++)
{
coeffB[i][i] = 1;
}
//System.IO.File.WriteAllText(
// "C:\\Block"+m+".txt",
// "A" + "\r\n" + MatrixUtil.GetFormatedText(coeffA) + "\r\n"
// + "L" + "\r\n" + MatrixUtil.GetFormatedText(obs) + "\r\n"
// + "Q" + "\r\n" + MatrixUtil.GetFormatedText(inverseOfObs) + "\r\n"
// + "Ab" + "\r\n" + MatrixUtil.GetFormatedText(coeffB) + "\r\n"
// );
BlockAdjustItem item = new BlockAdjustItem(coeffA, obs, inverseOfObs, coeffB);
items.Add(item);
}
BlockAdjustment ba = new BlockAdjustment(items.ToArray(), useParalell, core);
TimeSpan span = DateTime.Now - from;
StringBuilder sb = new StringBuilder();
sb.AppendLine("耗时:" + span + " = " + span.TotalMinutes + " 分");
System.IO.File.WriteAllText("C:\\IsP" + useParalell.ToString() + "obs" + +allObsCount + "_core" + core + "_block" + blockCount + ".txt", sb.ToString() + ba.ToString());
}
private double[][] CreateTrans(double deltaT)
{
double[][] trans = MatrixUtil.CreateIdentity(ParamCount);
//右上角的对角阵
for (int j = 0; j < 4; j++)
{
trans[j][j + 4] = deltaT;
}
return(trans);
}
/// <summary>
/// 先验参数
/// </summary>
protected override WeightedVector CreateInitAprioriParam()
{
double[] residualVec = new double[ParamCount]; //近似差为 0
double[][] inverseWeight = MatrixUtil.CreateIdentity(ParamCount);
inverseWeight[3][3] = 100000.0; //钟差
inverseWeight[7][7] = 5e-12; //kyc:钟漂,谱哥说给个小的值
if (Option.IsIndicatingApproxXyzRms)
{
int i = 0;
for (i = 0; i < 3; i++)
{
var item = Option.InitApproxXyzRms[i];
inverseWeight[i][i] = item * item;
}
for (int j = 4; j < 7; j++)//kyc
{
inverseWeight[j][j] = 10000;
}
}
else if (CurrentMaterial.SiteInfo.EstimatedXyz.IsZero)
{
for (int i = 0; i < 3; i++)
{
inverseWeight[i][i] = 1e10;
}
for (int j = 4; j < 7; j++)
{
inverseWeight[j][j] = 1e10;
} //kyc
}
else
{
for (int i = 0; i < 3; i++)
{
inverseWeight[i][i] = 1000;
}
for (int j = 4; j < 7; j++)
{
inverseWeight[j][j] = 1000;
} //kyc
}
return(new WeightedVector(residualVec, inverseWeight));
}
/// <summary>
/// 测试参数加权平差
/// </summary>
public static void Test()
{
Random random = new Random();//观测噪声
double[][] A = MatrixUtil.CreateIdentity(6);
double[][] L = MatrixUtil.Create(6, 1);
for (int i = 0; i < L.Length; i++)
{
L[i][0] = i + 1 + random.NextDouble() - 0.5;
}
double[][] QL = MatrixUtil.CreateDiagonal(6, 1);
double[][] Xa0 = MatrixUtil.Create(3, 1);
for (int i = 0; i < Xa0.Length; i++)
{
Xa0[i][0] = i + 1;
}
double[][] Qx0 = MatrixUtil.CreateDiagonal(3, 0.001);
// WeightedParamAdjustment adjust = new WeightedParamAdjustment(A, L, QL, Xa0, Qx0);
}
/// <summary>
/// 先验参数
/// </summary>
protected override WeightedVector CreateInitAprioriParam()
{
double[] residualVec = new double[ParamCount];//近似差为 0
double[][] inverseWeight = MatrixUtil.CreateIdentity(ParamCount);
if (Option.IsIndicatingApproxXyzRms)
{
int i = 0;
for (i = 0; i < 3; i++)
{
var item = Option.InitApproxXyzRms[i];
inverseWeight[i][i] = item * item;
}
}
else if (CurrentMaterial.SiteInfo.EstimatedXyz.IsZero)
{
for (int i = 0; i < 3; i++)
{
inverseWeight[i][i] = 1e10;
}
}
else
{
for (int i = 0; i < 3; i++)
{
inverseWeight[i][i] = 100000;
}
}
inverseWeight[3][3] = 1000000;//钟差
for (int i = 4; i < 3 + Option.SatelliteTypes.Count; i++)
{
inverseWeight[i][i] = 0.25;//系统时间偏差
}
return(new WeightedVector(residualVec, inverseWeight));
}
/// <summary>
/// Ambizap算法的实现。
/// 选择两个点的PPP计算结果，及其基线计算结果。
/// </summary>
private void Read()
{
string pointNameA = null, pointNameB = null;
//---------------------------------------------------数据读取//---------------------------------------------------
//只提取两个点的位置信息及其基线。
//读取PPP结果
string[] pppfilePathes = this.textBox_PathesOfPPP.Lines;
List <SinexFile> pppsinexFiles = SinexReader.Read(pppfilePathes);
List <SinexSiteDetail> pppSitesA = new List <SinexSiteDetail>();
List <SinexSiteDetail> pppSitesB = new List <SinexSiteDetail>();
foreach (var file in pppsinexFiles)
{
List <SinexSiteDetail> sites = file.GetSinexSites();
foreach (var item2 in sites)
{
if (pointNameA == null)
{
pointNameA = item2.Name;
}
if (pointNameB == null && pointNameA != item2.Name)
{
pointNameB = item2.Name;
}
if (item2.Name == pointNameA)
{
pppSitesA.Add(item2);
}
if (item2.Name == pointNameB)
{
pppSitesB.Add(item2);
}
}
}
if (pointNameA == null || pointNameB == null)
{
throw new ArgumentException("起算数据不足。");
}
//读取基线结果
//只需要一条基线就行了。
string[] baseLineFilePathes = this.textBox_pathesOfBaseLine.Lines;
List <SinexFile> baseLineSinexFiles = SinexReader.Read(baseLineFilePathes);
List <SinexSiteDetail> baseLineSites = new List <SinexSiteDetail>();
foreach (var file in baseLineSinexFiles)
{
List <SinexSiteDetail> sites = file.GetSinexSites();
foreach (var item2 in sites)
{
if ((item2.Name == pointNameA || item2.Name == pointNameB) && (baseLineSites.Find(m => m.Name == item2.Name) == null))
{
baseLineSites.Add(item2);
}
if (baseLineSites.Count == 2)
{
break;
}
}
if (baseLineSites.Count == 2)
{
break;
}
}
//组建基线
string name = baseLineSites[0].Name + "-" + baseLineSites[1].Name;
BaseLine line = new BaseLine(name, baseLineSites[1].EstimateXYZ - baseLineSites[0].EstimateXYZ);
//---------------------------------------------------------平差计算--------------------------------------------
//以第一个坐标为近似值
XYZ aprioriA = pppSitesA[0].EstimateXYZ;
XYZ aprioriB = pppSitesB[0].EstimateXYZ;
//构建矩阵，具有约束条件的参数平差中，误差方程和约束方程的未知数相同。
//参数有6个x0,y0,z0,x1,y1,z1,条件有三个 x1-x0 = a, y1-y0 = b, z1-z0 = c
double[][] coeff_error = MatrixUtil.Create((pppSitesA.Count + pppSitesB.Count) * 3, 6);
double[][] indentity3x3 = MatrixUtil.CreateIdentity(3);
for (int i = 0; i < pppSitesA.Count; i++)
{
MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, i * 3, 0);
}
for (int i = 0; i < pppSitesB.Count; i++)
{
MatrixUtil.SetSubMatrix(coeff_error, indentity3x3, (pppSitesA.Count + i) * 3, 3);
}
//obsMinusApriori
double[] obsMinusApriori_error = new double[(pppSitesA.Count + pppSitesB.Count) * 3];
for (int i = 0; i < pppSitesA.Count; i++)
{
int rowIndex = i * 3;
obsMinusApriori_error[rowIndex + 0] = pppSitesA[i].EstimateXYZ.X - aprioriA.X;
obsMinusApriori_error[rowIndex + 1] = pppSitesA[i].EstimateXYZ.Y - aprioriA.Y;
obsMinusApriori_error[rowIndex + 2] = pppSitesA[i].EstimateXYZ.Z - aprioriA.Z;
}
for (int i = 0; i < pppSitesB.Count; i++)
{
int rowIndex = (pppSitesA.Count + i) * 3;
obsMinusApriori_error[rowIndex + 0] = pppSitesB[i].EstimateXYZ.X - aprioriB.X;
obsMinusApriori_error[rowIndex + 1] = pppSitesB[i].EstimateXYZ.Y - aprioriB.Y;
obsMinusApriori_error[rowIndex + 2] = pppSitesB[i].EstimateXYZ.Z - aprioriB.Z;
}
//Coeff_Condition,条件有三个 x0-x1 - a = 0,
//条件方程：c x + w = 0. => c deltaX + c X0 + w = 0
double[][] coeff_condition = MatrixUtil.Create(3, 6);
for (int index = 0; index < 3; index++)
{
coeff_condition[index][index + 0] = -1;
coeff_condition[index][index + 3] = 1;
}
//ConstVector_Condition c deltaX - W = 0, W = w - c X0
double[] obsMinusApriori_condition = new double[3];
XYZ aprioriVector = aprioriB - aprioriA;
obsMinusApriori_condition[0] = line.Vector.X - aprioriVector.X;
obsMinusApriori_condition[1] = line.Vector.Y - aprioriVector.Y;
obsMinusApriori_condition[2] = line.Vector.Z - aprioriVector.Z;
ParamAdjustmentWithCondition adjust = new ParamAdjustmentWithCondition(coeff_error, obsMinusApriori_error, coeff_condition, obsMinusApriori_condition);
double[][] param = adjust.Solve();
//---------------------------------------------------------结果输出--------------------------------------------
StringBuilder sb = new StringBuilder();
sb.AppendLine("计算完毕");
XYZ estA = aprioriA + new XYZ(adjust.ParamVector);
XYZ estB = aprioriB + new XYZ(adjust.ParamVector[3], adjust.ParamVector[4], adjust.ParamVector[5]);
sb.AppendLine("近似值A：" + aprioriA);
sb.AppendLine("平差值A：" + estA);
sb.AppendLine("近似值B：" + aprioriB);
sb.AppendLine("平差值B：" + estB);
sb.AppendLine("条件向量 A->B：" + line.Vector);
sb.AppendLine("平差向量 A->B：" + (estB - estA));
sb.AppendLine("------------------------------------参数------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(param));
double[][] param2 = adjust.Solve_ToBeCheck();
sb.AppendLine("param2");
sb.AppendLine(MatrixUtil.GetFormatedText(param2));
sb.AppendLine("误差方程系数阵");
sb.AppendLine(MatrixUtil.GetFormatedText(coeff_error));
sb.AppendLine("------------------------------------obsMinusApriori------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_error)));
sb.AppendLine("------------------------------------Coeff_Condition------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(coeff_condition));
sb.AppendLine("------------------------------------ConstVector_Condition------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(MatrixUtil.Create(obsMinusApriori_condition)));
sb.AppendLine("------------------------------------误差方程法方程------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_error));
sb.AppendLine("------------------------------------条件方程法方程------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.Normal_condition));
sb.AppendLine("------------------------------------误差方程右手边------------------------------------");
sb.AppendLine(MatrixUtil.GetFormatedText(adjust.RightHandSide_error));
this.textBox_info.Text = sb.ToString();
//----------------------------------------------------地图准备//----------------------------------------------------
points.Clear();
GeoCoord c1 = CoordTransformer.XyzToGeoCoord(aprioriA);
GeoCoord c2 = CoordTransformer.XyzToGeoCoord(aprioriB);
GeoCoord c3 = CoordTransformer.XyzToGeoCoord(estA);
GeoCoord c4 = CoordTransformer.XyzToGeoCoord(estB);
points.Add(new AnyInfo.Geometries.Point(c1.Lon, c1.Lat, "aprA"));
points.Add(new AnyInfo.Geometries.Point(c2.Lon, c2.Lat, "aprB"));
points.Add(new AnyInfo.Geometries.Point(c3.Lon, c3.Lat, "estA"));
points.Add(new AnyInfo.Geometries.Point(c4.Lon, c4.Lat, "estB"));
}
