/// <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); }