使用IMU的室内跟踪应用程序

时间:2017-12-07 10:35:22

标签: android tracking android-sensors indoor-positioning-system

我正在尝试使用IMU创建Android室内跟踪应用。

现在我的应用程序基于加速度计和软件传感器ROTATION_VECTOR,它是通过融合来自ACCELEROMETER,GEOMAGNETIC_FIELD和GYROSCOPE的数据创建的。我使用加速度计检测步骤,使用ROTATION_VECTOR传感器进行定位。当检测到步骤时,我从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;
        }
    }
}

1 个答案:

答案 0 :(得分:0)

@Override
public void onSensorChanged(SensorEvent event) {


    final float alpha = 0.97f;

    synchronized (this) {
        if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {

            mGravity[0] = alpha * mGravity[0] + (1 - alpha)
                    * event.values[0];
            mGravity[1] = alpha * mGravity[1] + (1 - alpha)
                    * event.values[1];
            mGravity[2] = alpha * mGravity[2] + (1 - alpha)
                    * event.values[2];
        }

        if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {

            mGeomagnetic[0] = alpha * mGeomagnetic[0] + (1 - alpha)
                    * event.values[0];
            mGeomagnetic[1] = alpha * mGeomagnetic[1] + (1 - alpha)
                    * event.values[1];
            mGeomagnetic[2] = alpha * mGeomagnetic[2] + (1 - alpha)
                    * event.values[2];

        }

        float R[] = new float[9];
        float I[] = new float[9];
        boolean success = SensorManager.getRotationMatrix(R, I, mGravity,
                mGeomagnetic);
        if (success) {
            float orientation[] = new float[3];
            SensorManager.getOrientation(R, orientation);
            float azimuth = orientation[0]; //in radians
            azimuth = azimuth * 360 / (2 * (float) Math.PI); // -180 to 180


        }
    }
}