-
Notifications
You must be signed in to change notification settings - Fork 155
/
GPSAccKalmanFilter.java
169 lines (143 loc) · 4.83 KB
/
GPSAccKalmanFilter.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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
package mad.location.manager.lib.Filters;
import android.util.Log;
import mad.location.manager.lib.Commons.Matrix;
/**
* Created by lezh1k on 2/13/18.
*/
public class GPSAccKalmanFilter {
public static final String TAG = "GPSAccKalmanFilter";
private double m_timeStampMsPredict;
private double m_timeStampMsUpdate;
private int m_predictCount;
private KalmanFilter m_kf;
private double m_accSigma;
private boolean m_useGpsSpeed;
private double mVelFactor = 1.0;
private double mPosFactor = 1.0;
public GPSAccKalmanFilter(boolean useGpsSpeed,
double x, double y,
double xVel, double yVel,
double accDev, double posDev,
double timeStampMs,
double velFactor,
double posFactor) {
int mesDim = useGpsSpeed ? 4 : 2;
m_useGpsSpeed = useGpsSpeed;
m_kf = new KalmanFilter(4, mesDim, 2);
m_timeStampMsPredict = m_timeStampMsUpdate = timeStampMs;
m_accSigma = accDev;
m_predictCount = 0;
m_kf.Xk_k.setData(x, y, xVel, yVel);
m_kf.H.setIdentityDiag(); //state has 4d and measurement has 4d too. so here is identity
m_kf.Pk_k.setIdentity();
m_kf.Pk_k.scale(posDev);
mVelFactor = velFactor;
mPosFactor = posFactor;
}
private void rebuildF(double dtPredict) {
double f[] = {
1.0, 0.0, dtPredict, 0.0,
0.0, 1.0, 0.0, dtPredict,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0
};
m_kf.F.setData(f);
}
private void rebuildU(double xAcc,
double yAcc) {
m_kf.Uk.setData(xAcc, yAcc);
}
private void rebuildB(double dtPredict) {
double dt2 = 0.5*dtPredict*dtPredict;
double b[] = {
dt2, 0.0,
0.0, dt2,
dtPredict, 0.0,
0.0, dtPredict
};
m_kf.B.setData(b);
}
private void rebuildR(double posSigma, double velSigma) {
posSigma *= mPosFactor;
velSigma *= mVelFactor;
Log.i(TAG, "rebuildR: { " +
"velSigma : " + velSigma +
", posSigma : " + posSigma +
", velFactor : " + mVelFactor +
", posFactor :" + mPosFactor +
"}");
if (m_useGpsSpeed) {
double R[] = {
posSigma, 0.0, 0.0, 0.0,
0.0, posSigma, 0.0, 0.0,
0.0, 0.0, velSigma, 0.0,
0.0, 0.0, 0.0, velSigma
};
m_kf.R.setData(R);
} else {
m_kf.R.setIdentity();
m_kf.R.scale(posSigma);
}
}
private void rebuildQ(double dtUpdate,
double accDev) {
// now we use predictCount. but maybe there is way to use dtUpdate.
// m_kf.Q.setIdentity();
// m_kf.Q.scale(accSigma * dtUpdate);
double velDev = accDev * m_predictCount;
double posDev = velDev * m_predictCount / 2;
double covDev = velDev * posDev;
double posSig = posDev * posDev;
double velSig = velDev * velDev;
double Q[] = {
posSig, 0.0, covDev, 0.0,
0.0, posSig, 0.0, covDev,
covDev, 0.0, velSig, 0.0,
0.0, covDev, 0.0, velSig
};
m_kf.Q.setData(Q);
}
public void predict(double timeNowMs,
double xAcc,
double yAcc) {
double dtPredict = (timeNowMs - m_timeStampMsPredict) / 1000.0;
double dtUpdate = (timeNowMs - m_timeStampMsUpdate) / 1000.0;
rebuildF(dtPredict);
rebuildB(dtPredict);
rebuildU(xAcc, yAcc);
++m_predictCount;
rebuildQ(dtUpdate, m_accSigma);
m_timeStampMsPredict = timeNowMs;
m_kf.predict();
Matrix.matrixCopy(m_kf.Xk_km1, m_kf.Xk_k);
}
public void update(double timeStamp,
double x,
double y,
double xVel,
double yVel,
double posDev,
double velErr) {
m_predictCount = 0;
m_timeStampMsUpdate = timeStamp;
rebuildR(posDev, velErr);
if (m_useGpsSpeed) {
m_kf.Zk.setData(x, y, xVel, yVel);
} else {
m_kf.Zk.setData(x, y);
}
m_kf.update();
}
public double getCurrentX() {
return m_kf.Xk_k.data[0][0];
}
public double getCurrentY() {
return m_kf.Xk_k.data[1][0];
}
public double getCurrentXVel() {
return m_kf.Xk_k.data[2][0];
}
public double getCurrentYVel() {
return m_kf.Xk_k.data[3][0];
}
}