Esempio n. 1
0
        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;
            }
        }
Esempio n. 2
0
        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;
        }
Esempio n. 3
0
        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);
        }