卡尔曼滤波器的实现以过滤加速度并找到速度和位置

Posted 2023-02-21

【中文标题】卡尔曼滤波器的实现以过滤加速度并找到速度和位置

【英文标题】：implementation of kalman filter to filter acceleration and find velocity and position

【发布时间】：2018-12-08 10:08:41

【问题描述】：

我已经在 java 中实现了 3D 卡尔曼滤波器来过滤加速度并找到速度和位置，我将加速度作为传感器数据，但是当我应用过滤时，结果是不想要的，这应该是可能有一些错误和我想不通 有一个 MYKalmanFilter3D 类，第二个是 Matrix 类

这里是 MYKalmanFilter3D 类代码..

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);
    

这里是矩阵类代码...

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");

        
    

【参考方案1】：

我不精通 Java，因此我无法完全按照您的代码实现卡尔曼滤波器。然而，使用加速度计来获取速度和位置在理论上似乎是可行的，但在现实生活中，由于 MEMS 加速度计的不确定性不同，即使在很短的时间之后，您最终也会得到巨大的速度和位置误差。

看看这个link，它很好地介绍了加速度计的不确定性模型。

简而言之，不要期望仅使用消费级加速度计就能在位置和速度方面获得良好的结果。如果要测试代码，请使用可以控制不确定性的模拟假数据。