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);
}
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));
}
}
}
