// Set up the initial values for the Kalman filter
void InitializeKalmanFilter()
{
// Create the Kalman Filter
_kalman = Cv.CreateKalman(4, 2, 0);
// Set the Kalman filter initial state
_kalman.StatePre.Set2D(0, 0, _rectToTrack.X); // Initial X position
_kalman.StatePre.Set2D(1, 0, _rectToTrack.Y); // Initial Y position
_kalman.StatePre.Set2D(2, 0, 0); // Initial X velocity
_kalman.StatePre.Set2D(3, 0, 0); // Initial Y velocity
// Prediction Equations
// X position is a function of previous X positions and previous X velocities
_kalman.TransitionMatrix.Set2D(0, 0, 1);
_kalman.TransitionMatrix.Set2D(1, 0, 0);
_kalman.TransitionMatrix.Set2D(2, 0, 1);
_kalman.TransitionMatrix.Set2D(3, 0, 0);
// Y position is a function of previous Y positions and previous Y velocities
_kalman.TransitionMatrix.Set2D(0, 1, 0);
_kalman.TransitionMatrix.Set2D(1, 1, 1);
_kalman.TransitionMatrix.Set2D(2, 1, 0);
_kalman.TransitionMatrix.Set2D(3, 1, 1);
// X velocity is a function of previous X velocities
_kalman.TransitionMatrix.Set2D(0, 2, 0);
_kalman.TransitionMatrix.Set2D(1, 2, 0);
_kalman.TransitionMatrix.Set2D(2, 2, 1);
_kalman.TransitionMatrix.Set2D(3, 2, 0);
// Y velocity is a function of previous Y velocities
_kalman.TransitionMatrix.Set2D(0, 3, 0);
_kalman.TransitionMatrix.Set2D(1, 3, 0);
_kalman.TransitionMatrix.Set2D(2, 3, 0);
_kalman.TransitionMatrix.Set2D(3, 3, 1);
// set Kalman Filter
Cv.SetIdentity(_kalman.MeasurementMatrix, 1.0f);
Cv.SetIdentity(_kalman.ProcessNoiseCov, 0.4f); //0.4
Cv.SetIdentity(_kalman.MeasurementNoiseCov, 3f); //3
Cv.SetIdentity(_kalman.ErrorCovPost, 1.0f);
}
/// <summary>
/// kalman 初期化ルーチン
/// </summary>
/// <param name="elem">読み出した要素</param>
public void kalman_init()
{
CvKalman kalman = kv_kalman;
// 初期化(kalman)
kalman_id = 0;
Cv.SetIdentity(kalman.MeasurementMatrix, Cv.RealScalar(1.0));
Cv.SetIdentity(kalman.ProcessNoiseCov, Cv.RealScalar(1e-4));
Cv.SetIdentity(kalman.MeasurementNoiseCov, Cv.RealScalar(0.001));
Cv.SetIdentity(kalman.ErrorCovPost, Cv.RealScalar(1.0));
measurement.Zero();
// 等速直線運動モデル(kalman)
/* unsafe
* {
* kalman.DynamMatr[0] = 1.0f; kalman.DynamMatr[1] = 0.0f; kalman.DynamMatr[2] = 1.0f; kalman.DynamMatr[3] = 0.0f;
* kalman.DynamMatr[4] = 0.0f; kalman.DynamMatr[5] = 1.0f; kalman.DynamMatr[6] = 0.0f; kalman.DynamMatr[7] = 1.0f;
* kalman.DynamMatr[8] = 0.0f; kalman.DynamMatr[9] = 0.0f; kalman.DynamMatr[10] = 1.0f; kalman.DynamMatr[11] = 0.0f;
* kalman.DynamMatr[12] = 0.0f; kalman.DynamMatr[13] = 0.0f; kalman.DynamMatr[14] = 0.0f; kalman.DynamMatr[15] = 1.0f;
* }*/
kalman.TransitionMatrix.Set2D(0, 0, 1.0f);
kalman.TransitionMatrix.Set2D(0, 1, 0.0f);
kalman.TransitionMatrix.Set2D(0, 2, 1.0f);
kalman.TransitionMatrix.Set2D(0, 3, 0.0f);
kalman.TransitionMatrix.Set2D(1, 0, 0.0f);
kalman.TransitionMatrix.Set2D(1, 1, 1.0f);
kalman.TransitionMatrix.Set2D(1, 2, 0.0f);
kalman.TransitionMatrix.Set2D(1, 3, 1.0f);
kalman.TransitionMatrix.Set2D(2, 0, 0.0f);
kalman.TransitionMatrix.Set2D(2, 1, 0.0f);
kalman.TransitionMatrix.Set2D(2, 2, 1.0f);
kalman.TransitionMatrix.Set2D(2, 3, 0.0f);
kalman.TransitionMatrix.Set2D(3, 0, 0.0f);
kalman.TransitionMatrix.Set2D(3, 1, 0.0f);
kalman.TransitionMatrix.Set2D(3, 2, 0.0f);
kalman.TransitionMatrix.Set2D(3, 3, 1.0f);
}
public unsafe Kalman()
{
// cvKalmanPredict, cvKalmanCorrect
// カルマンフィルタを用いて回転する点を追跡する
// A matrix data
float[] A = new float[] { 1, 1, 0, 1 };
using (IplImage img = new IplImage(500, 500, BitDepth.U8, 3))
using (CvKalman kalman = new CvKalman(2, 1, 0))
using (CvWindow window = new CvWindow("Kalman", WindowMode.AutoSize))
{
// state is (phi, delta_phi) - angle and angle increment
CvMat state = new CvMat(2, 1, MatrixType.F32C1);
CvMat process_noise = new CvMat(2, 1, MatrixType.F32C1);
// only phi (angle) is measured
CvMat measurement = new CvMat(1, 1, MatrixType.F32C1);
measurement.SetZero();
CvRandState rng = new CvRandState(0, 1, -1, DistributionType.Uniform);
int code = -1;
for (; ;)
{
Cv.RandSetRange(rng, 0, 0.1, 0);
rng.DistType = DistributionType.Normal;
Marshal.Copy(A, 0, kalman.TransitionMatrix.Data, A.Length);
kalman.MeasurementMatrix.SetIdentity(1);
kalman.ProcessNoiseCov.SetIdentity(1e-5);
kalman.MeasurementNoiseCov.SetIdentity(1e-1);
kalman.ErrorCovPost.SetIdentity(1);
// choose random initial state
Cv.Rand(rng, kalman.StatePost);
rng.DistType = DistributionType.Normal;
for (; ;)
{
float state_angle = state.DataSingle[0];
CvPoint state_pt = CalcPoint(img, state_angle);
// predict point position
CvMat prediction = kalman.Predict(null);
float predict_angle = prediction.DataSingle[0];
CvPoint predict_pt = CalcPoint(img, predict_angle);
Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.MeasurementNoiseCov.DataSingle[0]), 0);
Cv.Rand(rng, measurement);
// generate measurement
Cv.MatMulAdd(kalman.MeasurementMatrix, state, measurement, measurement);
float measurement_angle = measurement.DataArraySingle[0];
CvPoint measurement_pt = CalcPoint(img, measurement_angle);
img.SetZero();
DrawCross(img, state_pt, CvColor.White, 3);
DrawCross(img, measurement_pt, CvColor.Red, 3);
DrawCross(img, predict_pt, CvColor.Green, 3);
img.Line(state_pt, measurement_pt, new CvColor(255, 0, 0), 3, LineType.AntiAlias, 0);
img.Line(state_pt, predict_pt, new CvColor(255, 255, 0), 3, LineType.AntiAlias, 0);
// adjust Kalman filter state
kalman.Correct(measurement);
Cv.RandSetRange(rng, 0, Math.Sqrt(kalman.ProcessNoiseCov.DataSingle[0]), 0);
Cv.Rand(rng, process_noise);
Cv.MatMulAdd(kalman.TransitionMatrix, state, process_noise, state);
window.ShowImage(img);
// break current simulation by pressing a key
code = CvWindow.WaitKey(100);
if (code > 0)
{
break;
}
}
// exit by ESCAPE
if (code == 27)
{
break;
}
}
}
}
