-deleted acceleration in Kalman Filter

bbiwarg/Utility/Kalman2DPositionFilter.cs

@@ -37,25 +37,23 @@ namespace bbiwarg.Utility
         public void reset()
         {
             // 6 state variables and 2 measurements (0 controls)
-            kalman = new Kalman(6, 2, 0);
+            kalman = new Kalman(4, 2, 0);
 
             // time step (s)
             float t = 1 / fps;
 
             // transition matrix 
             Matrix<float> transitionMatrix = new Matrix<float>(new float[,] 
-            { {1.0f, 0.0f, 1.0f * t, 0.0f, 0.5f * t * t, 0.0f},  
-              {0.0f, 1.0f, 0.0f, 1.0f * t, 0.0f, 0.5f * t * t},  
-              {0.0f, 0.0f, 1.0f, 0.0f, 1.0f * t, 0.0f},  
-              {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f * t},  
-              {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f},  
-              {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f}});
+            { {1.0f, 0.0f, 1.0f * t, 0.0f},  
+              {0.0f, 1.0f, 0.0f, 1.0f * t},  
+              {0.0f, 0.0f, 1.0f, 0.0f},  
+              {0.0f, 0.0f, 0.0f, 1.0f}});
             kalman.TransitionMatrix = transitionMatrix;
 
             // measurement matrix
             Matrix<float> measurementMatrix = new Matrix<float>(new float[,]
-            { {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // first measurement = x
-              {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}  // second measurement = y
+            { {1.0f, 0.0f, 0.0f, 0.0f}, // first measurement = x
+              {0.0f, 1.0f, 0.0f, 0.0f}  // second measurement = y
             });
             kalman.MeasurementMatrix = measurementMatrix;
 
@@ -69,12 +67,12 @@ namespace bbiwarg.Utility
             kalman.MeasurementNoiseCovariance = measurementNoiseCovarianceMatrix;
 
             // process noise covariance matrix
-            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(6, 6);
+            Matrix<float> processNoiseCovarianceMatrix = new Matrix<float>(4, 4);
             processNoiseCovarianceMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
             kalman.ProcessNoiseCovariance = processNoiseCovarianceMatrix;
 
             // error covariance post matrix (initial value)
-            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(6, 6);
+            Matrix<float> errorCovariancePostMatrix = new Matrix<float>(4, 4);
             errorCovariancePostMatrix.SetIdentity(new MCvScalar(processNoiseFactor));
             kalman.ErrorCovariancePost = errorCovariancePostMatrix;
 
@@ -84,7 +82,7 @@ namespace bbiwarg.Utility
         public void setInitialPosition(Vector2D initialPosition)
         {
             // initial state (x, y, v_x, v_y)
-            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f, 0.0f, 0.0f });
+            Matrix<float> initialState = new Matrix<float>(new float[] { initialPosition.X, initialPosition.Y, 0.0f, 0.0f });
             kalman.CorrectedState = initialState;
 
             Initialized = true;