Kalman2DPositionFilter.cs 5.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Text;
  5. using System.Threading.Tasks;
  6. using Emgu.CV;
  7. using Emgu.CV.Structure;
  8. namespace bbiwarg.Utility
  9. {
  10. /// <summary>
  11. /// Filter used to smooth a series of 2d positions.
  12. /// </summary>
  13. class Kalman2DPositionFilter
  14. {
  15. /// <summary>
  16. /// the emgu kalman filter
  17. /// </summary>
  18. private Kalman kalman;
  19. /// <summary>
  20. /// xx entry for the measurement noise covariance matrix
  21. /// </summary>
  22. private float mXX;
  23. /// <summary>
  24. /// xy and yx entry for the measurement noise covariance matrix
  25. /// </summary>
  26. private float mXY;
  27. /// <summary>
  28. /// yy entry for the measurement noise covariance matrix
  29. /// </summary>
  30. private float mYY;
  31. /// <summary>
  32. /// value used for all entries in the process noise covariance matrix
  33. /// </summary>
  34. private float processNoiseFactor;
  35. /// <summary>
  36. /// number of measurements per second
  37. /// </summary>
  38. private float fps;
  39. /// <summary>
  40. /// true iff the kalman filter is initialized
  41. /// </summary>
  42. public bool Initialized { get; private set; }
  43. // state: x, y, v_x, v_y
  44. // x: current x position
  45. // y: current y position
  46. // v_x: velocity in x direction
  47. // v_y: velocity in y direction
  48. // a_x: acceleration in x direction
  49. // a_y: acceleration in y direction
  50. /// <summary>
  51. /// Creates a Kalman2DPositionFilter.
  52. /// </summary>
  53. /// <param name="mXX">xx entry for the measurement noise covariance matrix</param>
  54. /// <param name="mXY">xy and yx entry for the measurement noise covariance matrix</param>
  55. /// <param name="mYY">yy entry for the measurement noise covariance matrix</param>
  56. /// <param name="processNoiseFactor">value used for all entries in the process noise covariance matrix</param>
  57. /// <param name="fps">number of measurements per second</param>
  58. public Kalman2DPositionFilter(float mXX, float mXY, float mYY, float processNoiseFactor = 1.0e-4f, int fps = 30)
  59. {
  60. this.mXX = mXX;
  61. this.mXY = mXY;
  62. this.mYY = mYY;
  63. this.processNoiseFactor = processNoiseFactor;
  64. this.fps = fps;
  65. reset();
  66. }
  67. /// <summary>
  68. /// Resets the kalman filter.
  69. /// </summary>
  70. public void reset()
  71. {
  72. // 6 state variables and 2 measurements (0 controls)
  73. kalman = new Kalman(2, 2, 0);
  74. // time step (s)
  75. float t = 1 / fps;
  76. // transition matrix
  77. Matrix<float> transitionMatrix = new Matrix<float>(new float[,]
  78. { {1.0f, 0.0f},
  79. {0.0f, 1.0f}});
  80. kalman.TransitionMatrix = transitionMatrix;
  81. // measurement matrix
  82. Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
  83. { {1.0f, 0.0f}, // first measurement = x
  84. {0.0f, 1.0f} // second measurement = y
  85. });
  86. kalman.MeasurementMatrix = measurementMatrix;
  87. // measurement noise covariance matrix
  88. Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);
  89. measurementNoiseCovarianceMatrix[0, 0] = mXX;
  90. measurementNoiseCovarianceMatrix[0, 1] = measurementNoiseCovarianceMatrix[1, 0] = mXY;
  91. measurementNoiseCovarianceMatrix[1, 1] = mYY;
  92. kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
  93. // process noise covariance matrix
  94. Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(2, 2);
  95. processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
  96. kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
  97. // error covariance post matrix (initial value)
  98. Matrix<float> errorCovariancePostMatrix = new Matrix<float>(2, 2);
  99. errorCovariancePostMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
  100. kalman.ErrorCovariancePost = errorCovariancePostMatrix;
  101. Initialized = false;
  102. }
  103. /// <summary>
  104. /// Sets the initial position.
  105. /// </summary>
  106. /// <param name="initialPosition">initial position</param>
  107. public void setInitialPosition(Vector2D initialPosition)
  108. {
  109. // initial state (x, y, v_x, v_y)
  110. Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y});
  111. kalman.CorrectedState = initialState;
  112. Initialized = true;
  113. }
  114. /// <summary>
  115. /// Computes a prediction for the next position based on the previous positions.
  116. /// </summary>
  117. /// <returns>prediction for the next position</returns>
  118. public Vector2D getPrediction()
  119. {
  120. Matrix<float> predicton = kalman.Predict();
  121. return new Vector2D(predicton[0, 0], predicton[1, 0]);
  122. }
  123. /// <summary>
  124. /// Computes a smoothed position for a measurement and updates the filter.
  125. /// </summary>
  126. /// <param name="rawPosition">the measurement</param>
  127. /// <returns>the smoothed position</returns>
  128. public Vector2D getCorrectedPosition(Vector2D rawPosition)
  129. {
  130. Matrix<float> rawPositionMatrix = new Matrix<float>(new float[,]
  131. { {rawPosition.X},
  132. {rawPosition.Y}});
  133. // prediction according to model
  134. kalman.Predict();
  135. // corrected point
  136. Matrix<float> estimate = kalman.Correct(rawPositionMatrix);
  137. return new Vector2D(estimate[0, 0], estimate[1, 0]);
  138. }
  139. }
  140. }