2014년 7월 4일 금요일

Android G-Sensor And KalmanFilter Algorithm

칼만필터 알고리즘으로 자이로센서의 흔들림을 보정한다.

KalmanFilter.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
package com.example.kalmanfilter;
 
public class KalmanFilter {
    private double Q = 0.00001;
    private double R = 0.001;
    private double X = 0, P = 1, K;
 
    public KalmanFilter(double initValue) {
        X = initValue;
    }
 
    private void measurementUpdate() {
        K = (P + Q) / (P + Q + R);
        P = R * (P + Q) / (R + P + Q);
    }
 
    public double update(double measurement) {
        measurementUpdate();
        X = X + (measurement - X) * K;
 
        return X;
    }
}
 

KalmanFilterActivity.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
package com.example.kalmanfilter;
 
import android.app.Activity;
import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Bundle;
import android.util.Log;
 
public class KalmanFilterActivity extends Activity {
 
    private static final String TAG = "KalmanFilterActivity";
 
    private Context mContext = null;
 
    // sensor
    private SensorManager mSensorManager;
 
    private Sensor mAccSensor;
    private Sensor mMagSensor;
 
    private static float[] mValues = new float[3];
    private float[] mGravity = new float[3];
    private float[] mGeoMagnetic = new float[3];
 
    private KalmanFilter[] mKalmanFilter = new KalmanFilter[3];
 
    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
 
        mContext = this;
 
        mKalmanFilter[0] = new KalmanFilter(0.0f);
        mKalmanFilter[1] = new KalmanFilter(0.0f);
        mKalmanFilter[2] = new KalmanFilter(0.0f);
 
        mSensorManager = (SensorManager) mContext
                .getSystemService(Context.SENSOR_SERVICE);
        mAccSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        mMagSensor = mSensorManager
                .getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
    }
 
    @Override
    protected void onResume() {
        // TODO Auto-generated method stub
        super.onResume();
 
        mSensorManager.registerListener(mSensorListener, mAccSensor,
                SensorManager.SENSOR_DELAY_GAME);
        mSensorManager.registerListener(mSensorListener, mMagSensor,
                SensorManager.SENSOR_DELAY_GAME);
    }
 
    @Override
    protected void onStop() {
        // TODO Auto-generated method stub
        super.onStop();
 
        mSensorManager.unregisterListener(mSensorListener);
    }
 
    SensorEventListener mSensorListener = new SensorEventListener() {
 
        public void onAccuracyChanged(Sensor sensor, int accuracy) {
        }
 
        public void onSensorChanged(SensorEvent event) {
 
            switch (event.sensor.getType()) {
 
            case Sensor.TYPE_ACCELEROMETER:
                mGravity = event.values.clone();
                break;
 
            case Sensor.TYPE_MAGNETIC_FIELD:
                mGeoMagnetic = event.values.clone();
                break;
 
            case Sensor.TYPE_ORIENTATION:
                mValues = event.values.clone();
                break;
            }
 
            // 둘 다 조사되어 있을 때만
            if (mGravity != null && mGeoMagnetic != null) {
 
                float[] R = new float[16];
                SensorManager
                        .getRotationMatrix(R, null, mGravity, mGeoMagnetic);
 
                float[] values = new float[3];
                SensorManager.getOrientation(R, values);
 
                values[0] = (float) Math.toDegrees(values[0]);
                values[1] = (float) Math.toDegrees(values[1]);
                values[2] = (float) Math.toDegrees(values[2]);
 
                // kalman filter algorithm
                values[0] = (float) mKalmanFilter[0].update(values[0]);
                values[1] = (float) mKalmanFilter[1].update(values[1]);
                values[2] = (float) mKalmanFilter[2].update(values[2]);
 
                Log.d("", "KalmanFilter X, Y, Z : " + values[0] + ", "
                        + values[1] + ", " + values[2]);
            }
        }
    };
}