//Setup Kalman Filter and predict methods
public void InitializeKalmanFilter()
{
_kalmanXYZ = new Kalman(6, 3, 0);
Matrix<float> state = new Matrix<float>(new float[]
{
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
});
_kalmanXYZ.CorrectedState = state;
_kalmanXYZ.TransitionMatrix = new Matrix<float>(new float[,]
{
// x-pos, y-pos, z-pos, x-velocity, y-velocity, z-velocity
{1, 0, 0, 1, 0, 0},
{0, 1, 0, 0, 1, 0},
{0, 0, 1, 0, 0, 1},
{0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 1, 0},
{0, 0, 0, 0, 0, 1}
});
_kalmanXYZ.MeasurementNoiseCovariance = new Matrix<float>(3, 3); //Fixed accordiong to input data
_kalmanXYZ.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1.0e-1));
_kalmanXYZ.ProcessNoiseCovariance = new Matrix<float>(6, 6); //Linked to the size of the transition matrix
_kalmanXYZ.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1.0e-4)); //The smaller the value the more resistance to noise
_kalmanXYZ.ErrorCovariancePost = new Matrix<float>(6, 6); //Linked to the size of the transition matrix
_kalmanXYZ.ErrorCovariancePost.SetIdentity();
_kalmanXYZ.MeasurementMatrix = new Matrix<float>(new float[,]
{
{ 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 }
});
}
private void initKalman()
{
last = lastEst = new System.Drawing.Point();
kf = new Kalman(kfData.state,kfData.transitionMatrix, kfData.measurementMatrix,
kfData.processNoise, kfData.measurementNoise);
kf.ErrorCovariancePost = kfData.errorCovariancePost;
}
public Matrix<float> FilterPoints(Kalman kalman, float x, float y, float z)
{
Matrix<float> prediction = kalman.Predict();
Matrix<float> estimated = kalman.Correct(new Matrix<float>(new[] { x, y, z }));
Matrix<float> results = new Matrix<float>(new[,]
{
{ prediction[0, 0], prediction[1, 0], prediction[2, 0] },
{ estimated[0, 0], estimated[1, 0], estimated[2, 0] }
});
return results;
}
public Ball()
{
m_PreviousBallPositions = new List<PointF>();
m_KalmanFilter = new Kalman(4, 2, 0);
m_KalmanFilter.CorrectedState = new Matrix<float>(new float[] { 0f, 0f, 0f, 0f });
m_KalmanFilter.TransitionMatrix = new Matrix<float>(new float[,] { { 1f, 0, 1, 0 }, { 0, 1f, 0, 1 }, { 0, 0, 0.85f, 0 }, { 0, 0, 0, 0.85f } });
m_KalmanFilter.MeasurementNoiseCovariance = new Matrix<float>(new float[,] { { 0.0006f, 0 }, { 0, 0.0006f } });
m_KalmanFilter.ProcessNoiseCovariance = new Matrix<float>(new float[,] { { 0.1f, 0, 0, 0 }, { 0, 0.1f, 0, 0 }, { 0, 0, 0.1f, 0 }, { 0, 0, 0, 0.1f } });
m_KalmanFilter.ErrorCovariancePost = new Matrix<float>(new float[,] { { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 }, { 0, 0, 1f, 0 }, { 0, 0, 0, 1f } });
m_KalmanFilter.MeasurementMatrix = new Matrix<float>(new float[,] { { 1f, 0, 0, 0 }, { 0, 1f, 0, 0 } });
}
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;
}
// Инициализация фильтра Кальмана
public KalmanFilter()
{
m_timeLastMeasurement = clock();
int dynam_params = 4; // x,y,dx,dy
int measure_params = 2;
m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params);
state = cvCreateMat( dynam_params, 1, CV_32FC1 );
// Генератор случайных чисел
cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
cvRandSetRange( &rng, 0, 1, 0 );
rng.disttype = CV_RAND_NORMAL;
cvRand( &rng, state );
process_noise = cvCreateMat( dynam_params, 1, CV_32FC1 ); // (w_k)
measurement = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (z_k)
measurement_noise = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (v_k)
cvZero(measurement);
// F matrix data
// F is transition matrix. It relates how the states interact
// For single input fixed velocity the new value
// depends on the previous value and velocity- hence 1 0 1 0
// on top line. Новое значение скорости не зависит от предыдущего
// значения координаты, и зависит только от предыдущей скорости- поэтому 0 1 0 1 on second row
const float[] F = {
1, 0, 1, 0,//x + dx
0, 1, 0, 1,//y + dy
0, 0, 1, 0,//dx = dx
0, 0, 0, 1,//dy = dy
};
memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F));
cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H)
cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q)
cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); // (R)
cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1));
// choose random initial state
cvRand( &rng, m_pKalmanFilter->state_post );
//InitializeCriticalSection(&mutexPrediction);
}
private bool isInitialized; // true if any data has been fed
public KalmanFilter(int variables)
{
variablesCount = variables;
int measurementVariables = variables;
int dynamicVariables = variables * 2;
float[] state = new float[dynamicVariables];
for (int i = 0; i < dynamicVariables; ++i)
state[i] = 0.0f;
Matrix<float> transitionMatrix = new Matrix<float>(dynamicVariables, dynamicVariables);
transitionMatrix.SetZero();
for (int i = 0; i < dynamicVariables; ++i)
{
transitionMatrix[i, i] = 1.0f;
if (i >= measurementVariables)
transitionMatrix[i - measurementVariables, i] = 1;
}
Matrix<float> measurementMatrix = new Matrix<float>(measurementVariables, dynamicVariables);
measurementMatrix.SetZero();
for (int i = 0; i < measurementVariables; ++i)
measurementMatrix[i, i] = 1.0f;
Matrix<float> processNoise = new Matrix<float>(dynamicVariables, dynamicVariables);
processNoise.SetIdentity(new MCvScalar(1));//1.0e-4));
Matrix<float> measurementNoise = new Matrix<float>(measurementVariables, measurementVariables);
measurementNoise.SetIdentity(new MCvScalar(4));//1.0e-1));
Matrix<float> errorCovariancePost = new Matrix<float>(dynamicVariables, dynamicVariables);
errorCovariancePost.SetIdentity();
kalman = new Kalman(dynamicVariables, measurementVariables, 0);
kalman.CorrectedState = new Matrix<float>(state);
kalman.TransitionMatrix = transitionMatrix;
kalman.MeasurementNoiseCovariance = measurementNoise;
kalman.ProcessNoiseCovariance = processNoise;
kalman.ErrorCovariancePost = errorCovariancePost;
kalman.MeasurementMatrix = measurementMatrix;
}
private void inicializarKalman()
{
kal = new Kalman(4, 2, 0);
kal.TransitionMatrix = new Matrix<float>(new float[4, 4] { {1, 0, 1, 0}, {0, 1, 0, 1}, {0, 0, 1, 0}, {0, 0, 0, 1} });
kal.MeasurementMatrix.SetIdentity();
kal.ProcessNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
kal.MeasurementNoiseCovariance.SetIdentity(new MCvScalar(1e-4));
kal.ErrorCovariancePost.SetIdentity(new MCvScalar(1e-4));
medidaKalman = new Matrix<float>(2, 1);
}
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();
}
