private void init()
{
DataReader dr = new SyntheticData();
CellDescription cellData = dr.FetchData();
travellingDistance = cellData.TravellingTimeArm1; //LUDDE: Why is this set to Arm1's Travelling arrivalTime?
// number of vehicles we have
nbRoutes = 3;
// number of visists we have to make
nbNodes = 58 + nbRoutes * 2;
List <int> initStarts = new List <int>();
List <int> initEnds = new List <int>();
// ENDS AND STARTS NEED TO BE IN THE BEGINNING!
for (int i = 0; i < nbRoutes; i++)
{
initStarts.Add(i);
initEnds.Add(i + nbRoutes);
}
starts = initStarts.ToArray();
ends = initEnds.ToArray();
routing = new RoutingModel(nbNodes, nbRoutes, starts, ends);
}
private void InitializeKalman(float strengthTMatrix, double processNoise, double measurementNoise)
{
_syntheticData = new SyntheticData(strengthTMatrix, processNoise, measurementNoise);
_kalman = new Emgu.CV.Kalman(_syntheticData.State, _syntheticData.TransitionMatrix, _syntheticData.MeasurementMatrix, _syntheticData.ProcessNoise, _syntheticData.MeasurementNoise)
{
ErrorCovariancePost = _syntheticData.ErrorCovariancePost
};
}
public KalmanFilter()
{
dataPoints = new List<PointF>();
kalmanPoints = new List<PointF>();
kalFilter = new Kalman(2, 2, 0);
syntheticData = new SyntheticData();
Matrix<float> state = new Matrix<float>(new float[]
{
0.0f, 0.0f
});
kalFilter.CorrectedState = state;
kalFilter.TransitionMatrix = syntheticData.transitionMatrix;
kalFilter.MeasurementNoiseCovariance = syntheticData.measurementNoise;
kalFilter.ProcessNoiseCovariance = syntheticData.processNoise;
kalFilter.ErrorCovariancePost = syntheticData.errorCovariancePost;
kalFilter.MeasurementMatrix = syntheticData.measurementMatrix;
}
//[Test]
public void TestKalman()
{
Image<Bgr, Byte> img = new Image<Bgr, byte>(400, 400);
SyntheticData syntheticData = new SyntheticData();
//Matrix<float> state = new Matrix<float>(new float[] { 0.0f, 0.0f}); //initial guess
#region initialize Kalman filter
KalmanFilter tracker = new KalmanFilter(2, 1, 0);
syntheticData.TransitionMatrix.Mat.CopyTo(tracker.TransitionMatrix);
syntheticData.MeasurementMatrix.Mat.CopyTo(tracker.MeasurementMatrix);
syntheticData.ProcessNoise.Mat.CopyTo(tracker.ProcessNoiseCov);
syntheticData.MeasurementNoise.Mat.CopyTo(tracker.MeasurementNoiseCov);
syntheticData.ErrorCovariancePost.Mat.CopyTo(tracker.ErrorCovPost);
tracker.StatePost.SetTo(new float[] { 0.0f, 0.0f });
#endregion
System.Converter<double, PointF> angleToPoint =
delegate(double radianAngle)
{
return new PointF(
(float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
(float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle)));
};
Action<PointF, Bgr> drawCross =
delegate(PointF point, Bgr color)
{
img.Draw(new Cross2DF(point, 15, 15), color, 1);
};
ImageViewer viewer = new ImageViewer();
System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
timer.Interval = 200;
timer.Tick += new EventHandler(delegate(object sender, EventArgs e)
{
Matrix<float> measurement = syntheticData.GetMeasurement();
// adjust Kalman filter state
tracker.Correct(measurement.Mat);
tracker.Predict();
#region draw the state, prediction and the measurement
float[] correctedState = new float[2];
float[] predictedState = new float[2];
tracker.StatePost.CopyTo(correctedState);
tracker.StatePre.CopyTo(predictedState);
PointF statePoint = angleToPoint(correctedState[0]);
PointF predictPoint = angleToPoint(predictedState[0]);
PointF measurementPoint = angleToPoint(measurement[0, 0]);
img.SetZero(); //clear the image
drawCross(statePoint, new Bgr(Color.White)); //draw current state in White
drawCross(measurementPoint, new Bgr(Color.Red)); //draw the measurement in Red
drawCross(predictPoint, new Bgr(Color.Green)); //draw the prediction (the next state) in green
img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position
//Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
#endregion
syntheticData.GoToNextState();
viewer.Image = img;
});
timer.Start();
viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
viewer.Text = "Actual State: White; Measurement: Red; Prediction: Green";
viewer.ShowDialog();
}
public void TestKalman()
{
Image <Bgr, Byte> img = new Image <Bgr, byte>(400, 400);
SyntheticData syntheticData = new SyntheticData();
// state is (phi, delta_phi) - angle and angle increment
Matrix <float> state = new Matrix <float>(new float[] { 0.0f, 0.0f }); //initial guess
#region initialize Kalman filter
Kalman tracker = new Kalman(2, 1, 0);
tracker.TransitionMatrix = syntheticData.TransitionMatrix;
tracker.MeasurementMatrix = syntheticData.MeasurementMatrix;
tracker.ProcessNoiseCovariance = syntheticData.ProcessNoise;
tracker.MeasurementNoiseCovariance = syntheticData.MeasurementNoise;
tracker.ErrorCovariancePost = syntheticData.ErrorCovariancePost;
tracker.CorrectedState = state;
#endregion
System.Converter <double, PointF> angleToPoint =
delegate(double radianAngle)
{
return(new PointF(
(float)(img.Width / 2 + img.Width / 3 * Math.Cos(radianAngle)),
(float)(img.Height / 2 - img.Width / 3 * Math.Sin(radianAngle))));
};
Action <PointF, Bgr> drawCross =
delegate(PointF point, Bgr color)
{
img.Draw(new Cross2DF(point, 15, 15), color, 1);
};
ImageViewer viewer = new ImageViewer();
System.Windows.Forms.Timer timer = new System.Windows.Forms.Timer();
timer.Interval = 200;
timer.Tick += new EventHandler(delegate(object sender, EventArgs e)
{
Matrix <float> measurement = syntheticData.GetMeasurement();
// adjust Kalman filter state
tracker.Correct(measurement);
tracker.Predict();
#region draw the state, prediction and the measurement
PointF statePoint = angleToPoint(tracker.CorrectedState[0, 0]);
PointF predictPoint = angleToPoint(tracker.PredictedState[0, 0]);
PointF measurementPoint = angleToPoint(measurement[0, 0]);
img.SetZero(); //clear the image
drawCross(statePoint, new Bgr(Color.White)); //draw current state in White
drawCross(measurementPoint, new Bgr(Color.Red)); //draw the measurement in Red
drawCross(predictPoint, new Bgr(Color.Green)); //draw the prediction (the next state) in green
img.Draw(new LineSegment2DF(statePoint, predictPoint), new Bgr(Color.Magenta), 1); //Draw a line between the current position and prediction of next position
//Trace.WriteLine(String.Format("Velocity: {0}", tracker.CorrectedState[1, 0]));
#endregion
syntheticData.GoToNextState();
viewer.Image = img;
});
timer.Start();
viewer.Disposed += delegate(Object sender, EventArgs e) { timer.Stop(); };
viewer.Text = "Actual State: White; Measurement: Red; Prediction: Green";
viewer.ShowDialog();
}
