Kalman2DPositionFilter.cs 3.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110
  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. class Kalman2DPositionFilter
  11. {
  12. private Kalman kalman;
  13. private float mXX, mXY, mYY, processNoiseFactor;
  14. private float fps;
  15. public bool Initialized { get; private set; }
  16. // state: x, y, v_x, v_y
  17. // x: current x position
  18. // y: current y position
  19. // v_x: velocity in x direction
  20. // v_y: velocity in y direction
  21. // a_x: acceleration in x direction
  22. // a_y: acceleration in y direction
  23. public Kalman2DPositionFilter(float mXX, float mXY, float mYY, float processNoiseFactor = 1.0e-4f, int fps = 30)
  24. {
  25. this.mXX = mXX;
  26. this.mXY = mXY;
  27. this.mYY = mYY;
  28. this.processNoiseFactor = processNoiseFactor;
  29. this.fps = fps;
  30. reset();
  31. }
  32. public void reset()
  33. {
  34. // 6 state variables and 2 measurements (0 controls)
  35. kalman = new Kalman(2, 2, 0);
  36. // time step (s)
  37. float t = 1 / fps;
  38. // transition matrix
  39. Matrix<float> transitionMatrix = new Matrix<float>(new float[,]
  40. { {1.0f, 0.0f},
  41. {0.0f, 1.0f}});
  42. kalman.TransitionMatrix = transitionMatrix;
  43. // measurement matrix
  44. Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
  45. { {1.0f, 0.0f}, // first measurement = x
  46. {0.0f, 1.0f} // second measurement = y
  47. });
  48. kalman.MeasurementMatrix = measurementMatrix;
  49. // measurement noise covariance matrix
  50. Matrix<float> measurementNoiseCovarianceMatrix = new Matrix<float>(2, 2);
  51. measurementNoiseCovarianceMatrix[0, 0] = mXX;
  52. measurementNoiseCovarianceMatrix[0, 1] = measurementNoiseCovarianceMatrix[1, 0] = mXY;
  53. measurementNoiseCovarianceMatrix[1, 1] = mYY;
  54. kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
  55. // process noise covariance matrix
  56. Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(2, 2);
  57. processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
  58. kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
  59. // error covariance post matrix (initial value)
  60. Matrix<float> errorCovariancePostMatrix = new Matrix<float>(2, 2);
  61. errorCovariancePostMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
  62. kalman.ErrorCovariancePost = errorCovariancePostMatrix;
  63. Initialized = false;
  64. }
  65. public void setInitialPosition(Vector2D initialPosition)
  66. {
  67. // initial state (x, y, v_x, v_y)
  68. Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y});
  69. kalman.CorrectedState = initialState;
  70. Initialized = true;
  71. }
  72. public Vector2D getPrediction()
  73. {
  74. Matrix<float> predicton = kalman.Predict();
  75. return new Vector2D(predicton[0, 0], predicton[1, 0]);
  76. }
  77. // updates the filter and returns the corrected position
  78. public Vector2D getCorrectedPosition(Vector2D rawPosition)
  79. {
  80. Matrix<float> rawPositionMatrix = new Matrix<float>(new float[,]
  81. { {rawPosition.X},
  82. {rawPosition.Y}});
  83. // prediction according to model
  84. kalman.Predict();
  85. // corrected point
  86. Matrix<float> estimate = kalman.Correct(rawPositionMatrix);
  87. return new Vector2D(estimate[0, 0], estimate[1, 0]);
  88. }
  89. }
  90. }