protected Position3D Localize2(int[] samplemaxes, ASIOLatencies lat)
{
Complex d1 = 0, d2 = 0, d3 = 0;
//Working vars
Position3D new_loc, loc = new Position3D();
int new_points = 0, points = 0;
double new_points_std = 0, points_std = 0;
for (int k = 0; k < 4; k++)
{
Complex C;
if (samplemaxes.Length >= 4)
{
if (k == 1)
{
d1 = samplemaxes[0] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[1] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[3] * c / ASIO.Fs; // X-direction
}
else if (k == 2)
{
d1 = samplemaxes[2] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[3] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[1] * c / ASIO.Fs; // X-direction
}
else if (k == 3)
{
d1 = samplemaxes[1] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[0] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[2] * c / ASIO.Fs; // X-direction
}
else if (k == 4)
{
d1 = samplemaxes[3] * c / ASIO.Fs; // Base point (0,0)
d2 = samplemaxes[2] * c / ASIO.Fs; // Y-direction
d3 = samplemaxes[0] * c / ASIO.Fs; // X-direction
}
// !This is where the magic happens!: calculate the time difference between the transmission of the audio and the start time of the sampling (absolute time at sample index 0), this was found using the Matlab equation solver
C =
(
(fieldWidth * Complex.Pow(d1, 3) - Complex.Pow(fieldWidth, 3) * d1 + Complex.Pow(fieldWidth, 3) * d2 - fieldHeight * Complex.Pow(d1, 3) + 2 * d1 *
Complex.Sqrt(
((fieldWidth + d1 - d3) * (fieldWidth - d1 + d3) * (Complex.Pow(fieldWidth, 4) - 2 * Complex.Pow(fieldWidth, 3) * d1 + 2 * Complex.Pow(fieldWidth, 3) * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(fieldHeight, 2) + 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - 2 * Complex.Pow(fieldWidth, 2) * d1 * d3 - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) + 2 * Complex.Pow(fieldWidth, 2) * d2 * d3 - 2 * fieldWidth * fieldHeight * Complex.Pow(d1, 2) + 4 * fieldWidth * fieldHeight * d1 * d2 - 2 * fieldWidth * fieldHeight * Complex.Pow(d2, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d2 + 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + 4 * fieldWidth * d1 * Complex.Pow(d2, 2) - 4 * fieldWidth * d1 * d2 * d3 - 2 * fieldWidth * Complex.Pow(d2, 3) + 2 * fieldWidth * Complex.Pow(d2, 2) * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d1, 2) + 2 * Complex.Pow(fieldHeight, 2) * d1 * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d2 - 2 * fieldHeight * Complex.Pow(d1, 2) * d3 - 4 * fieldHeight * d1 * Complex.Pow(d2, 2) + 4 * fieldHeight * d1 * d2 * d3 + 2 * fieldHeight * Complex.Pow(d2, 3) - 2 * fieldHeight * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * d2 * d3 + Complex.Pow(d1, 2) * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 3) + 4 * d1 * Complex.Pow(d2, 2) * d3 - 2 * d1 * d2 * Complex.Pow(d3, 2) + Complex.Pow(d2, 4) - 2 * Complex.Pow(d2, 3) * d3 + Complex.Pow(d2, 2) * Complex.Pow(d3, 2))) / 4
)
-
2 * d2 *
Complex.Sqrt(
(
(fieldWidth + d1 - d3) * (fieldWidth - d1 + d3) * (Complex.Pow(fieldWidth, 4) - 2 * Complex.Pow(fieldWidth, 3) * d1 + 2 * Complex.Pow(fieldWidth, 3) * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(fieldHeight, 2) + 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - 2 * Complex.Pow(fieldWidth, 2) * fieldHeight * d2 + Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - 2 * Complex.Pow(fieldWidth, 2) * d1 * d3 - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) + 2 * Complex.Pow(fieldWidth, 2) * d2 * d3 - 2 * fieldWidth * fieldHeight * Complex.Pow(d1, 2) + 4 * fieldWidth * fieldHeight * d1 * d2 - 2 * fieldWidth * fieldHeight * Complex.Pow(d2, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d2 + 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + 4 * fieldWidth * d1 * Complex.Pow(d2, 2) - 4 * fieldWidth * d1 * d2 * d3 - 2 * fieldWidth * Complex.Pow(d2, 3) + 2 * fieldWidth * Complex.Pow(d2, 2) * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d1, 2) + 2 * Complex.Pow(fieldHeight, 2) * d1 * d3 - Complex.Pow(fieldHeight, 2) * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d2 - 2 * fieldHeight * Complex.Pow(d1, 2) * d3 - 4 * fieldHeight * d1 * Complex.Pow(d2, 2) + 4 * fieldHeight * d1 * d2 * d3 + 2 * fieldHeight * Complex.Pow(d2, 3) - 2 * fieldHeight * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * d2 * d3 + Complex.Pow(d1, 2) * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 3) + 4 * d1 * Complex.Pow(d2, 2) * d3 - 2 * d1 * d2 * Complex.Pow(d3, 2) + Complex.Pow(d2, 4) - 2 * Complex.Pow(d2, 3) * d3 + Complex.Pow(d2, 2) * Complex.Pow(d3, 2))
)
/
4
)
- Complex.Pow(d1, 3) * d2 + d1 * Complex.Pow(d3, 3) + Complex.Pow(d1, 3) * d3 - d2 * Complex.Pow(d3, 3) + Complex.Pow(fieldWidth, 4) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d1, 2) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d2, 2) - Complex.Pow(fieldWidth, 2) * Complex.Pow(d3, 2) + Complex.Pow(d1, 2) * Complex.Pow(d2, 2) - 2 * Complex.Pow(d1, 2) * Complex.Pow(d3, 2) + Complex.Pow(d2, 2) * Complex.Pow(d3, 2) + Complex.Pow(fieldWidth, 2) * fieldHeight * d1 - Complex.Pow(fieldWidth, 2) * fieldHeight * d2 - fieldWidth * Complex.Pow(d1, 2) * d2 + Complex.Pow(fieldWidth, 2) * d1 * d2 + fieldWidth * d1 * Complex.Pow(d3, 2) - 2 * fieldWidth * Complex.Pow(d1, 2) * d3 + Complex.Pow(fieldWidth, 2) * d1 * d3 - fieldWidth * d2 * Complex.Pow(d3, 2) + Complex.Pow(fieldWidth, 2) * d2 * d3 + fieldHeight * Complex.Pow(d1, 2) * d2 - fieldHeight * d1 * Complex.Pow(d3, 2) + 2 * fieldHeight * Complex.Pow(d1, 2) * d3 + fieldHeight * d2 * Complex.Pow(d3, 2) + d1 * d2 * Complex.Pow(d3, 2) - 2 * d1 * Complex.Pow(d2, 2) * d3 + Complex.Pow(d1, 2) * d2 * d3 + 2 * fieldWidth * d1 * d2 * d3 - 2 * fieldHeight * d1 * d2 * d3) / (Complex.Pow(fieldWidth, 2) - 2 * Complex.Pow(d1, 2) + 2 * d1 * d2 + 2 * d1 * d3 - Complex.Pow(d2, 2) - Complex.Pow(d3, 2)) - Complex.Pow(fieldWidth, 2) - Complex.Pow(d1, 2) + Complex.Pow(d2, 2)
)
/
(
2 * d1 - 2 * d2
);
}
else
{
C = 0;
}
//Make it a time.
C = C / c;
double[] t = new double[samplemaxes.Length];
for (int i = 0; i < t.Length; i++)
{
t[i] = lat.Input / ASIO.Fs + C.Real + samplemaxes[i] / ASIO.Fs;
}
Laterate(t, beaconHeight, c, out new_loc, out new_points, out new_points_std);
if (new_points > points || (new_points == points && new_points_std < points_std))
{
loc = new_loc;
points = new_points;
points_std = new_points_std;
}
}
return loc;
}
public LocationUpdatedEventArgs(Position3D _pos)
{
Position = _pos;
}
/// <summary>
///
/// </summary>
/// <param name="samplemaxes">Sample numbers of all microphones (one per microphone)</param>
/// <param name="lat">Latencies which are retrieved from the ASIO driver</param>
/// <returns></returns>
protected Position3D Localize(int[] samplemaxes, ASIOLatencies lat)
{
//Working vars
Position3D loc = new Position3D();
int nmics = Math.Min(Microphones.Count, asio.InChannels);
int row = 0;
if (nmics * (nmics - 1) < nmics + 1) {
Debug.WriteLine("Inconclusive.");
return null;
}
Matrix<double> a = DenseMatrix.Create(nmics * (nmics - 1), nmics + 1, 0);
Vector<double> b = DenseVector.Create(nmics * (nmics - 1), 0);
for (int i = 0; i < nmics; i++)
{
for (int j = 0; j < nmics; j++)
{
if (j <= i)
continue;
a[row, 0] = 2 * (Microphones[j].Position.X - Microphones[i].Position.X);
a[row, 1] = 2 * (Microphones[j].Position.Y - Microphones[i].Position.Y);
a[row, 1 + j] = -2 * (samplemaxes[i] - samplemaxes[j]) / ASIO.Fs * c;
b[row] = Math.Pow(((samplemaxes[i] - samplemaxes[j]) / ASIO.Fs * c), 2) - Math.Pow(Position3D.GetMagnitude(Microphones[i].Position), 2) + Math.Pow(Position3D.GetMagnitude(Microphones[j].Position), 2);
row++;
}
}
Vector<double> y = a.Solve(b);
loc = new Position3D() { X = y[0], Y = y[1], Z = 0 };
if (!Double.IsNaN(loc.X) && !Double.IsNaN(loc.Y))
{
return loc;
}
else
{
Debug.WriteLine("Inconclusive (NaN).");
return null;
}
}
protected void Laterate(double[] t, double h, double c, out Position3D loc, out int points, out double points_std)
{
/*loc = new Position3D() { X = 0, Y = 0, Z = beaconHeight };
points = 5;
points_std = 0;*/
List<Position3D> y = new List<Position3D>();
int passes = Math.Min(Microphones.Count, t.Length);
for (int i = 0; i < passes; i++)
{
for (int j = 0; j < passes; j++)
{
if (i == j || i < j)
{
continue; // Do not compare with own time or twice thesame
}
double d = Math.Sqrt(Math.Pow(Microphones[i].Position.X - Microphones[j].Position.X, 2) + Math.Pow(Microphones[i].Position.Y - Microphones[j].Position.Y, 2));
double h1 = Microphones[i].Position.X - h;
double h2 = Microphones[j].Position.X - h;
double t1_ = (Complex.Sqrt(Math.Pow(t[i] * c, 2) - Math.Pow(h1, 2))).Real / c;
double t2_ = (Complex.Sqrt(Math.Pow(t[j] * c, 2) - Math.Pow(h2, 2))).Real / c;
double d1 = (Math.Pow(t1_ * c, 2) - Math.Pow(t2_ * c, 2) + Math.Pow(d, 2)) / (2 * d);
double x = (Complex.Sqrt((Math.Pow(t1_ * c, 2) - Math.Pow(d1, 2)))).Real;
double mx = d1 / d * (Microphones[j].Position.X - Microphones[i].Position.X);
double my = d1 / d * (Microphones[j].Position.Y - Microphones[i].Position.Y);
Vector<double> r = DenseVector.OfArray(new double[] { Math.Sqrt(Math.Pow(mx, 2) + Math.Pow(my, 2)), x });
double angle = -Math.Atan2(my, mx);
Matrix<double> rot = DenseMatrix.OfArray(new double[,] { { Math.Cos(angle), -Math.Sin(angle) }, { Math.Sin(angle), Math.Cos(angle) } });
Vector<double> disp = DenseVector.OfArray(new double[] { Microphones[i].Position.X, Microphones[i].Position.Y });
Vector<double> coord = r * rot + disp;
y.Add(new Position3D() { X = coord[0], Y = coord[1], Z = h });
}
}
y = Filter(y);
y = RemoveOutliers(y); // Remove the mirror images and other outliers due to input data errors
loc = GetAveragePosition(y);
points = y.Count;
points_std = y.Select(o => o.X).StdDev() + y.Select(o => o.Y).StdDev();
}
public static double GetMagnitude(Position3D x)
{
return x.Magnitude;
}
void localizer_OnLocationUpdated(object sender, LocationUpdatedEventArgs e)
{
try
{
App.Current.Dispatcher.Invoke((Action)delegate
{
if (localizer != null)
{
if (localizer.lastData != null)
{
List<string> legend = new List<string>();
for (int i = 0; i < localizer.lastData.ColumnCount; i++)
{
legend.Add(String.Format("Channel {0}", i + 1));
}
if (pw != null)
{
if (pw.IsLoaded)
pw.Update("Data", localizer.lastData, legend, ASIO.T, localizer.lastMaxes);
}
else
{
pw = new PlotWindow("Data", localizer.lastData, legend, ASIO.T, localizer.lastMaxes);
pw.Show();
}
}
}
//Debug.WriteLine(e.Position);
Position3D pos = new Position3D() { X = (lpos.X * 2 + e.Position.X) / 3, Y = (lpos.Y * 2 + e.Position.Y) / 3, Z = 0 };
posistionLog.AppendText(pos.ToString() + "\n");
lpos = e.Position;
posistionLog.ScrollToEnd();
});
}
catch
{
//nothing
}
}
public Microphone()
{
Position = new Position3D();
ChannelIndex = 0;
}
