|
@@ -9,14 +9,49 @@ using Emgu.CV.Structure;
|
|
|
|
|
|
namespace bbiwarg.Utility
|
|
|
{
|
|
|
+ /// <summary>
|
|
|
+ /// Filter used to smooth a series of 2d positions.
|
|
|
+ /// </summary>
|
|
|
class Kalman2DPositionFilter
|
|
|
{
|
|
|
+ /// <summary>
|
|
|
+ /// the emgu kalman filter
|
|
|
+ /// </summary>
|
|
|
private Kalman kalman;
|
|
|
- private float mXX, mXY, mYY, processNoiseFactor;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// xx entry for the measurement noise covariance matrix
|
|
|
+ /// </summary>
|
|
|
+ private float mXX;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// xy and yx entry for the measurement noise covariance matrix
|
|
|
+ /// </summary>
|
|
|
+ private float mXY;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// yy entry for the measurement noise covariance matrix
|
|
|
+ /// </summary>
|
|
|
+ private float mYY;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// value used for all entries in the process noise covariance matrix
|
|
|
+ /// </summary>
|
|
|
+ private float processNoiseFactor;
|
|
|
+
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// number of measurements per second
|
|
|
+ /// </summary>
|
|
|
private float fps;
|
|
|
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// true iff the kalman filter is initialized
|
|
|
+ /// </summary>
|
|
|
public bool Initialized { get; private set; }
|
|
|
|
|
|
+
|
|
|
// state: x, y, v_x, v_y
|
|
|
// x: current x position
|
|
|
// y: current y position
|
|
@@ -24,6 +59,14 @@ namespace bbiwarg.Utility
|
|
|
// v_y: velocity in y direction
|
|
|
// a_x: acceleration in x direction
|
|
|
// a_y: acceleration in y direction
|
|
|
+ /// <summary>
|
|
|
+ /// Creates a Kalman2DPositionFilter.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="mXX">xx entry for the measurement noise covariance matrix</param>
|
|
|
+ /// <param name="mXY">xy and yx entry for the measurement noise covariance matrix</param>
|
|
|
+ /// <param name="mYY">yy entry for the measurement noise covariance matrix</param>
|
|
|
+ /// <param name="processNoiseFactor">value used for all entries in the process noise covariance matrix</param>
|
|
|
+ /// <param name="fps">number of measurements per second</param>
|
|
|
public Kalman2DPositionFilter(float mXX, float mXY, float mYY, float processNoiseFactor = 1.0e-4f, int fps = 30)
|
|
|
{
|
|
|
this.mXX = mXX;
|
|
@@ -34,6 +77,9 @@ namespace bbiwarg.Utility
|
|
|
reset();
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// Resets the kalman filter.
|
|
|
+ /// </summary>
|
|
|
public void reset()
|
|
|
{
|
|
|
// 6 state variables and 2 measurements (0 controls)
|
|
@@ -77,6 +123,10 @@ namespace bbiwarg.Utility
|
|
|
Initialized = false;
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// Sets the initial position.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="initialPosition">initial position</param>
|
|
|
public void setInitialPosition(Vector2D initialPosition)
|
|
|
{
|
|
|
// initial state (x, y, v_x, v_y)
|
|
@@ -86,13 +136,21 @@ namespace bbiwarg.Utility
|
|
|
Initialized = true;
|
|
|
}
|
|
|
|
|
|
+ /// <summary>
|
|
|
+ /// Computes a prediction for the next position based on the previous positions.
|
|
|
+ /// </summary>
|
|
|
+ /// <returns>prediction for the next position</returns>
|
|
|
public Vector2D getPrediction()
|
|
|
{
|
|
|
Matrix<float> predicton = kalman.Predict();
|
|
|
return new Vector2D(predicton[0, 0], predicton[1, 0]);
|
|
|
}
|
|
|
|
|
|
- // updates the filter and returns the corrected position
|
|
|
+ /// <summary>
|
|
|
+ /// Computes a smoothed position for a measurement and updates the filter.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="rawPosition">the measurement</param>
|
|
|
+ /// <returns>the smoothed position</returns>
|
|
|
public Vector2D getCorrectedPosition(Vector2D rawPosition)
|
|
|
{
|
|
|
Matrix<float> rawPositionMatrix = new Matrix<float>(new float[,]
|