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]);
}
}
};
}
|
댓글 없음:
댓글 쓰기