public static void Main(string[] args) { PosAndDistance beacon1 = new PosAndDistance(1.0, 1.0, 1.0, 1.0); PosAndDistance beacon2 = new PosAndDistance(3.0, 1.0, 2.0, 1.0); PosAndDistance beacon3 = new PosAndDistance(2.0, 2.0, 1.0, 2.0); PosAndDistance beacon4 = new PosAndDistance(4.0, 3.0, 2.0, 3.0); Beacon beacons = new Beacon(4); beacons.AddBeacon(beacon1); beacons.AddBeacon(beacon2); beacons.AddBeacon(beacon3); beacons.AddBeacon(beacon4); Trilateration tr = new Trilateration(); tr.Trilaterationfunction(beacons); }
private void JacobiMethod(Beacon beacons, double[,] m, double[] b, out double x, out double y, out double z) { double x1 = beacons.xpos(0); double y1 = beacons.ypos(0); double z1 = beacons.zpos(0); double[,] mt = new double[m.GetLength(1), m.GetLength(0)]; double[,] multiplied = new double[mt.GetLength(0), m.GetLength(1)]; mt = transpose(m); multiplied = multiply(mt, m); double det = determinant(multiplied); double[,] A = inverse(multiplied, det); double[,] d = multiply(A, mt); double[] q = multbyvector(d, b); x = q[0] + x1; y = q[1] + y1; z = q[2] + z1; }
public double[] test(Beacon beacons, double[] coor, int rows) { double[] dest = new double[beacons.size()]; double[] res = new double[beacons.size()]; Beacon beaconr = beacons; int c = 0; while (c != 100) { for (int i = 0; i < beacons.size(); i++) { dest[i] = Math.Sqrt(Math.Pow(beaconr.xpos(i) - coor[0], 2) + Math.Pow(beaconr.ypos(i) - coor[1], 2) + Math.Pow(beaconr.zpos(i) - coor[2], 2)); res[i] = Math.Abs(beacons.rpos(i) - dest[i]); } if (res[0] <= 1E-10 && res[1] <= 1E-10 & res[2] <= 1E-10) { c = 99; } else { for (int i = 0; i < beaconr.size(); i++) { double[,] m = new double[rows, 3]; double x, y, z; double[] b = new double[rows]; beaconr.to(i, beaconr.xpos(i), beaconr.ypos(i), beaconr.zpos(i), newr(beaconr.rpos(i), dest[i])); getMatrixAndVector(beaconr, rows, out m, out b); JacobiMethod(beaconr, m, b, out x, out y, out z); coor[0] = x; coor[1] = y; coor[2] = z; } Console.WriteLine(c + ": " + res[0] + " " + res[1] + " " + res[2] + " " + res[3]); } c++; } Console.WriteLine("Final: " + res[0] + " " + res[1] + " " + res[2] + " " + res[3]); return(coor); }
private void getMatrixAndVector(Beacon beacons, int rows, out double[,] m, out double[] b) { m = new double[rows, 3]; b = new double[rows]; double x1 = beacons.xpos(0); double y1 = beacons.ypos(0); double z1 = beacons.zpos(0); double r1 = beacons.rpos(0); for (int i = 1; i < beacons.size(); i++) { double x2 = beacons.xpos(i); double y2 = beacons.ypos(i); double z2 = beacons.zpos(i); double r2 = beacons.rpos(i); m[i - 1, 0] = x2 - x1; m[i - 1, 1] = y2 - y1; m[i - 1, 2] = z2 - z1; b[i - 1] = (Math.Pow(r1, 2) - Math.Pow(r2, 2) + Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2) + Math.Pow(z2 - z1, 2)) / 2; } }
public bool Trilaterationfunction(Beacon beacons) { if (beacons.size() < 4) { return(false); } int rows = 3; double[,] m = new double[rows, 3]; double[] b = new double[rows]; double rx, ry, rz; getMatrixAndVector(beacons, rows, out m, out b); JacobiMethod(beacons, m, b, out rx, out ry, out rz); Console.WriteLine("First result: (" + rx + "; " + ry + "; " + rz + ")"); double[] coor = new double[3] { rx, ry, rz }; test(beacons, coor, rows); Console.WriteLine("Second result: (" + coor[0] + "; " + coor[1] + "; " + coor[2] + ")"); return(true); }