Приложение для внутреннего слежения с использованием IMU

Я пытаюсь создать приложение для отслеживания в помещении Android с помощью IMU.

Сейчас мое приложение основано на акселерометре и программном датчике ROTATION_VECTOR, который создается путем объединения данных из ACCELEROMETER, GEOMAGNETIC_FIELD и GYROSCOPE. Я использую акселерометр для определения шагов и датчик ROTATION_VECTOR для ориентации. Когда ступенька обнаружена, я беру данные из ВЕКТОРА ВРАЩЕНИЯ, вычисляю разницу углов между начальным и текущим углом, вычисляю новые координаты и показываю новое местоположение (я использую ступени с фиксированной длиной)

Проблемная часть - точность ориентации. Я прочитал несколько статей, в которых предлагается использовать фильтр Калмана, однако для меня все еще остается загадкой, как его реализовать.

Буду очень признателен, если кто-то мне поможет с этой проблемой. Предложите несколько руководств, как понять фильтр Калмана, или покажите мне, как повысить точность моего приложения.

Большое спасибо.

Мой код:

package com.example.jozef.gyrouhol;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.support.v7.app.AppCompatActivity;
import android.widget.TextView;
import android.widget.Toast;
import android.app.Activity;
import java.lang.Math;
import java.util.ArrayList;
import java.util.List;
import java.util.Locale;
import java.util.concurrent.TimeUnit;

public class Gyro extends AppCompatActivity implements SensorEventListener {

    private SensorManager mSensorManager;
    private Sensor mRotationSensor, mStepSensor;

    private static final int SENSOR_DELAY = 1000;
    private static final int FROM_RADS_TO_DEGS = -57;
    private double norming;

    private ObjectHandler mData;

    private int count = 0;
    private int pmin = 0, pmax=0;
    private long actualTime = 0;

    private float mStartingAngle;
    private HouseBackground myView;
    @Override
    protected void onCreate(Bundle savedInstanceState) {

        super.onCreate(savedInstanceState);
        myView = new HouseBackground(this);
        setContentView(myView);

        try {

            mSensorManager = (SensorManager) getSystemService(Activity.SENSOR_SERVICE);
            mRotationSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
            mStepSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
            mSensorManager.registerListener(this, mRotationSensor, SENSOR_DELAY);
            mSensorManager.registerListener(this,mStepSensor,SENSOR_DELAY);
        } 
        catch (Exception e) {
            Toast.makeText(this, "Hardware compatibility issue", Toast.LENGTH_LONG).show();
        }

        mData = new ObjectHandler();
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
        // TODO Auto-generated method stub
    }

    @Override
    public void onSensorChanged(SensorEvent event) {


        if (event.sensor == mRotationSensor) {

            update(event.values);
        }

        if(event.sensor == mStepSensor) {

            norming = Math.sqrt((event.values[0]*event.values[0])+(event.values[1]*event.values[1])+(event.values[2]*event.values[2]));
            stepCount(norming);
        }
    }

    private void update(float[] vectors) {

        float[] rotationMatrix = new float[9];
        SensorManager.getRotationMatrixFromVector(rotationMatrix, vectors);

        float[] orientation = new float[3];
        SensorManager.getOrientation(rotationMatrix, orientation);

        float xdeg =  orientation[0]* FROM_RADS_TO_DEGS;
        mData.ObjectHandlersetAngle(xdeg);
    }

    protected void onPause() {

        mSensorManager.unregisterListener((SensorEventListener) this);
        super.onPause();
    }

    public void stepCount (double mNorming){
        if (norming > 10.403 )

            pmax = 1;

        if (norming < 8.45)

            pmin = 1;

        if (pmax == 1 && pmin == 1) {

            if (count == 0){
                count++;
                actualTime = System.currentTimeMillis();
                if(mStartingAngle == 0)
                {
                    mStartingAngle = mData.ObjectHandlergetAngle();
                }
                myView.newPointAdd((int) (myView.getLastX()-Math.round(93*Math.cos(Math.toRadians(mData.ObjectHandlergetAngle()-mStartingAngle))) ), (int) (myView.getLastY()-Math.round(93*Math.sin(Math.toRadians(mData.ObjectHandlergetAngle()-mStartingAngle)))));

            }

            else {
                if (System.currentTimeMillis() - actualTime > 400) {
                    count++;
                    actualTime = System.currentTimeMillis();
                    int xnew = (int) (myView.getLastX()-Math.round(93*Math.cos(Math.toRadians(mData.ObjectHandlergetAngle()-mStartingAngle))));
                    int ynew = (int) (myView.getLastY()-Math.round(93*Math.sin(Math.toRadians(mData.ObjectHandlergetAngle()-mStartingAngle))));
                    myView.newPointAdd(xnew,ynew);
                }
            }

            pmin = 0;
            pmax = 0;
        }
    }
}