Filtro Kalman su dati accelerometro Android

Una implementazione del filtro kalman su dati sui dati dell'accelerometro in Android (in realta' i dati sono pitch e roll ma derivano dall'accelerometro)

Per il calcolo e' stata utilizzata la libreria JKalman.  Dal pacchetto zip si estrae il file .jar e dalla cartella dist e lo si copia nella cartella /libs del progetto Android. A questo punto si clicca destro sul file .jar in Android Studio e si clicca "Add as Library"

Si aggiunge la classe ripresa da qui

------------------------------------------------------------------

package innocenti.luca.com.camerakit;

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

}

------------------------------------------------------------------

a questo punto e' sufficiente creare il filtro in OnCreate

kf = new KalmanFilter();

try {
    kf.initialize2();
} catch (Exception e) {
    e.printStackTrace();
}

si popola poi l'array dei dati e si ottiene il valore filtrato

try {
    kf.push(pi,ro);
} catch (Exception e) {
    e.printStackTrace();
}

try {
    double[] test = kf.getKalmanPoint2();
    Log.d("Kalman", Double.toString(Math.toDegrees(test[0]))+ " "+ 

 Double.toString(Math.toDegrees(test[1])) + " " +Math.toDegrees(pi)+ " "+

 Math.toDegrees(ro));
} catch (Exception e) {
    e.printStackTrace();
}