/// <summary>Construct singular value decomposition.</summary>
public SVD(ArrayMatrix A)
{
m = A.RowCount;
n = A.Columns;
U = new ArrayMatrix(m, n);
D = new double[n];
V = new ArrayMatrix(n, n);
ArrayMatrix E = new ArrayMatrix(n, n);
//E初始化为单位阵
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
E[i, j] = 0;
}
E[i, i] = 1;
}
double p = 1; //
double kci = 0.000001; //足够小的正数
while (p > kci)
{
p = 0;
for (int i = 0; i < n; i++)
{
double[] sum = new double[3];
for (int j = i + 1; j < n; j++)
{
sum[0] = 0; sum[1] = 0; sum[2] = 0;
for (int k = 0; k < m; k++)
{
sum[0] += A[k, i] * A[k, j];
sum[1] += A[k, i] * A[k, i];
sum[2] += A[k, j] * A[k, j];
}
double aa = 0; double bb = 0; double rr = 0;
double s = 0; double c = 0;
double[] temp = new double[m];
double[] temp1 = new double[n];
if (Math.Abs(sum[0]) > kci)
{
aa = 2 * sum[0];
bb = sum[1] - sum[2];
rr = Math.Sqrt(aa * aa + bb * bb);
if (bb >= 0)
{
c = Math.Sqrt((bb + rr) / (2 * rr));
s = aa / (2 * rr * c);
}
else
{
s = Math.Sqrt((rr - bb) / (2 * rr));
c = aa / (2 * rr * s);
}
for (int k = 0; k < m; k++)
{
temp[k] = c * A[k, i] + s * A[k, j];
A[k, j] = (-s) * A[k, i] + c * A[k, j];
}
for (int k = 0; k < n; k++)
{
temp1[k] = c * E[k, i] + s * E[k, j];
E[k, j] = (-s) * E[k, i] + c * E[k, j];
}
for (int k = 0; k < m; k++)
{
A[k, i] = temp[k];
}
for (int k = 0; k < n; k++)
{
E[k, i] = temp1[k];
}
if (Math.Abs(sum[0]) > p)
{
p = Math.Abs(sum[0]);
}
}
}
}
}
double sums = 0;
for (int i = 0; i < n; i++)
{
sums = 0;
for (int j = 0; j < m; j++)
{
sums += A[j, i] * A[j, i];
}
D[i] = Math.Sqrt(sums);
}
for (int j = 0; j < n; j++)
{
for (int i = 0; i < m; i++)
{
U[i, j] = A[i, j] / D[j];
}
}
V = E.Transpose();
}
//dictionary holding the information regarding every satellite
//private SortedDictionary<SatelliteNumber, List<DataItem>> satData { get; set; }
#endregion
/// <summary>
/// 如果有周跳，则返回 1. 否则没有探测出。
/// Method tat implements the LI cycle slip detection algorithm
/// </summary>
/// <param name="gpsTime">Time of observations</param>
/// <param name="prn">SatId</param>
/// <param name="liValue">Current LI observation value</param>
/// <returns></returns>
private bool GetDetection(Time gpsTime, SatelliteNumber prn, double liValue, bool alreadyHasSlip)
{
int isCS = 0; //没有周跳
var list = GetOrCreate(prn);
int s = list.Count;
//get current buffer aboutSize
// int bufferSize = satData[satelliteType].Count;
//Get the difference between current epoch and last epoch, in fraction, but prevObj test if we have epoch satData inside LIData
double deltaTime = 0.0;//difference between currrent and former epochs, in sec
if (s > 0)
{
deltaTime = Math.Abs(gpsTime - list.Last().Time);
}
else
{
deltaTime = Math.Abs(gpsTime - Time.StartOfMjd);
}
//check if receiver already declared cycle slip or too much time has elapsed
if (alreadyHasSlip || deltaTime > this.MaxTimeSpan)
{
list.Clear();
s = list.Count;
isCS = 1;
}
//check if we have enough satData to start processing
if (s < MinBufferSize)
{
isCS = 1;
//Let's prepare for the next epoch//store current epoch at the end of deque
list.Add(new TimeValue(gpsTime, liValue));
//默认没有周跳，误判比漏判更坏？？
return(true);
// return false;
}//没有足够的数据也算。
if (s >= MinBufferSize)
{
//declare a List for measurements
Geo.Algorithm.ArrayMatrix y = new Geo.Algorithm.ArrayMatrix(s, 1, 0.0);
//declare a Matrix for epoch information
Geo.Algorithm.ArrayMatrix M = new Geo.Algorithm.ArrayMatrix(s, 3, 0.0);
//we store here the oldest (or prevObj) epoch in buffer for future reference. this is important because adjustment will be made with respect to that prevObj epoch
Time firstEpoch = list.First().Time;
//feed 'y' with satData
for (int i = 0; i < s; i++)
{
//the newest goes prevObj in 'y'
y[i, 0] = list[s - 1 - i].Value;
}
//feed 'M' with satData
for (int i = 0; i < s; i++)
{
//compute epoch difference with respect to prevObj epoch
double dT = list[s - 1 - i].Time - firstEpoch;
M[i, 0] = 1.0;
M[i, 1] = dT;
M[i, 2] = dT * dT;
}
//now, proceed to find a 2nd order fiting curve using a least mean squares(LMS) adjustment
Geo.Algorithm.ArrayMatrix MT = M.Transpose();
//Geo.Algorithm.Matrix covMatrix1 = MT * M;
var covMatrix = Geo.Algorithm.ArrayMatrix.ATA(M);
//let's try to invert MT*M matrix
try
{
covMatrix = covMatrix.GetInverse();//.Inverse;
}
catch
{
//if covMatrix can't be inverted we have a serious problem with satData, so reset buffer and declare cycle slip
list.Clear();
//return true;
isCS = 1;
//Let's prepare for the next epoch//store current epoch at the end of deque
list.Add(new TimeValue(gpsTime, liValue));
return(true);
}
//Now, compute the Vector holding the results of adjustment to second order curve
IMatrix a = covMatrix.Multiply(MT).Multiply(y);// *MT * y;
//the nest step is to compute the maximum deviation from adjustment, in order to assess if our adjustment is too noisy
double maxDeltaLI = 0.0;
for (int i = 0; i < s; i++)
{
int index = s - 1 - i;
// if (satData[satelliteType].Count <= index + 1) continue; //出错
//compute epoch difference with respect to prevObj epoch
double dT = list[index].Time - firstEpoch;
//compute adjusted LI value，二次多项式拟合值
double LIa = a[0, 0] + a[1, 0] * dT + a[2, 0] * dT * dT;
//find maximum deviation in current satData buffer
double deltaLI = Math.Abs(LIa - list[index].Value);
if (deltaLI > maxDeltaLI)
{
maxDeltaLI = deltaLI;
}
}
//compute epoch difference with respect to prevObj epoch
double deltaT = gpsTime - firstEpoch;
//compute current adjusted LI value
double currentLIa = a[0, 0] + a[1, 0] * deltaT + a[2, 0] * deltaT * deltaT;
//difference between current and adjusted LI value
double currentBias = Math.Abs(currentLIa - liValue);
//we will continue processsing only if we trust our current adjustment, time.e: it is not too noisy.
if ((2.0 * maxDeltaLI) < currentBias)
{
//compute limit to declare cycle slip
// double deltaTime = this.MaxBreakingEpochCount * IntervalSecond;
double deltaLimit = SatThreshold / (1.0 + (1.0 / Math.Exp(deltaTime / TimeConst)));
//check if current LI deviation is above deltaLimit threshold
if (currentBias > deltaLimit)
{
//reset buffer and declare cycle slip
list.Clear();
// return true;
isCS = 1;
}
}
}
//Let's prepare for the next epoch//store current epoch at the end of deque
list.Add(new TimeValue(gpsTime, liValue));
//check if we have exceeded maximum window aboutSize
if (list.Count > MaxBufferSize)
{
list.RemoveAt(0);
}
// return false;
return(isCS == 1);
}
