title: implementation of kalman filter to filter acceleration and find velocity and position
body: <p>i have implement the 3D kalman filter in java to filter the acceleration and find velocity &amp; position, i have acceleration as sensor data but when i apply filtering the result is not desired which is supposed to be may be there is some mistake and i cant figure it out, 
  there is one MYKalmanFilter3D class and second is Matrix class </p>
  
  <p>here is the MYKalmanFilter3D class code ..</p>
  
  <pre><code>public class MyKalmanFilter3D {
      Matrix X;          //State Space
      Matrix P;          //error coveriance
      double T;          //Delta T time
      Matrix F;          //Transition Matrix
      double Q;          //Proces Noise Matrix
      double R;          //Measurment Noise or variance in sensor
      double Y;          //Residual
      Matrix K;          //Kalman Gain
      double Bu;         //Model Control input
      Matrix H;          //Measurement funtion
  
      //Cunstructer initializing the state space
      public MyKalmanFilter3D(double accelration, double t, double r) { //recieving sensor initial value, time and noise in sensor
          //State Space 3 x 1
          double[][] x = new double[][]{{0.}, {0.}, {accelration}};
          this.X = new Matrix(x);
          //error coveriance 3 x 3
          double[][] p = new double[][]{{10., 0., 0}, {0., 10, 0.}, {0., 0., 10.}};
          this.P = new Matrix(p);
          //Delta T time
          this.T = t;
          //State Transition Matrix 3 x 3
          double[][] f = new double[][]{{1., T, 0.5 * (T * T)}, {0., 1., T}, {0., 0., 1.}};
          this.F = new Matrix(f);
          //Proces Noise Matrix
          this.Q = 0;
          //Measurment Noise or variance in sensor
          this.R = r;
          //Residual
          this.Y = 0;
          //Kalman Gain
          double[][] k = new double[][]{{1.}, {1.}, {1.}};
          this.K = new Matrix(k);
          //Model Control input
          this.Bu = 0;
          //Measurement Funtion
          double[][] h = new double[][]{{0., 0., 1.}};
          this.H = new Matrix(h);
      }
  
      //getter for accelration in state space
      double estimatedAccelration() {
          return X.elementAt(2, 0);
      }
  
      //getter for velocity in state space
      double velocity() {
          return X.elementAt(1, 0);
      }
  
      //getter for position in state space
      double position() {
          return X.elementAt(0, 0);
      }
  
      //Predict
      // X' = X*F + B*u
      // P' = F*P*Ft + Q
  
      public void predict() {
          X = F.times(X);
          P = F.times(P).times(F.transpose());
      }
  
      //Update
      // Y = Z - H*X'
      // K = P*H't / (H*P*H't + R)
      // X = X' + K*Y
      // p = (1 - K*H)*P'
  
      public void update(double Z) { //here is Z measurement value from sensor which is to be filter
          Y = Z - H.times(X).elementAt(0, 0);
          K = P.times(H.transpose()).dividedByNumber((H.times(P.times(H.transpose())).elementAt(0, 0) + R));
          X = X.plus(K.multiplyByNumber(Y));
          P = (K.numberSubtractedByMatrix(1).times(H)).times(P);
      }
  }
  </code></pre>
  
  <p>and here it is the Matrix Class code ...</p>
  
  <pre><code>import java.io.OutputStreamWriter;
  import java.io.PrintWriter;
  import java.io.UnsupportedEncodingException;
  import java.util.Locale;
  import android.util.Log;
  final public class Matrix {
      private final int M;             // number of rows
      private final int N;             // number of columns
      private final double[][] data;   // M-by-N array
  
      // create M-by-N matrix of 0's
      public Matrix(int M, int N) {
          this.M = M;
          this.N = N;
          data = new double[M][N];
      }
  
      // create matrix based on 2d array
      public Matrix(double[][] data) {
          M = data.length;
          N = data[0].length;
          this.data = new double[M][N];
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  this.data[i][j] = data[i][j];
      }
  
      // copy constructor
      private Matrix(Matrix A) { this(A.data); }
  
      // create and return a random M-by-N matrix with values between 0 and 1
      public static Matrix random(int M, int N) {
          Matrix A = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  A.data[i][j] = Math.random();
          return A;
      }
  
      // create and return the N-by-N identity matrix
      public static Matrix identity(int N) {
          Matrix I = new Matrix(N, N);
          for (int i = 0; i &lt; N; i++)
              I.data[i][i] = 1;
          return I;
      }
  
      // swap rows i and j
      private void swap(int i, int j) {
          double[] temp = data[i];
          data[i] = data[j];
          data[j] = temp;
      }
  
      // create and return the transpose of the invoking matrix
      public Matrix transpose() {
          Matrix A = new Matrix(N, M);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  A.data[j][i] = this.data[i][j];
          return A;
      }
  
      // return A = A / number
      public Matrix dividedByNumber(double num) {
          Matrix A = this;
          Matrix C = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  C.data[i][j] = A.data[i][j] / num;
          return C;
      }
  
      // return A = number - A
      public Matrix numberSubtractedByMatrix(double num) {
          Matrix A = this;
          Matrix C = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  C.data[i][j] = num - A.data[i][j];
          return C;
      }
      // return A = A x number
      public Matrix multiplyByNumber(double num) {
          Matrix A = this;
          Matrix C = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  C.data[i][j] = A.data[i][j] * num;
          return C;
      }
  
      // return C = A + B
      public Matrix plus(Matrix B) {
          Matrix A = this;
          if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
          Matrix C = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  C.data[i][j] = A.data[i][j] + B.data[i][j];
          return C;
      }
  
      // return element at m x n
      public double elementAt(int m, int n) {
          Matrix A = this;
          return A.data[m][n];
      }
  
      // return element at m x n of given Matrix
      public double elementAt(int m, int n, Matrix M) {
          return M.data[m][n];
      }
  
      // return C = A - B
      public Matrix minus(Matrix B) {
          Matrix A = this;
          if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
          Matrix C = new Matrix(M, N);
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  C.data[i][j] = A.data[i][j] - B.data[i][j];
          return C;
      }
  
      // does A = B exactly?
      public boolean eq(Matrix B) {
          Matrix A = this;
          if (B.M != A.M || B.N != A.N) throw new RuntimeException("Illegal matrix dimensions.");
          for (int i = 0; i &lt; M; i++)
              for (int j = 0; j &lt; N; j++)
                  if (A.data[i][j] != B.data[i][j]) return false;
          return true;
      }
  
      // return C = A * B
      public Matrix times(Matrix B) {
          Matrix A = this;
          if (A.N != B.M) throw new RuntimeException("Illegal matrix dimensions.");
          Matrix C = new Matrix(A.M, B.N);
          for (int i = 0; i &lt; C.M; i++)
              for (int j = 0; j &lt; C.N; j++)
                  for (int k = 0; k &lt; A.N; k++)
                      C.data[i][j] += (A.data[i][k] * B.data[k][j]);
          return C;
      }
  
  
      // return x = A^-1 b, assuming A is square and has full rank
      public Matrix solve(Matrix rhs) {
          if (M != N || rhs.M != N || rhs.N != 1)
              throw new RuntimeException("Illegal matrix dimensions.");
  
          // create copies of the data
          Matrix A = new Matrix(this);
          Matrix b = new Matrix(rhs);
  
          // Gaussian elimination with partial pivoting
          for (int i = 0; i &lt; N; i++) {
  
              // find pivot row and swap
              int max = i;
              for (int j = i + 1; j &lt; N; j++)
                  if (Math.abs(A.data[j][i]) &gt; Math.abs(A.data[max][i]))
                      max = j;
              A.swap(i, max);
              b.swap(i, max);
  
              // singular
              if (A.data[i][i] == 0.0) throw new RuntimeException("Matrix is singular.");
  
              // pivot within b
              for (int j = i + 1; j &lt; N; j++)
                  b.data[j][0] -= b.data[i][0] * A.data[j][i] / A.data[i][i];
  
              // pivot within A
              for (int j = i + 1; j &lt; N; j++) {
                  double m = A.data[j][i] / A.data[i][i];
                  for (int k = i+1; k &lt; N; k++) {
                      A.data[j][k] -= A.data[i][k] * m;
                  }
                  A.data[j][i] = 0.0;
              }
          }
  
          // back substitution
          Matrix x = new Matrix(N, 1);
          for (int j = N - 1; j &gt;= 0; j--) {
              double t = 0.0;
              for (int k = j + 1; k &lt; N; k++)
                  t += A.data[j][k] * x.data[k][0];
              x.data[j][0] = (b.data[j][0] - t) / A.data[j][j];
          }
          return x;
  
      }
  
      // print matrix to standard output
      public void show() {
          for (int i = 0; i &lt; M; i++) {
              for (int j = 0; j &lt; N; j++) {
                  System.out.print(data[i][j]);
                  System.out.print(" ");
              }
  
                  //Log.d("MATRIX: ", String.valueOf();
              System.out.print("\n");
              //Log.d("","\n");
  
          }
      }
  }
  </code></pre>