Android 实现姿态传感器kalman滤波

采用卡尔曼滤波器实现，java代码如下

卡尔曼（Kalman）滤波器java实现Jkalman_卡尔曼滤波java实现,java卡尔曼滤波-Java文档类资源-CSDN下载

对于要输入三个方向的测量角度，可参考以下代码重新定义kalman filter

accelerometer - Implementing JKalman on Android - Stack Overflow

实施JKalman在Android - IT屋-程序员软件开发技术分享社区

实例化 Kalman class

JKalman kalman = new JKalman(4, 2);

Where according to the definition

public JKalman(int dynam_params, int measure_params) throws Exception {
    this(dynam_params, measure_params, 0);
}

dynam_params is "the number of measurement vector dimensions" and measure_params is "the number of state vector dimensions".

How should the sensor data read in Android be mapped to these?

Below is the data from accelerometer to be seen, that is sampled every 500ms. In other listeners there are data from gyroscope and compass. How should I transform this data to input to Kalman?

    @Override
    public void onSensorChanged(SensorEvent event) {
        actualTime = System.currentTimeMillis();
        if(actualTime - lastUpdateAcc < 500)
            return;
        else{
            lastUpdateAcc = actualTime;
            //update myPosition
            TextView tv = (TextView)findViewById(R.id.textView3);
            tv.setText(String.format("X: %8.4f -- Y: %8.4f -- Z: %8.4f",
                    event.values[0], event.values[1], event.values[2]));
            //draw on the screen

            //draw new path, if one exists
        }
    }

解决方案

this is how I do it with 2 variables (GPS:LAT, LON):

import jkalman.JKalman;
import jama.Matrix;

public class KalmanFilter {
    private int variables;
    private JKalman kalman;
    private Matrix s; // state [x, y, dx, dy, dxy]
    private Matrix c; // corrected state [x, y, dx, dy, dxy]
    private Matrix m; // measurement [x]

    /*
     * Inicializa el filtro kalman con 2 variables
     */
    public void initialize2() throws Exception{
        double dx, dy;

        if(variables != 0){
             throw new RuntimeException();
        }
        variables = 2;
        kalman = new JKalman(4, 2);

        // constant velocity
        dx = 0.2;
        dy = 0.2;

        s = new Matrix(4, 1); // state [x, y, dx, dy, dxy]        
        c = new Matrix(4, 1); // corrected state [x, y, dx, dy, dxy]                

        m = new Matrix(2, 1); // measurement [x]
        m.set(0, 0, 0);
        m.set(1, 0, 0);

        // transitions for x, y, dx, dy
        double[][] tr = { {1, 0, dx, 0}, 
                          {0, 1, 0, dy}, 
                          {0, 0, 1, 0}, 
                          {0, 0, 0, 1} };
        kalman.setTransition_matrix(new Matrix(tr));

        // 1s somewhere?
        kalman.setError_cov_post(kalman.getError_cov_post().identity());

    }

    /*
     * Aplica Filtro a variables
     */
    public void push(double x,double y) throws Exception{
         m.set(0, 0, x);
         m.set(1, 0, y);

         c = kalman.Correct(m);
         s = kalman.Predict();
    }

    /*
     * obtiene arreglo con datos filtrados.
     */
    public double[] getKalmanPoint2() throws Exception{
        double[] point = new double[2];
        point[0] = c.get(0,0);
        point[1] = c.get(1,0);
        return point;
    }

    /*
     * obtiene arreglo con prediccion de punto.
     */
    public double[] getPredict2() throws Exception{
        double[] point = new double[2];
        point[0] = s.get(0,0);
        point[1] = s.get(1,0);
        return point;
    }

    /*
     * obtiene cantidad de variables del objeto
     */
    public int getNVariables() throws Exception{
        return this.variables;
    }

}

But I don't know how to set the first point, I always start from (0,0) and take 50 samples to reach the point, with a loop is not very elegant.

通过SensorManager获取传感器参数，输入kalman filter 即可，后续实现方法类似，在项目中引入Jkalman.jar即可