Android: измерение скорости и расстояния с помощью акселерометра

С тех пор, как у меня появился гуглофон, периодически бродят в голове мысли «а что бы такого забавного сделать с этим самым телефоном?» Поиграв в игрушки с управлением акселерометром, подумал — а что еще можно с помощью этого датчика сотворить? Конечно же, измерить ускорение! И, как следствие, вычислить скорость и пройденный путь. Разумеется, использование лишь акселерометра накладывает ряд ограничений на измеряемое: во-первых, движение должно быть прямолинейное, во-вторых — ориентация аппарата в пространстве не должна меняться, в третьих — желательно откалибровать датчик перед стартом измерения. Сразу скажу — есть способы смягчить эти требования, но об этом потом.

Главный вопрос, как обычно, «зачем?». Зачем это, если есть GPS? Что ж, верное замечание. Однако, GPS работает не везде, а акселерометр — он с собой в телефоне. Например, пробовали поймать спутники в метро?..

С «Зачем» разобрались, переходим к «Как»...

Для того, чтобы реагировать на изменение ускорения, необходимо реализовать где-нибудь интерфейс SensorEventListener. Так как мы еще не придумали, что с ним делать, создадим абстрактный класс

public abstract class Accelerometer implements SensorEventListener {
    protected float lastX;
    protected float lastY;
    protected float lastZ;
    public abstract Point getPoint();
    public void onAccuracyChanged(Sensor arg0, int arg1) {
    }
}

И, заодно, класс для хранения показания счетчика датчика:

public class Point {
    private float x = 0;
    private float y = 0;
    private float z = 0;
    private int cnt = 1;

    public float getX() {
        return x/(float)cnt;
    }

    public float getY() {
        return y/(float)cnt;
    }

    public float getZ() {
        return z/(float)cnt;
    }

    public Point(float x, float y, float z, int cnt) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.cnt = cnt;
    }
   
}

И подумаем, что же делать дальше. Период обновления информации с сенсора в режиме SENSOR_DELAY_GAME — примерно 20 миллисекунд. Это достаточно часто, наша задача такого не требует. С другой стороны, снимая показания реже, мы рискуем попасть на «выбросы», и потерять точность. Логично каким-то образом регулярно получать усредненное значение ускорения, скажем, за последнюю секунду. Хранить массив и вычислять среднее значение — накладно, гораздо проще складывать все получаемые значения и делить на количество. Также, предусмотрим dX, dY, dZ — нашу еще не реализованную калибровку.
Вот что получится:

public class XYZAccelerometer extends Accelerometer {
      private static final int BUFFER_SIZE = 500;
    // calibration
    private  float dX = 0;
    private  float dY = 0;
    private  float dZ = 0;
    // buffer variables
    private float X;
    private float Y;
    private float Z;
    private int cnt = 0;
    
    // returns last SenorEvent parameters
    public Point getLastPoint(){
        return new Point(lastX, lastY, lastZ, 1);
    }

    // returrns parameters, using buffer: average acceleration
    // since last call of getPoint(). 
    public Point getPoint(){
        
        if (cnt == 0){
            return new Point(lastX, lastY, lastZ, 1);
        }
                
        Point p =  new Point(X, Y, Z, cnt);
        
        reset();
        return p;
    }
    
    // resets buffer
    public void reset(){
        cnt = 0;
        X = 0;
        Y = 0;
        Z = 0;
    }
    
    
    public void onSensorChanged(SensorEvent se) {
        float x = se.values[SensorManager.DATA_X] + dX;
        float y = se.values[SensorManager.DATA_Y] + dY;
        float z = se.values[SensorManager.DATA_Z] + dZ;
        
        lastX = x;
        lastY = y;
        lastZ = z;
        
        X+= x;
        Y+= y;
        Z+= z;
        
        if (cnt < BUFFER_SIZE-1) {
            cnt++;
        } else
        {
            reset();
        }
    }

    public  void setdX(float dX) {
        this.dX = dX;
    }

    public  void setdY(float dY) {
        this.dY = dY;
    }

    public  void setdZ(float dZ) {
        this.dZ = dZ;
    }
}

С Вашего позволения, я пропущу описания методов калибровки датчика. Достаточно сказать, что необходимо в течении некоторого времени снимать покания, затем установить у нашего XYZAccelerometer соответствующие dX, dY, dZ. Пренебрегать этой процедурой нельзя, т.к. пока мы спим постоянно действует ускорение свободного падения, и датчик его измеряет.

Для пущей важности, обзаведемся классом для хранения и вычисления параметров движения на интервале:

public class MeasurePoint {
    private float x;
    private float y;
    private float z;
    private float speedBefore;
    private float speedAfter;
    private float distance;
    private float acceleration;
    private long interval;


    public MeasurePoint(float x, float y, float z, float speedBefore, long interval) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.speedBefore = speedBefore;
        this.interval = interval;
        speedAfter = 0;
        calc();
    }
    
    private void calc(){
        //Acceleration as projection of current vector on average
        acceleration = Math.sqrt(this.x*this.x+this.y*this.y*+this.z*this.z);
        float t = ((float)interval / 1000f);
        speedAfter = speedBefore + acceleration * t;
        distance = speedBefore*t + acceleration*t*t/2;
        
    }
// add getters
}

И классом для хранения информации обо всем эксперименте:

public class MeasureData {
    // points from accelerometr
    private LinkedList accData;
    private LinkedList data;
    // timer interval of generating points
    private long interval;

    public MeasureData(long interval) {
        this.interval = interval;
        accData = new LinkedList ();
        data = new LinkedList ();
    }
    
    public void addPoint(Point p){
        accData.add(p);
    }
    
    public void process(){
        
        for(int i = 0; i < accData.size(); ++i){
            Point p = accData.get(i);
            float speed = 0;
            
            if(i > 0){
                speed = data.get(i-1).getSpeedAfter();
            }
            data.add(new MeasurePoint(p.getX(), p.getY(), p.getZ(), speed, interval));
        }
    }
           
    public float getLastSpeed(){
        return data.getLast().getSpeedAfter();
    }
    
    public float getLastSpeedKm(){
        float ms = getLastSpeed();
        return ms*3.6f;
    }
}

Думаю, тут все просто и понятно. Осталось только использовать это в нашей Activity… которой, кстати, еще нет. Начнем с layout:

<serviceLinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
    android:orientation="vertical"
    android:layout_width="fill_parent"
    android:layout_height="fill_parent"
    >
    <serviceButton 
        android:id="@+id/btn"
        android:text="TEST"
        android:layout_width="300px"
        android:layout_height="200px"
        android:onClick="onButtonTest"  />

        <serviceTextView  
    android:id = "@+id/txt"
    android:layout_width="fill_parent" 
    android:layout_height="wrap_content" 
    android:text=":"
    />
<service/LinearLayout>

И код:

public class TestActivity extends Activity {
    static final int TIMER_DONE = 2;
    static final int START = 3;

    private StartCatcher mStartListener;
    private XYZAccelerometer xyzAcc;
    private SensorManager mSensorManager;
    private static final long UPDATE_INTERVAL = 500;
    private static final long MEASURE_TIMES = 20;
    private Timer timer;
    private TextView tv;
    private Button testBtn;
    int counter;
    private MeasureData mdXYZ;


    /** handler for async events*/
    Handler hRefresh = new Handler() {

        @Override
        public void handleMessage(Message msg) {
            switch (msg.what) {
                case TIMER_DONE:

                    onMeasureDone();
                    String es1 = Float.toString(Math.round(mdXYZ.getLastSpeedKm()*100)/100f);
                    tv.append(" END SPEED " + es1  + " \n");
                    enableButtons();
                    break;
                case START:
                    tv.append(" START");
                    timer = new Timer();
                    timer.scheduleAtFixedRate(
                            new TimerTask() {

                                public void run() {
                                    dumpSensor();
                                }
                            },
                            0,
                            UPDATE_INTERVAL);

                    break;
                }
        }
    };

    /** Called when the activity is first created. */
    @Override
    public void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.main);
        tv = (TextView) findViewById(R.id.txt);
        testBtn = (Button) findViewById(R.id.btn);
    }

    @Override
    protected void onResume() {
        super.onResume();
        tv.append("\n ..");
        mSensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        setAccelerometer();
        setStartCatcher();
        mSensorManager.registerListener(xyzAcc,
                mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
                SensorManager.SENSOR_DELAY_GAME);
      
    }

    @Override
    protected void onPause() {
        mSensorManager.unregisterListener(xyzAcc);
        super.onPause();
    }


    public void onButtonTest(View v) {
        disableButtons();
        mdXYZ = new MeasureData(UPDATE_INTERVAL);
        counter = 0;
        tv.setText("");
       hRefresh.sendEmptyMessage(START);  
    }

    void dumpSensor() {
        ++counter;
        mdXYZ.addPoint(xyzAcc.getPoint());
         
        if (counter > MEASURE_TIMES) {
            timer.cancel();
            hRefresh.sendEmptyMessage(TIMER_DONE);
        }

    }

    private void enableButtons() {
        testBtn.setEnabled(true);

    }


    private void setAccelerometer() {
        xyzAcc = new XYZAccelerometer();
        mSensorManager.registerListener(xyzAcc,
                mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
                SensorManager.SENSOR_DELAY_UI);
    }


    private void disableButtons() {
        testBtn.setEnabled(false);
    }

    private void onMeasureDone() {
            mdXYZ.process();
    }
}

Вот и все. Удивительно, но на ровной траектории такой метод дает весьма неплохую точность измерения.

Прилагаю график одного эксперимента: синяя линяя — скорость, расчитанная акселерометром, красная — снимаемая с GPS с максимальной частотой. Черная клякса — скорость по спидометру в конце эксперимента.