public static void writeVectorToFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector vector, string file, System.Windows.Forms.RichTextBox tb1, MainForm frm)
{
System.IO.Stream fileStream = new System.IO.FileStream(file, System.IO.FileMode.Create);
System.IO.BinaryWriter bw = new System.IO.BinaryWriter(fileStream, System.Text.Encoding.Default);
double[] arr = vector.Values;
System.IO.TextWriter tr = new System.IO.StreamWriter(file + ".metainfo");
tr.WriteLine("Items count");
tr.WriteLine(vector.Count);
tr.Flush();
tr.Close();
int len = arr.Length;
vector = null;
int index = 0;
byte[] bytes = new byte[arr.Length * 8];
foreach (double item in arr)
{
BitConverter.GetBytes(item).CopyTo(bytes, index);
if (index / 8 / 500D == Math.Round(index / 8 / 500D))
{
tb1.BeginInvoke(new MainForm.setProgressDel(frm.addVal), new object[] { (int)index / 8, len, "Записано" });
}
index += 8;
}
bw.Write(bytes);
bw.Flush();
tb1.BeginInvoke(new MainForm.setProgressDel(frm.addVal), new object[] { (int)0, len, "" });
bw.Flush();
bw.Close();
fileStream.Close();
}
private void Simulate(int expiry, int tenor)
{
var accumulateSum = new DenseVector[tenor + 1];
for (int i = 0; i < tenor + 1; i++)
{
accumulateSum[i] = new DenseVector(_param.NumberOfFactors);
}
for (int i = 0; i < expiry; i++)
{
var bm = BrownianMotion.BMStep(0.25, _param.NumberOfFactors);
accumulateSum[i].Clear();
for (int j = i + 1; j < tenor + 1; j++)
{
double temp = System.Math.Max(0, System.Math.Min(1, _v[i][j] / _discount[i][j]));
accumulateSum[j] = accumulateSum[j - 1] + temp * _xi[i].RowD(j - i - 1);//accumulateSum[j - 1] + temp * _xi[i][j - i - 1, Range.All];
}
for (int j = tenor; j > i; j--)
{
var vol = _xi[i].RowD(j - i - 1) - accumulateSum[j]; //_xi[i][j - i - 1, Range.All] - accumulateSum[j];
_v[i + 1][j] = _v[i][j] * System.Math.Exp(vol * bm - 0.5 * (vol * vol) * 0.25);
if (j < tenor)
{
_discount[i + 1][j] = _discount[i + 1][j + 1] * (1 - 0.25 * _shift[j]) + _v[i + 1][j];
}
else
{
_discount[i + 1][j] = _v[i + 1][j];
}
}
}
}
private ExitCondition ExitCriteriaSatisfied(IEvaluation candidate_point, IEvaluation last_point)
{
Vector <double> rel_grad = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(candidate_point.Point.Count);
double relative_gradient = 0.0;
double normalizer = Math.Max(Math.Abs(candidate_point.Value), 1.0);
for (int ii = 0; ii < rel_grad.Count; ++ii)
{
double tmp = candidate_point.Gradient[ii] * Math.Max(Math.Abs(candidate_point.Point[ii]), 1.0) / normalizer;
relative_gradient = Math.Max(relative_gradient, Math.Abs(tmp));
}
if (relative_gradient < this.GradientTolerance)
{
return(ExitCondition.RelativeGradient);
}
if (last_point != null)
{
double most_progress = 0.0;
for (int ii = 0; ii < candidate_point.Point.Count; ++ii)
{
var tmp = Math.Abs(candidate_point.Point[ii] - last_point.Point[ii]) / Math.Max(Math.Abs(last_point.Point[ii]), 1.0);
most_progress = Math.Max(most_progress, tmp);
}
if (most_progress < this.ParameterTolerance)
{
return(ExitCondition.LackOfProgress);
}
}
return(ExitCondition.None);
}
public static void writeVectorToTxtFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector dm, string file)
{
StringBuilder sb = new StringBuilder();
foreach (var item in dm.Values)
{
sb.AppendFormat("{0,5:0.0000000000000e+00}\r\n", item);
}
System.IO.File.WriteAllText(file, sb.ToString());
}
///<summary>
///</summary>
///<param name="i"></param>
///<returns></returns>
public MathNet.Numerics.LinearAlgebra.Double.DenseVector RowD(int i)
{
var sp = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(ColumnCount);
for (int j = 0; j < ColumnCount; j++)
{
sp[j] = this[i, j];
}
return(sp);
}
/// <summary>
/// Run tests for Math.Net
/// </summary>
/// <seealso cref="http://www.mathdotnet.com/"/>
/// <param name="results">The collection of results to add to</param>
/// <remarks>Included using Nuget</remarks>
private static void RunMathNetTests(Dictionary <string, long> results)
{
var libraryName = "MathNet";
var a = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[] { 2, 4, 6 });
var b = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[] { 5, 7, 10 });
results.Add(
"Cross Product => " + libraryName,
Measure(
iterations,
() =>
{
var result = a.OuterProduct(b); // Not sure if this is cross product or not
}));
results.Add(
"Dot Product => " + libraryName,
Measure(
iterations,
() =>
{
var result = a.DotProduct(b);
}));
results.Add(
"Normalize => " + libraryName,
Measure(
iterations,
() =>
{
var result = a.Normalize(0);
}));
results.Add(
"Add => " + libraryName,
Measure(
iterations,
() =>
{
var result = a + b;
}));
results.Add(
"Subtract => " + libraryName,
Measure(
iterations,
() =>
{
var result = a - b;
}));
}
public double Get3DNoiseValue(double x, double y, double z)
{
// find unit grid cell containing point
int floorX = (int)Math.Floor(x);
int floorY = (int)Math.Floor(y);
int floorZ = (int)Math.Floor(z);
// get relative XYZ coordinates of point in cell
double relX = x - floorX;
double relY = y - floorY;
double relZ = z - floorZ;
//gradients of cube vertices
VectorD g000 = GetGradient(floorX, floorY, floorZ);
VectorD g001 = GetGradient(floorX, floorY, floorZ + 1);
VectorD g010 = GetGradient(floorX, floorY + 1, floorZ);
VectorD g011 = GetGradient(floorX, floorY + 1, floorZ + 1);
VectorD g100 = GetGradient(floorX + 1, floorY, floorZ);
VectorD g101 = GetGradient(floorX + 1, floorY, floorZ + 1);
VectorD g110 = GetGradient(floorX + 1, floorY + 1, floorZ);
VectorD g111 = GetGradient(floorX + 1, floorY + 1, floorZ + 1);
// noise contribution from each of the eight corner
double n000 = g000 * new VectorD(new[] { relX, relY, relZ });
double n100 = g100 * new VectorD(new[] { relX - 1, relY, relZ });
double n010 = g010 * new VectorD(new[] { relX, relY - 1, relZ });
double n110 = g110 * new VectorD(new[] { relX - 1, relY - 1, relZ });
double n001 = g001 * new VectorD(new[] { relX, relY, relZ - 1 });
double n101 = g101 * new VectorD(new[] { relX - 1, relY, relZ - 1 });
double n011 = g011 * new VectorD(new[] { relX, relY - 1, relZ - 1 });
double n111 = g111 * new VectorD(new[] { relX - 1, relY - 1, relZ - 1 });
// compute the fade curve value for each x, y, z
double u = BlendingCurve(relX);
double v = BlendingCurve(relY);
double w = BlendingCurve(relZ);
// interpolate along x the contribution from each of the corners
double nx00 = Interpolation(n000, n100, u);
double nx01 = Interpolation(n001, n101, u);
double nx10 = Interpolation(n010, n110, u);
double nx11 = Interpolation(n011, n111, u);
// interpolate the four results along y
double nxy0 = Interpolation(nx00, nx10, v);
double nxy1 = Interpolation(nx01, nx11, v);
// interpolate the two last results along z
double nxyz = Interpolation(nxy0, nxy1, w);
return(nxyz);
}
private void AddCorner(int id, int t1, int t2, int t3)
{
TCorner c = Corners[id];
TTile[] t = new[] { Tiles[t1], Tiles[t2], Tiles[t3] };
VectorD v = t[0].TileCenterPosition + t[1].TileCenterPosition + t[2].TileCenterPosition;
c.V = (VectorD)v.Normalize(2.0);
for (int i = 0; i < 3; i++)
{
t[i].Corners[t[i].GetTilePosition(t[(i + 2) % 3])] = c;
c.Tiles[i] = t[i];
}
}
public static MathNet.Numerics.LinearAlgebra.Double.DenseVector readDenceVectorFromBinFile(string file, MainForm.setProgressDel d)
{
string[] meta = System.IO.File.ReadAllLines(file + ".metainfo");
System.IO.FileStream fs = new System.IO.FileStream(file, System.IO.FileMode.Open);
System.IO.BinaryReader br = new System.IO.BinaryReader(fs, System.Text.Encoding.Default);
int cols = int.Parse(meta[1]);
MathNet.Numerics.LinearAlgebra.Double.DenseVector dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(cols);
for (int j = 0; j < cols; j++)
{
dv[j] = br.ReadDouble();
d.Invoke(br.BaseStream.Position / br.BaseStream.Length, 1, "Завантаження");
}
br.Close();
fs.Close();
fs.Dispose();
return(dv);
}
public Vector3 GetDerivative(float t, int order)
{
// make sure t is in range [0,1]
Assert.IsFalse(t < 0.0f || 1.0f < t, "t is not in Range [0,1]: t = " + t);
Matrix <double> casteljau = GetCasteljauMatrix(pointsList.Count);
Matrix <double> points = GetPointsMatrix();
// use calculus derivative rules to generically derive the curve
//tVector = ((n-o)!*t^(n-o), (n-o-1)!*t^(n-o-2), ... 1);
Vector <double> tVector = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(pointsList.Count);
for (int i = 0; i < pointsList.Count; i++)
{
int exponent = pointsList.Count - i - 1; //count down - 1
double derivationFactor;
if (order == 0) // no derivative
{
derivationFactor = 1;
}
else if (exponent <= order - 1) // constants and below are thrown away thus set to 0
{
tVector[i] = 0;
continue;
}
else
{
derivationFactor = SpecialFunctions.Factorial(exponent) / SpecialFunctions.Factorial(exponent - order);
}
tVector[i] = derivationFactor * Mathf.Pow(t, exponent - order);
}
//now calculate the actual result
Vector <double> result = points * casteljau * tVector;
Vector3 ret;
ret.x = (float)result[0];
ret.y = (float)result[1];
ret.z = (float)result[2];
return(ret);
}
private D gradfD(double z, double[] a)
{
D result;
double estimatedError;
D zD = D.NewD(z);
var xvec = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);
var yvec = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);
var dydx = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(a.Length);
//Func<double, double> fbeta = x => fD(z, a);
//Func<D, D[], D> fbeta2 = (x, y) => fD(zD, a);
DiffSharp.Interop.D[] aD = new DiffSharp.Interop.D[100];
D[] aDF = new D[100];
// convert double arr to D[], two types
// also put in to densearrays
for (int i = 0; i < a.Length; i++)
{
aD[i] = a[i]; // interop type
//aDF[i] = fbeta2(zD, aDF); // forward type
}
// nonsense
//result = fbeta2(zD, aDF);
//result = fbeta.ForwardDerivative(a[0], out estimatedError);
//var gfDel = fbeta2(zD, aDF);
//var gf = DiffSharp.Interop.AD.Grad(x => fbeta(z, a), a);
//var gf1 = DiffOps.grad(fbeta2(zD, aDF), aDF);
//var iRes = gf.DynamicInvoke(aD);
return(D.One);
}
private double[] Polyfit(double[] x, double[] y, int degree)
{
// Vandermonde matrix
var v = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(x.Length, degree + 1);
for (int i = 0; i < v.RowCount; i++)
{
for (int j = 0; j <= degree; j++)
{
v[i, j] = Math.Pow(x[i], j);
}
}
var yv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(y).ToColumnMatrix();
QR <double> qr = v.QR(QRMethod.Full);
// Math.Net doesn't have an "economy" QR, so:
// cut R short to square upper triangle, then recompute Q
var r = qr.R.SubMatrix(0, degree + 1, 0, degree + 1);
var q = v.Multiply(r.Inverse());
var p = r.Inverse().Multiply(q.TransposeThisAndMultiply(yv));
return(p.Column(0).ToArray());
}
public static double GetNumericCLC(List<double> d)
{
//d = new List<double> { 2, 3, 4, 5, 6, 7, 8 };
List<double> xs = new List<double>();
for (int i = 0; i < d.Count; ++i)
xs.Add(i + 1);
var X = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.CreateFromColumns(
new[] { new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xs.Count, 1),
new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xs.ToArray<double>()) });
var y = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(d.ToArray<double>());
var p = MathNet.Numerics.LinearAlgebra.Double.ExtensionMethods.QR(X).Solve(y);
var a = p[0];
var b = p[1];
double sum = 0;
for (int i = 0; i < d.Count; ++i)
{
double yy = xs[i] * a + b;
sum += Math.Abs(yy - d[i]);
}
return sum / d.Count;
}
/// <summary>
/// Find Homography.
/// </summary>
/// <param name="source"> The source Transformer. </param>
/// <param name="destination"> The destination Transformer. </param>
/// <returns> The homologous matrix. </returns>
public static Matrix3x2 FindHomography(ITransformerLTRB source, ITransformerLTRB destination)
{
float x0 = source.LeftTop.X, x1 = source.RightTop.X, x2 = source.LeftBottom.X, x3 = source.RightBottom.X;
float y0 = source.LeftTop.Y, y1 = source.RightTop.Y, y2 = source.LeftBottom.Y, y3 = source.RightBottom.Y;
float u0 = destination.LeftTop.X, u1 = destination.RightTop.X, u2 = destination.LeftBottom.X, u3 = destination.RightBottom.X;
float v0 = destination.LeftTop.Y, v1 = destination.RightTop.Y, v2 = destination.LeftBottom.Y, v3 = destination.RightBottom.Y;
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix matrix = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.OfArray(new double[008, 008]
{
{ x0, y0, 1, 0, 0, 0, -u0 * x0, -u0 * y0 },
{ 0, 0, 0, x0, y0, 1, -v0 * x0, -v0 * y0 },
{ x1, y1, 1, 0, 0, 0, -u1 * x1, -u1 * y1 },
{ 0, 0, 0, x1, y1, 1, -v1 * x1, -v1 * y1 },
{ x3, y3, 1, 0, 0, 0, -u3 * x3, -u3 * y3 },
{ 0, 0, 0, x3, y3, 1, -v3 * x3, -v3 * y3 },
{ x2, y2, 1, 0, 0, 0, -u2 * x2, -u2 * y2 },
{ 0, 0, 0, x2, y2, 1, -v2 * x2, -v2 * y2 },
});
MathNet.Numerics.LinearAlgebra.Double.DenseVector vector = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(new double[008]
{
u0, v0, u1, v1,
u3, v3, u2, v2
});
MathNet.Numerics.LinearAlgebra.Matrix <double> PseudoInverse = Transformer.PseudoInverse2(matrix);
MathNet.Numerics.LinearAlgebra.Vector <double> ret = Transformer.Multiply2(PseudoInverse, vector);
return(new Matrix3x2
(
m11: (float)ret[0], m12: (float)ret[3],
m21: (float)ret[1], m22: (float)ret[4],
m31: (float)ret[2], m32: (float)ret[5]
));
}
public static MathNet.Numerics.LinearAlgebra.Double.DenseVector readDenceVectorFromBinFile(string file, MainForm.setProgressDel d)
{
string[] meta = System.IO.File.ReadAllLines(file + ".metainfo");
System.IO.FileStream fs = new System.IO.FileStream(file, System.IO.FileMode.Open);
System.IO.BinaryReader br = new System.IO.BinaryReader(fs, System.Text.Encoding.Default);
int cols = int.Parse(meta[1]);
MathNet.Numerics.LinearAlgebra.Double.DenseVector dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(cols);
for (int j = 0; j < cols; j++)
{
dv[j] = br.ReadDouble();
d.Invoke(br.BaseStream.Position / br.BaseStream.Length, 1, "Завантаження");
}
br.Close();
fs.Close();
fs.Dispose();
return dv;
}
/// <summary>
/// Create steering vectors geometrically for each discrete frequency under the far-field assumption.
/// </summary>
/// <param name="microphones">The microphones.</param>
/// <param name="direction">The horizontal direction of arrival of the sound.</param>
/// <param name="pitch">The vertical direction of arrival of the sound.</param>
/// <param name="sampleRate">The sampling frequency.</param>
/// <param name="dftLength">The length of the DFT.</param>
/// <returns>
/// The steering vectors for each discrete frequency.
/// Since the components higher than the Nyquist frequency are discarded,
/// the length of the returned array is dftLength / 2 + 1.
/// </returns>
public static Vector <Complex>[] FromFarFieldGeometry(IReadOnlyList <Microphone> microphones, double direction, double pitch, int sampleRate, int dftLength)
{
if (microphones == null)
{
throw new ArgumentNullException(nameof(microphones));
}
if (microphones.Count == 0)
{
throw new ArgumentException(nameof(microphones), "The number of microphones must be greater than zero.");
}
if (microphones.Any(mic => mic == null))
{
throw new ArgumentException(nameof(microphones), "All the microphone object must be non-null.");
}
if (sampleRate <= 0)
{
throw new ArgumentException(nameof(sampleRate), "The sampling frequency must be greater than zero.");
}
if (dftLength <= 0 || dftLength % 2 != 0)
{
throw new ArgumentException(nameof(dftLength), "The length of the DFT must be positive and even.");
}
var channelCount = microphones.Count;
var destination = new Vector <Complex> [dftLength / 2 + 1];
for (var w = 0; w < destination.Length; w++)
{
destination[w] = new DenseVector(channelCount);
}
var dv = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(3);
dv[0] = Math.Cos(direction) * Math.Cos(pitch);
dv[1] = Math.Sin(direction) * Math.Cos(pitch);
dv[2] = Math.Sin(pitch);
var distances = new double[channelCount];
for (var ch = 0; ch < channelCount; ch++)
{
distances[ch] = dv * microphones[ch].Position;
}
var offset = distances.Min();
for (var ch = 0; ch < channelCount; ch++)
{
distances[ch] -= offset;
}
for (var ch = 0; ch < channelCount; ch++)
{
var distance = distances[ch];
var time = distance / AcousticConstants.SoundSpeed;
var delaySampleCount = sampleRate * time;
var delayFilter = Filtering.CreateFrequencyDomainDelayFilter(dftLength, delaySampleCount);
for (var w = 0; w < destination.Length; w++)
{
destination[w][ch] = delayFilter[w];
}
}
return(destination);
}
private void button4_Click(object sender, EventArgs e)
{
Action fileProc = () =>
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
MathNet.Numerics.Control.LinearAlgebraProvider = new MathNet.Numerics.Algorithms.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
MathNet.Numerics.Control.NumberOfParallelWorkerThreads = Environment.ProcessorCount;
addText("Обробка файлу вимірювань...\r\n");
double[][] SGG_data = null;
if (System.IO.File.Exists("sgg_data.bin"))
{
SGG_data = IOFunc.binLoad_SGG_data("sgg_data.bin");
}
else
{
SGG_data = Data.IOFunc.read_SGG_data(SphericalHarmonicAnalyze.Properties.Settings.Default.SGG_measures, new setProgressDel(addVal));
IOFunc.binwrite_SGG_data("sgg_data.bin", SGG_data);
}
addText("Дані вимірювань оброблено: {0} шт.\r\n", SGG_data.Length); Thread.Sleep(500);
ReferenceSystem elipsoid = new ReferenceSystem(ReferenceSystem.Default.TideFree);
elipsoid.gridParameters.cellSize = SphericalHarmonicAnalyze.Properties.Settings.Default.GridCellSize;
elipsoid.gridParameters.coLatitudeBounds = SphericalHarmonicAnalyze.Properties.Settings.Default.minCoLatitude;
elipsoid.maxDegree = SphericalHarmonicAnalyze.Properties.Settings.Default.modelMaxOrder;
int greedColumnsCount, greedRowsCount;
List<double[]> greed = MathFunc.generateGrid(elipsoid.gridParameters.cellSize, out greedColumnsCount, out greedRowsCount, elipsoid.gridParameters.coLatitudeBounds,180 - elipsoid.gridParameters.coLatitudeBounds);
addText("Сітку згенеровано: {0} комірок \r\n", greed.Count);
double avgR = MathFunc.getAvgRadius(SGG_data);
List<int>[] map = MathFunc.getMappingOfPoints(elipsoid, SGG_data, greed.ToArray(), greedRowsCount, greedColumnsCount, avgR); sw.Stop(); addText("Точки віднесено до комірок сітки за: {0}.\r\n", sw.Elapsed.ToString());
addText("Кількість клітинок сітки всього: {0}\r\n", greed.Count);
int res1 = 0; foreach (var item in map) { res1 += item.Count; } addText("Використано вимірів: {0}\r\nСер радіус: {1}\r\n", res1, avgR);
test.checkMap(SGG_data, map, greed, elipsoid);
List<int>[] newMap = null;
MathFunc.checkGreed(ref greed, map, out newMap);
addText("Кількість клітинок сітки, в яких присутні дані вимірювань: {0}\r\n", greed.Count);
map = newMap; newMap = null;
IOFunc.writeGreedToCsvFileWithMeasureCount(greed, map, "greed_new_map.txt");
double[] avgRadius; sw.Restart();
double[] regularisedValues = MathFunc.regularization(SGG_data, greed.ToArray(), map, out avgRadius); sw.Stop(); addText("Регуляризація (на основі сферичної відстані) виконана за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeGreedToCsvFileWithMeasureS(greed,regularisedValues, "greed_regular_grad.txt");
avgRadius[0] = Math.Round(avgRadius[0]);
elipsoid.satelliteSphere = avgRadius[0];
addText("Середній радіус: {0,10:0.000}.\r\nМінімальний радіус: {1,10:0.0000}\r\nМаксимальний радіус:{2,10:0.0000}\r\n", avgRadius[0], avgRadius[1], avgRadius[2]);
SGG_data = null; map = null;
int[][] t_nm = MathFunc.get_nm(elipsoid.maxDegree);
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(greed.Count, (MathFunc.getArraySize(elipsoid.maxDegree) - 3) * 2 - (elipsoid.maxDegree-1));
sw.Stop(); addText("Пам'ять для матриці коефіцієнтів виділено за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
int progress = 0;
//Обчислення елементів матриці
var p= Parallel.For(0, dm.RowCount, (i) =>
{
double[] line = MathFunc.getCoefMatrixLineKoop(elipsoid, elipsoid.maxDegree, t_nm, elipsoid.satelliteSphere, greed[i][0], greed[i][1]);
lock (dm)
{
dm.SetRow(i,line);
}
progress++;
if (progress / 100D == Math.Round(progress / 100D)) {addVal(progress, dm.RowCount, "Визначено");}
});
if (!p.IsCompleted) { throw new Exception("Parallel.For"); };
IOFunc.writeMatrixToMatLabFile(dm, @"matlab\A.mat","A");
sw.Stop();
richTextBox1.Invoke(new setProgressDel(addVal), new object[] { 0, dm.RowCount, "" });
addText("Матриця {0} на {1} ({2}MB) згенерована за: {3,10}\r\n", dm.RowCount, dm.ColumnCount, dm.ColumnCount * dm.RowCount * 8 / 1000000,sw.Elapsed.ToString()/* + "\r\nЗапис у файл...\r\n"*/);
if(true){
GravityModel gm08 = new GravityModel(elipsoid.maxDegree);
gm08.loadFromFile("GO_CONS_EGM_GCF_2.gfc", new setProgressDel(addVal));
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm08.getGradientForGrid(elipsoid,greed));//regularisedValues);
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL2;
GravityModel gm = new GravityModel(elipsoid.maxDegree);
if (radioButton1.Checked) {
sw.Restart();
gm.loadFromFile(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel, new setProgressDel(addVal));
sw.Stop(); addText("Вихідна модель завантажена за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid,greed));
sw.Stop(); addText("Градієнти за вихідною моделлю обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
else
{
sw.Restart();
gm = GravityModel.getNormalModel(elipsoid, elipsoid.maxDegree);
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
sw.Stop(); addText("Нормальні градієнти обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
dmL = dmL - dmL2;
dmL2 = null;
IOFunc.writeMatrixToMatLabFile(dmL.ToColumnMatrix(), @"matlab\L.mat", "L");
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmLNormal = null;
dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dm.TransposeThisAndMultiply(dmL);
dmL = null;
IOFunc.writeMatrixToMatLabFile(dmLNormal.ToColumnMatrix(), @"matlab\LNorm.mat", "LNorm");
sw.Stop(); addText("Стовпчик вільних членів обчислений за: {0}.\r\n", sw.Elapsed.ToString());
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmANorm = null;
sw.Restart();
dmANorm = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.TransposeThisAndMultiply(dm); dm = null;
sw.Stop(); addText("Нормальна матриця коефіціэнтів обчислена за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeMatrixToMatLabFile(dmANorm, @"matlab\ANorm.mat", "ANorm");
//dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dmLNormal.Multiply(5e-8);
var x = dmANorm.Inverse();
var res = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)x.Multiply(dmLNormal);
IOFunc.writeModeVectorlToTxtFile(res, elipsoid, @"matlab\_out.AL");
addText(@"Результат за методом A\L знайдено...");
x = null;
GravityModel gm_R = new GravityModel(gm);
gm_R.addDeltaCoef(res.ToArray()); res = null;
double[] h = GravityModel.getGeoidHeight(elipsoid, gm_R, greed);
double[] dg = GravityModel.getAnomaly(elipsoid, gm_R, greed);
IOFunc.writeGeoidHeightsAndAnomalysToTxt(greed, h, dg, elipsoid, @"output\result_AL.txt");
IOFunc.writeGravityModelToTxtFile(gm_R, @"output\model_AL.gcf");
sw.Restart();
addText(dmANorm.Rank().ToString() + "\r\n");
dmANorm = null;
dmLNormal = null;
sw.Stop(); addText("Невідомі знайдено за: {0}.\r\n", sw.Elapsed.ToString());
}
};
if (System.IO.File.Exists(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel)) {
tabControl1.SelectedTab = tabControl1.TabPages[1];
this.UseWaitCursor = true;
ts = new CancellationTokenSource();
ct = ts.Token;
tsk = Task.Factory.StartNew(fileProc,ct);
var setCur = Task.Factory.StartNew(() => { tsk.Wait(); this.UseWaitCursor = false; addText("Обчислення завершені!"); });
richTextBox1.SaveFile(@"output\zvit.rtf");
}
}
private double PolynomialRegressionAndDerivativeAtMidPoint_MathNet(Vector3[] points, ref Vector2 midPoint)
{
int numPoints = points.Length;
double[] xd = new double[points.Length];
double[] yd = new double[points.Length];
//checking if the points are actualy spread over X-axis.
//other wise I swap the X-Y axises to make te polyfit function of matlab work.
Vector2 first = points[0].Xy;
Vector2 last = points[points.Length - 1].Xy;
bool invereseXY = false;
if (Math.Abs(first.X - last.X) < Math.Abs(first.Y - last.Y))
{
invereseXY = true;
}
if (invereseXY)
{
first = new Vector2(first.Y, first.X);
last = new Vector2(last.Y, last.X);
}
Vector2 midOfFirstLast = (first + last) / 2;
Common.Line2D FLLine = new Common.Line2D(first, last);
Common.Line2D midPerpLine = new Common.Line2D(midOfFirstLast, FLLine.PerpendicularMu());
if (!invereseXY)
{
for (int i = 0; i < points.Length; i++)
{
xd[i] = points[i].X;
yd[i] = points[i].Y;
}
}
else
{
for (int i = 0; i < points.Length; i++)
{
xd[i] = points[i].Y;
yd[i] = points[i].X;
}
}
//Vector2d midPoint = new Vector2d(xd[minDistToMidPerpLineIndex], yd[minDistToMidPerpLineIndex]);
var vx = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(xd);
var vy = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(yd);
Common.PolynominalRegression poly = new Common.PolynominalRegression(vx, vy, 3);
switch (config.bbpChoice)
{
case OrthoAid.Common.BBPChoice.User:
if (invereseXY)
{
midPoint = new Vector2(midPoint.Y, midPoint.X);
}
break;
case OrthoAid.Common.BBPChoice.AutoMiddle:
Vector2[] crossPoints = midPerpLine.CrosspointWithCurve(poly.GetCoefficients());
if (crossPoints.Length == 1)
{
midPoint = crossPoints[0];
}
else
{
midPoint = crossPoints[0];
for (int i = 1; i < crossPoints.Length; i++)
{
if ((crossPoints[i] - midOfFirstLast).LengthSquared <
(midPoint - midOfFirstLast).LengthSquared)
{
midPoint = crossPoints[i];
}
}
}
break;
default:
break;
}
double mu = poly.DerivativeAtPoint(midPoint.X);
//var t = new MathNet.Numerics.Interpolation.Algorithms.LinearSplineInterpolation(
if (invereseXY)
{
mu = 1 / mu;
midPoint = new Vector2(midPoint.Y, midPoint.X);
}
return(mu);
}
private void save_thread(object none)
{
int selected = selected_record;
byte[] full_file = File.ReadAllBytes("C:\\temp\\log_" + selected + ".txt");
Dispatcher.Invoke(new Action(() => set_stage("stage 1")));
uint[] ticks = new uint[full_file.Length / 35 + 1]; // примерное количество IMU пакетов
uint[] ticks2 = new uint[ticks.Length/15]; // примерное количество GPS пакетов неизвестно,
byte[] type = new byte[ticks.Length]; // но оно примерно в 20 раз меньше (взято с запасом)
byte[] pack = new byte[32];
byte[] pack2 = new byte[26];
int crc;
int k = 0;
int k2 = 0;
int tt = 0;
int[] counter = new int[ticks.Length];
double[,] a = new double[ticks.Length, 3];
double[,] w = new double[ticks.Length, 3];
double[,] m = new double[ticks.Length, 3];
double[,] q = new double[ticks.Length, 4];
double[] anglex = new double[ticks.Length];
double[] angley = new double[ticks.Length];
double[] anglez = new double[ticks.Length];
double[] lat = new double[ticks2.Length];
double[] lon = new double[ticks2.Length];
double[] speed = new double[ticks2.Length];
double[] course = new double[ticks2.Length];
double[] time = new double[ticks2.Length];
double[] stat = new double[ticks2.Length];
double[] date = new double[ticks2.Length];
byte[] buffer = new byte[2];
byte[] buffer2 = new byte[4];
double[] corr_mag = { 1, 2.2391, 1, 2.1586, 2.3980,
1, 1, 1.9201, 1.9963, 2.1538,
1, 1, 2.7105, 1.9782, 1.9013 };
double[] corr_accl = { 1, 0.9778, 1, 0.8928, 1.3899,
1, 0.9722, 0.9766, 0.9599, 0.9379,
0.9183, 0.9924, 1.2657, 0.9892, 0.9598 };
double[,] corr_gyr = { {1, 1, 1, 1, 1, 1, 1, 1, 1}, // 1
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 2
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 3
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 4
{-0.00373042367688305, 1.4558979622744, -0.0258818845609887, 0.00460253725775466, 1.4522057450117,
-0.035372440971534, -0.00301630637615598, 1.46124337866467, 0.0112681875599293}, // 5
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 6
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 7
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 8
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 9
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 10
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 11
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 12
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 13
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 14
{1, 1, 1, 1, 1, 1, 1, 1, 1}, // 15
}; // Gyro correction coeffs are listed in XYZ order
double accl_scale = 0;
if (block_index < 16)
accl_scale = corr_accl[block_index - 1];
double mag_scale = 0;
if (block_index < 16)
mag_scale = corr_mag[block_index - 1];
double[] gyr_scale = new double[9];
if (block_index < 16)
for (int t = 0; t < 9; t++)
gyr_scale[t] = corr_gyr[block_index - 1, t];
addText("\n" + get_time() + " Начало сохранения файла...");
for (int i = 0; i < full_file.Length - 30; i++)
{
if ((i < full_file.Length - 35) && (full_file[i + 34] == 3) && (full_file[i + 33] == 16) &&
(full_file[i] == 16) && (full_file[i + 1] == 49)) // условие начала IMU пакета
{
crc = 0;
for (int j = 0; j < 32; j++)
{
pack[j] = full_file[i + j + 1];
if (j < 31)
crc = crc ^ pack[j];
}
if (crc == pack[pack.Length - 1])
{
ticks[k] = BitConverter.ToUInt32(pack, 1);
type[k] = pack[0];
if (type[k] == 49)
{
buffer[0] = pack[7]; buffer[1] = pack[8];
a[k, 0] = ((double)BitConverter.ToInt16(buffer, 0)) * 0.001766834114354 *accl_scale;
buffer[0] = pack[5]; buffer[1] = pack[6];
a[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;
buffer[0] = pack[9]; buffer[1] = pack[10];
a[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.001766834114354 *accl_scale;
buffer[0] = pack[13]; buffer[1] = pack[14];
w[k, 0] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
buffer[0] = pack[11]; buffer[1] = pack[12];
w[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
buffer[0] = pack[15]; buffer[1] = pack[16];
w[k, 2] = -(double)BitConverter.ToInt16(buffer, 0) * 0.00053264;
w[k, 0] = gyr_scale[0] * Math.Pow(w[k, 0], 2) + gyr_scale[1] * w[k, 0] + gyr_scale[2];
w[k, 1] = gyr_scale[3] * Math.Pow(w[k, 1], 2) + gyr_scale[4] * w[k, 1] + gyr_scale[5];
w[k, 2] = gyr_scale[6] * Math.Pow(w[k, 2], 2) + gyr_scale[7] * w[k, 2] + gyr_scale[8];
buffer[0] = pack[17]; buffer[1] = pack[18];
m[k, 0] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
buffer[0] = pack[19]; buffer[1] = pack[20];
m[k, 1] = -((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
buffer[0] = pack[21]; buffer[1] = pack[22];
m[k, 2] = ((double)BitConverter.ToInt16(buffer, 0) * 0.00030518) * mag_scale;
buffer[0] = pack[23]; buffer[1] = pack[24];
q[k, 0] = (double)BitConverter.ToInt16(buffer, 0);
buffer[0] = pack[25]; buffer[1] = pack[26];
q[k, 1] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
buffer[0] = pack[27]; buffer[1] = pack[28];
q[k, 2] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
buffer[0] = pack[29]; buffer[1] = pack[30];
q[k, 3] = (double)BitConverter.ToInt16(buffer, 0) * 0.00003125;
}
counter[k] = k2;
k++;
}
else
tt++;
}
if ((full_file[i + 29] == 3) && (full_file[i + 28] == 16) && (full_file[i] == 16) &&
(full_file[i + 1] == 50)) // условие начала GPS пакета
{
crc = 50;
for (int j = 0; j < 26; j++)
{
pack2[j] = full_file[i + j + 2];
if (j < 25)
crc = crc ^ pack2[j];
}
if (crc == pack2[pack2.Length - 1])
{
ticks2[k2] = BitConverter.ToUInt32(pack2, 0);
buffer2[0] = pack2[4]; buffer2[1] = pack2[5]; buffer2[2] = pack2[6]; buffer2[3] = pack2[7];
lat[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
buffer2[0] = pack2[8]; buffer2[1] = pack2[9]; buffer2[2] = pack2[10]; buffer2[3] = pack2[11];
lon[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 600000;
buffer[0] = pack2[12]; buffer[1] = pack2[13];
speed[k2] = (double)BitConverter.ToInt16(buffer, 0) / 100;
buffer[0] = pack2[14]; buffer[1] = pack2[15];
course[k2] = (double)BitConverter.ToInt16(buffer, 0) / 160;
buffer2[0] = pack2[16]; buffer2[1] = pack2[17]; buffer2[2] = pack2[18]; buffer2[3] = pack2[19];
time[k2] = ((double)BitConverter.ToInt32(buffer2, 0)) / 10;
stat[k2] = pack2[20];
buffer2[0] = pack2[21]; buffer2[1] = pack2[22]; buffer2[2] = pack2[23]; buffer2[3] = pack2[24];
date[k2] = ((double)BitConverter.ToInt32(buffer2, 0));
k2++;
}
}
}
// -------------- Сохранение в IMU/GPS
double[] angles = new double[3];
double[] mw, ma, mm;
ma = new double[3];
mw = new double[3];
mm = new double[3];
FileStream fs_imu = File.Create(file2save + ".imu", 2048, FileOptions.None);
BinaryWriter str_imu = new BinaryWriter(fs_imu);
FileStream fs_gps = File.Create(file2save + ".gps", 2048, FileOptions.None);
BinaryWriter str_gps = new BinaryWriter(fs_gps);
Int16 buf16; Byte buf8; Int32 buf32;
Double bufD; Single bufS; UInt32 bufU32;
addText("\n" + get_time() + " Сохранение файлов:\n" + file2save + ".imu\n" + file2save + ".gps");
int pbar_adj = 50;
Dispatcher.Invoke(new Action(() => progressBar1.Maximum = (k+k2)/pbar_adj + 1));
Dispatcher.Invoke(new Action(() => progressBar1.Value = 0));
double[] magn_c = get_magn_coefs(block_index);
double[] accl_c = get_accl_coefs(block_index);
double[] gyro_c = new double[12];
double[] w_helper = new double[ticks.Length];
// Получение уголов путем интегрирования угловых скоростей (простой вариант)
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 0];
//anglex = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 1];
//angley = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//for (int i = 0; i < w_helper.Length; i++) w_helper[i] = w[i, 2];
//anglez = Signal_processing.Zero_average_corr(w_helper, w_helper.Length);
//-----------------------------------------------------------------------------------------------
// Получение углов путем использования фильтра Калмана (сложный вариант)
MathNet.Numerics.LinearAlgebra.Double.DenseVector Magn_coefs =
new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_magn_coefs(block_index));
MathNet.Numerics.LinearAlgebra.Double.DenseVector Accl_coefs =
new MathNet.Numerics.LinearAlgebra.Double.DenseVector(get_accl_coefs(block_index));
MathNet.Numerics.LinearAlgebra.Double.DenseVector Gyro_coefs = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(12);
Kalman_class.Parameters Parameters = new Kalman_class.Parameters(Accl_coefs, Magn_coefs, Gyro_coefs);
System.Func<int, int, double> filler = (x, y) => 0;
//Kalman_class.Sensors Sensors = new Kalman_class.Sensors(new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0),
// new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0), new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 3, 0));
//MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(1, 4, 0);
Kalman_class.Sensors Sensors = new Kalman_class.Sensors(MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1,3, filler),
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler), MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 3, filler));
MathNet.Numerics.LinearAlgebra.Double.Matrix Initia_quat = MathNet.Numerics.LinearAlgebra.Double.DenseMatrix.Create(1, 4, filler);
Initia_quat[0, 0] = 1;
Kalman_class.State State = new Kalman_class.State(Kalman_class.ACCLERATION_NOISE, Kalman_class.MAGNETIC_FIELD_NOISE, Kalman_class.ANGULAR_VELOCITY_NOISE,
Math.Pow(10, -6), Math.Pow(10, -15), Math.Pow(10, -15), Initia_quat);
Tuple<MathNet.Numerics.LinearAlgebra.Double.Vector, Kalman_class.Sensors, Kalman_class.State> AHRS_result;
//----------------------------------------------------------------------------------------------
System.Diagnostics.Stopwatch timer = new System.Diagnostics.Stopwatch();
timer.Start();
for (int i = 0; i < k; i++)
{
if (i % pbar_adj == 0)
Dispatcher.Invoke(new Action(() => progressBar1.Value++));
if (i == ((int)(k + k2) / 20))
{
long min = (timer.ElapsedMilliseconds * 16) / 1000 / 60;
long sec = (timer.ElapsedMilliseconds * 16 - min * 1000 * 60) / 1000;
addText("\n" + get_time() + " Сохранение файлов займет около\n" +
min + " минут(ы) и " + sec + " секунд(ы).");
}
//------------------------------------------------------------------
// Легкий варинат - интегрирование угловых скоростей
mm = single_correction(magn_c, m[i, 0], m[i, 1], m[i, 2]);
ma = single_correction(accl_c, a[i, 0], a[i, 1], a[i, 2]);
mw = single_correction(gyro_c, w[i, 0], w[i, 1], w[i, 2]);
//angles[0] = (anglez[i]);
//angles[1] = (angley[i]);
//angles[2] = (anglex[i]);
//----------------------------------------------------------------------
// Сложный вариант - фильтр Калмана
for (int j = 0; j < 3; j++)
{
Sensors.a.At(0, j, a[i, j]);
Sensors.w.At(0, j, w[i, j]);
Sensors.m.At(0, j, m[i, j]);
}
//Sensors.a.At(0, 0, a[i, 0]);
//Sensors.a.At(0, 1, a[i, 1]);
//Sensors.a.At(0, 2, a[i, 2]);
//Sensors.w.At(0, 0, w[i, 0]);
//Sensors.w.At(0, 1, w[i, 1]);
//Sensors.w.At(0, 2, w[i, 2]);
//Sensors.m.At(0, 0, m[i, 0]);
//Sensors.m.At(0, 1, m[i, 1]);
//Sensors.m.At(0, 2, m[i, 2]);
AHRS_result = Kalman_class.AHRS_LKF_EULER(Sensors, State, Parameters);
State = AHRS_result.Item3;
//ma[0] = AHRS_result.Item2.a[0, 0];
//ma[1] = AHRS_result.Item2.a[0, 1];
//ma[2] = AHRS_result.Item2.a[0, 2];
//mw[0] = w[i, 0];
//mw[1] = w[i, 1];
//mw[2] = w[i, 2];
//ma[0] = a[i, 0];
//ma[1] = a[i, 1];
//ma[2] = a[i, 2];
//mm[0] = m[i, 0];
//mm[1] = m[i, 1];
//mm[2] = m[i, 2];
angles[0] = (AHRS_result.Item1.At(0));
angles[1] = (AHRS_result.Item1.At(1));
angles[2] = (AHRS_result.Item1.At(2));
//------------------------------------------------------------------------
// IMU
buf16 = (Int16)(angles[0] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[1] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(angles[2] * 10000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mw[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(ma[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[0] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[1] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(mm[2] * 3000);
str_imu.Write(buf16);
buf16 = (Int16)(q[i, 0]);
str_imu.Write(buf16);
buf32 = (Int32)(ticks[i]);
str_imu.Write(buf32);
buf8 = (Byte)(0);
str_imu.Write(buf8);
}
for (int i = 0; i < k2; i++)
{
if (i % pbar_adj == 0)
Dispatcher.Invoke(new Action(() => progressBar1.Value++));
// GPS
bufD = (Double)(lat[i]) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(lon[i]) / ((180 / Math.PI) * 16.66);
str_gps.Write(bufD);
bufD = (Double)(0);
str_gps.Write(bufD);
bufS = (Single)(time[i]);
str_gps.Write(bufS);
bufS = (Single)(speed[i]);
str_gps.Write(bufS);
bufS = (Single)(0);
str_gps.Write(bufS);
str_gps.Write(bufS);
bufU32 = (UInt32)(ticks2[i]);
str_gps.Write(bufU32);
buf8 = (Byte)(0);
str_gps.Write(buf8);
str_gps.Write(buf8);
str_gps.Write(buf8);
}
// Запись даты в конец gps файла
bool inertial = false, sattelite = false;
if (k > 0)
inertial = true;
if (k2 > 0)
{
sattelite = true;
int day = (int)date[k2 - 1] / 10000;
int month = (int)(date[k2 - 1] - day * 10000) / 100;
int year = (int)(2000 + date[k2 - 1] - day * 10000 - month * 100);
string datarec = String.Format("{0:d2}.{1:d2}.{2:d4}", day, month, year);
str_gps.Write(datarec);
}
if (!inertial || !sattelite)
addText("\n" + get_time() + "Неполный комплект данных:\nИнерциальные данные - " + inertial + "\nСпутниковые данные - " + sattelite);
str_imu.Flush();
str_imu.Close();
str_gps.Flush();
str_gps.Close();
Dispatcher.Invoke(new Action(() => progressBar1.Value = (k+k2)/pbar_adj + 1));
//addText("\n" + get_time() + " Сохранение завершено! Всего " + timer.ElapsedMilliseconds / 1000 + " секунд!");
addText("\n" + get_time() + " Сохранение завершено!");
System.Media.SystemSounds.Asterisk.Play();
timer.Stop();
Dispatcher.Invoke(new Action(() => set_stage("stage 2")));
}
private void button4_Click(object sender, EventArgs e)
{
Action fileProc = () =>
{
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
MathNet.Numerics.Control.LinearAlgebraProvider = new MathNet.Numerics.Algorithms.LinearAlgebra.Mkl.MklLinearAlgebraProvider();
MathNet.Numerics.Control.NumberOfParallelWorkerThreads = Environment.ProcessorCount;
addText("Обробка файлу вимірювань...\r\n");
double[][] SGG_data = null;
if (System.IO.File.Exists("sgg_data.bin"))
{
SGG_data = IOFunc.binLoad_SGG_data("sgg_data.bin");
}
else
{
SGG_data = Data.IOFunc.read_SGG_data(SphericalHarmonicAnalyze.Properties.Settings.Default.SGG_measures, new setProgressDel(addVal));
IOFunc.binwrite_SGG_data("sgg_data.bin", SGG_data);
}
addText("Дані вимірювань оброблено: {0} шт.\r\n", SGG_data.Length); Thread.Sleep(500);
ReferenceSystem elipsoid = new ReferenceSystem(ReferenceSystem.Default.TideFree);
elipsoid.gridParameters.cellSize = SphericalHarmonicAnalyze.Properties.Settings.Default.GridCellSize;
elipsoid.gridParameters.coLatitudeBounds = SphericalHarmonicAnalyze.Properties.Settings.Default.minCoLatitude;
elipsoid.maxDegree = SphericalHarmonicAnalyze.Properties.Settings.Default.modelMaxOrder;
int greedColumnsCount, greedRowsCount;
List <double[]> greed = MathFunc.generateGrid(elipsoid.gridParameters.cellSize, out greedColumnsCount, out greedRowsCount, elipsoid.gridParameters.coLatitudeBounds, 180 - elipsoid.gridParameters.coLatitudeBounds);
addText("Сітку згенеровано: {0} комірок \r\n", greed.Count);
double avgR = MathFunc.getAvgRadius(SGG_data);
List <int>[] map = MathFunc.getMappingOfPoints(elipsoid, SGG_data, greed.ToArray(), greedRowsCount, greedColumnsCount, avgR); sw.Stop(); addText("Точки віднесено до комірок сітки за: {0}.\r\n", sw.Elapsed.ToString());
addText("Кількість клітинок сітки всього: {0}\r\n", greed.Count);
int res1 = 0; foreach (var item in map)
{
res1 += item.Count;
}
addText("Використано вимірів: {0}\r\nСер радіус: {1}\r\n", res1, avgR);
test.checkMap(SGG_data, map, greed, elipsoid);
List <int>[] newMap = null;
MathFunc.checkGreed(ref greed, map, out newMap);
addText("Кількість клітинок сітки, в яких присутні дані вимірювань: {0}\r\n", greed.Count);
map = newMap; newMap = null;
IOFunc.writeGreedToCsvFileWithMeasureCount(greed, map, "greed_new_map.txt");
double[] avgRadius; sw.Restart();
double[] regularisedValues = MathFunc.regularization(SGG_data, greed.ToArray(), map, out avgRadius); sw.Stop(); addText("Регуляризація (на основі сферичної відстані) виконана за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeGreedToCsvFileWithMeasureS(greed, regularisedValues, "greed_regular_grad.txt");
avgRadius[0] = Math.Round(avgRadius[0]);
elipsoid.satelliteSphere = avgRadius[0];
addText("Середній радіус: {0,10:0.000}.\r\nМінімальний радіус: {1,10:0.0000}\r\nМаксимальний радіус:{2,10:0.0000}\r\n", avgRadius[0], avgRadius[1], avgRadius[2]);
SGG_data = null; map = null;
int[][] t_nm = MathFunc.get_nm(elipsoid.maxDegree);
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dm = new MathNet.Numerics.LinearAlgebra.Double.DenseMatrix(greed.Count, (MathFunc.getArraySize(elipsoid.maxDegree) - 3) * 2 - (elipsoid.maxDegree - 1));
sw.Stop(); addText("Пам'ять для матриці коефіцієнтів виділено за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
int progress = 0;
//Обчислення елементів матриці
var p = Parallel.For(0, dm.RowCount, (i) =>
{
double[] line = MathFunc.getCoefMatrixLineKoop(elipsoid, elipsoid.maxDegree, t_nm, elipsoid.satelliteSphere, greed[i][0], greed[i][1]);
lock (dm)
{
dm.SetRow(i, line);
}
progress++;
if (progress / 100D == Math.Round(progress / 100D))
{
addVal(progress, dm.RowCount, "Визначено");
}
});
if (!p.IsCompleted)
{
throw new Exception("Parallel.For");
}
;
IOFunc.writeMatrixToMatLabFile(dm, @"matlab\A.mat", "A");
sw.Stop();
richTextBox1.Invoke(new setProgressDel(addVal), new object[] { 0, dm.RowCount, "" });
addText("Матриця {0} на {1} ({2}MB) згенерована за: {3,10}\r\n", dm.RowCount, dm.ColumnCount, dm.ColumnCount * dm.RowCount * 8 / 1000000, sw.Elapsed.ToString() /* + "\r\nЗапис у файл...\r\n"*/);
if (true)
{
GravityModel gm08 = new GravityModel(elipsoid.maxDegree);
gm08.loadFromFile("GO_CONS_EGM_GCF_2.gfc", new setProgressDel(addVal));
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm08.getGradientForGrid(elipsoid, greed));//regularisedValues);
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmL2;
GravityModel gm = new GravityModel(elipsoid.maxDegree);
if (radioButton1.Checked)
{
sw.Restart();
gm.loadFromFile(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel, new setProgressDel(addVal));
sw.Stop(); addText("Вихідна модель завантажена за: {0}.\r\n", sw.Elapsed.ToString());
sw.Restart();
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
sw.Stop(); addText("Градієнти за вихідною моделлю обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
else
{
sw.Restart();
gm = GravityModel.getNormalModel(elipsoid, elipsoid.maxDegree);
dmL2 = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(gm.getGradientForGrid(elipsoid, greed));
sw.Stop(); addText("Нормальні градієнти обчислені для сітки за: {0}.\r\n", sw.Elapsed.ToString());
}
dmL = dmL - dmL2;
dmL2 = null;
IOFunc.writeMatrixToMatLabFile(dmL.ToColumnMatrix(), @"matlab\L.mat", "L");
sw.Restart();
MathNet.Numerics.LinearAlgebra.Double.DenseVector dmLNormal = null;
dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dm.TransposeThisAndMultiply(dmL);
dmL = null;
IOFunc.writeMatrixToMatLabFile(dmLNormal.ToColumnMatrix(), @"matlab\LNorm.mat", "LNorm");
sw.Stop(); addText("Стовпчик вільних членів обчислений за: {0}.\r\n", sw.Elapsed.ToString());
MathNet.Numerics.LinearAlgebra.Double.DenseMatrix dmANorm = null;
sw.Restart();
dmANorm = (MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.TransposeThisAndMultiply(dm); dm = null;
sw.Stop(); addText("Нормальна матриця коефіціэнтів обчислена за: {0}.\r\n", sw.Elapsed.ToString());
IOFunc.writeMatrixToMatLabFile(dmANorm, @"matlab\ANorm.mat", "ANorm");
//dmLNormal = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)dmLNormal.Multiply(5e-8);
var x = dmANorm.Inverse();
var res = (MathNet.Numerics.LinearAlgebra.Double.DenseVector)x.Multiply(dmLNormal);
IOFunc.writeModeVectorlToTxtFile(res, elipsoid, @"matlab\_out.AL");
addText(@"Результат за методом A\L знайдено...");
x = null;
GravityModel gm_R = new GravityModel(gm);
gm_R.addDeltaCoef(res.ToArray()); res = null;
double[] h = GravityModel.getGeoidHeight(elipsoid, gm_R, greed);
double[] dg = GravityModel.getAnomaly(elipsoid, gm_R, greed);
IOFunc.writeGeoidHeightsAndAnomalysToTxt(greed, h, dg, elipsoid, @"output\result_AL.txt");
IOFunc.writeGravityModelToTxtFile(gm_R, @"output\model_AL.gcf");
sw.Restart();
addText(dmANorm.Rank().ToString() + "\r\n");
dmANorm = null;
dmLNormal = null;
sw.Stop(); addText("Невідомі знайдено за: {0}.\r\n", sw.Elapsed.ToString());
}
};
if (System.IO.File.Exists(SphericalHarmonicAnalyze.Properties.Settings.Default.inGravityModel))
{
tabControl1.SelectedTab = tabControl1.TabPages[1];
this.UseWaitCursor = true;
ts = new CancellationTokenSource();
ct = ts.Token;
tsk = Task.Factory.StartNew(fileProc, ct);
var setCur = Task.Factory.StartNew(() => { tsk.Wait(); this.UseWaitCursor = false; addText("Обчислення завершені!"); });
richTextBox1.SaveFile(@"output\zvit.rtf");
}
}
public static Vector3 ToVector3(this VectorD vector)
{
return(new Vector3((float)vector[0], (float)vector[1], (float)vector[2]));
}
/// <summary>
/// Returns unityvector holding the direction
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
public static XYPoint GetDirection(IXYPoint p1, IXYPoint p2)
{
MathNet.Numerics.LinearAlgebra.Double.DenseVector v = new MathNet.Numerics.LinearAlgebra.Double.DenseVector(2);
v[0] = p2.X - p1.X;
v[1] = p2.Y - p1.Y;
v.Normalize(1);
return new XYPoint(v[0], v[1]);
}
public static void writeModeVectorlToTxtFile(MathNet.Numerics.LinearAlgebra.Double.DenseVector dm, ReferenceSystem rs, string file)
{
System.IO.File.WriteAllText(file, matrixToString((MathNet.Numerics.LinearAlgebra.Double.DenseMatrix)dm.ToColumnMatrix(), rs), System.Text.Encoding.Default);
}