void InitializeRobotState(double x, double y, double theta)
{
X_rob_predict = Vector <double> .Build.DenseOfArray(new double[] { 0, 0, 0 });
X_rob_actual = X_rob_predict.Clone();
observedLandmarks = ObserveLandmarks(X_rob_actual, rangeNoiseFactor, bearingNoiseFactor);
BuildGraph(observedLandmarks, landmarkTrace, ref graph);
robotTrace_predict.Add(X_rob_predict.Clone());
robotTrace_actual.Add(X_rob_actual.Clone());
landmarkTrace.Add(observedLandmarks);
graphVisualizer.VisualizerRobotRegistration((float)x, (float)y, (float)theta);
}
