implementation of kalman filter to filter acceleration and find velocity and position

Question

i have implement the 3D kalman filter in java to filter the acceleration and find velocity & 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

here is the MYKalmanFilter3D class 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);
    }
}

and here it is the Matrix Class 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < M; i++)
            for (int j = 0; j < 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 < C.M; i++)
            for (int j = 0; j < C.N; j++)
                for (int k = 0; k < 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 < N; i++) {

            // find pivot row and swap
            int max = i;
            for (int j = i + 1; j < N; j++)
                if (Math.abs(A.data[j][i]) > 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 < 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 < N; j++) {
                double m = A.data[j][i] / A.data[i][i];
                for (int k = i+1; k < 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 >= 0; j--) {
            double t = 0.0;
            for (int k = j + 1; k < 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 < M; i++) {
            for (int j = 0; j < N; j++) {
                System.out.print(data[i][j]);
                System.out.print(" ");
            }

                //Log.d("MATRIX: ", String.valueOf();
            System.out.print("\n");
            //Log.d("","\n");

        }
    }
}

Answer 1

I am not fluent in Java, so I could not fully follow your code with respect to Kalman filter implementation. However, using an accelerometer to get velocity and position seems doable in theory, but in real life, due to different uncertainties with MEMS accelerometers, you would end up getting huge errors in velocity and position even after a very short period of time.

Take a look at this link which gives a good intro to uncertainty models for accelerometers.

In short, do not expect good results in terms of position and velocity by only using a consumer-grade accelerometer. If you want to test the code, use simulated fake data where you can control the uncertainty.