Пример #1
0
        public void Setup()
        {
            Config.SetLinear2DDefaults();

            LinearPose2D     pose      = new LinearPose2D();
            List <double[]>  landmarks = new List <double[]>();
            Linear2DMeasurer measurer  = new Linear2DMeasurer(6.5);

            pose.X = 1;
            pose.Y = 2;

            landmarks.Add(new double[3] {
                2, 2, 0
            });
            landmarks.Add(new double[3] {
                3, 4, 0
            });
            landmarks.Add(new double[3] {
                3, 3.5, 0
            });

            vehicle   = new SimulatedVehicle(pose, landmarks, measurer);
            navigator = new PHDNavigator(vehicle, 1, true);
        }
Пример #2
0
        public void LogLike2D()
        {
            LinearPose2D      realpose = new LinearPose2D();
            SimulatedVehicle2 estimate = new SimulatedVehicle2();
            Map map          = new Map(3);
            var measurements = new List <LinearMeasurement2D>();

            measurements.Add(new LinearMeasurement2D(0, 1));
            measurements.Add(new LinearMeasurement2D(0.2, 0.6));

            map.Add(new Gaussian(new double[3] {
                0, 1.45, 0
            }, identitycov, 1.0));
            map.Add(new Gaussian(new double[3] {
                0, 0.65, 0
            }, identitycov, 1.0));
            map.Add(new Gaussian(new double[3] {
                1.0, 0, 0
            }, identitycov, 1.0));

            int n = 201;

            double[]     x        = new double[n];
            double[]     y        = new double[n];
            double[][]   loglike  = new double[n][];
            double[][][] dloglike = new double[n][][];

            for (int i = 0; i < x.Length; i++)
            {
                x[i] = ((double)i / (n - 1) - 0.5) / 0.5;
            }

            for (int k = 0; k < x.Length; k++)
            {
                y[k] = ((double)k / (n - 1) - 0.5) / 0.5;
            }

            for (int i = 0; i < x.Length; i++)
            {
                loglike[i]  = new double[n];
                dloglike[i] = new double[n][];
                for (int k = 0; k < y.Length; k++)
                {
                    double[] ds = new double[2];
                    dloglike[i][k] = new double[2];
                    ds[0]          = x[i];
                    ds[1]          = y[k];

                    estimate.Pose = realpose.Add(ds);
                    loglike[i][k] = PHDNavigator2.QuasiSetLogLikelihood(measurements, map, estimate, out dloglike[i][k]);
                }
            }

            string xstr      = string.Join(" ", x.Convert(i => i.ToString("e12")));
            string ystr      = string.Join(" ", y.Convert(i => i.ToString("e12")));
            string likestr   = string.Join("\n", loglike.Convert(i => string.Join(" ", i.Convert(k => k.ToString("e12")))));
            string dlike1str = string.Join("\n", dloglike.Convert(i => string.Join(" ", i.Convert(k => k[0].ToString("e12")))));
            string dlike2str = string.Join("\n", dloglike.Convert(i => string.Join(" ", i.Convert(k => k[1].ToString("e12")))));

            string root = Environment.GetFolderPath(Environment.SpecialFolder.Personal);

            System.IO.File.WriteAllText(System.IO.Path.Combine(root, "x.data"), xstr);
            System.IO.File.WriteAllText(System.IO.Path.Combine(root, "y.data"), ystr);
            System.IO.File.WriteAllText(System.IO.Path.Combine(root, "like.data"), likestr);
            System.IO.File.WriteAllText(System.IO.Path.Combine(root, "dlike1.data"), dlike1str);
            System.IO.File.WriteAllText(System.IO.Path.Combine(root, "dlike2.data"), dlike2str);

            for (int i = 1; i < x.Length - 1; i++)
            {
                for (int k = 1; k < y.Length - 1; k++)
                {
                    double xnumerical = (loglike[i + 1][k] - loglike[i - 1][k]) / (x[i + 1] - x[i - 1]);
                    double ynumerical = (loglike[i][k + 1] - loglike[i][k - 1]) / (y[k + 1] - y[k - 1]);

                    Console.WriteLine(xnumerical + " " + dloglike[i][k][0] + " " + ynumerical + " " + dloglike[i][k][1]);

                    Assert.IsTrue(xnumerical.IsEqual(dloglike[i][k][0], 0.5));
                    Assert.IsTrue(ynumerical.IsEqual(dloglike[i][k][1], 0.5));
                }
            }
        }