-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathexample_ekf.cpp
249 lines (216 loc) · 9.09 KB
/
example_ekf.cpp
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
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
/**
* An example of OpenCX, especially for 'ekf.hpp'
*/
#include "opencx.hpp"
#include <iostream>
using namespace std;
// An interface for 2D pose and velocity estimation from GPS data
class GPSLocalizer
{
public:
virtual ~GPSLocalizer() { }
virtual bool applyGPS(const cv::Vec2d& gps, double time) = 0;
virtual cv::Vec3d getPose() const = 0;
virtual cv::Vec2d getVelocity() const = 0;
virtual bool setControlNoise(double noise) = 0;
virtual bool setGPSNoise(const cv::Vec2d& noise, const cv::Vec2d& offset) = 0;
};
// GPS localization using 'cx::EKF'
// - The state variable: $\mathbf{x} = [ x, y, \theta, v, w ]^\top$
// - The state transition model: The constant velocity model
// - The observation model: GPS data with off-centered installation
// - Reference: Choi and Kim, Leveraging Localization Accuracy with Off-centered GPS, IEEE T-ITS, Vol. 21, No. 6, 2020 <a href="http://rit.kaist.ac.kr/home/International_Journal?action=AttachFile&do=get&target=paper_0425.pdf">PDF</a> <a href="http://doi.org/10.1109/TITS.2019.2915108">DOI</a>
class GPSLocalizerCx : public GPSLocalizer, public cx::EKF
{
public:
GPSLocalizerCx()
{
initialize(cv::Mat::zeros(5, 1, CV_64F), cv::Mat::eye(5, 5, CV_64F));
m_ctrl_noise = 1;
m_gps_noise = cv::Mat::eye(2, 2, CV_64F);
m_prev_time = -1;
}
virtual bool applyGPS(const cv::Vec2d& gps, double timestamp)
{
double dt = 0;
if (m_prev_time > 0) dt = timestamp - m_prev_time;
if (dt > DBL_EPSILON) predict(dt);
m_prev_time = timestamp;
return correct(gps);
}
virtual cv::Vec3d getPose() const { return m_state_vec.rowRange(0, 3); }
virtual cv::Vec2d getVelocity() const { return m_state_vec.rowRange(3, 5); }
virtual bool setControlNoise(double noise)
{
m_ctrl_noise = noise;
return true;
}
virtual bool setGPSNoise(const cv::Vec2d& noise, const cv::Vec2d& offset = cv::Vec2d(0, 0))
{
m_gps_noise = 0;
m_gps_noise.at<double>(0, 0) = noise(0) * noise(0);
m_gps_noise.at<double>(1, 1) = noise(1) * noise(1);
m_gps_offset = offset;
return true;
}
protected:
virtual cv::Mat transitFunc(const cv::Mat& state, const cv::Mat& control, cv::Mat& jacobian, cv::Mat& noise)
{
const double dt = control.at<double>(0);
const double vt = state.at<double>(3) * dt;
const double wt = state.at<double>(4) * dt;
const double c = cos(state.at<double>(2) + wt / 2), s = sin(state.at<double>(2) + wt / 2);
cv::Mat func = (cv::Mat_<double>(5, 1) <<
state.at<double>(0) + vt * c,
state.at<double>(1) + vt * s,
state.at<double>(2) + wt,
state.at<double>(3),
state.at<double>(4));
jacobian = (cv::Mat_<double>(5, 5) <<
1, 0, -vt * s, dt * c, -vt * dt * s / 2,
0, 1, vt * c, dt * s, vt * dt * c / 2,
0, 0, 1, 0, dt,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1);
cv::Mat W = (cv::Mat_<double>(5, 1) <<
state.at<double>(3) * c - vt * s,
state.at<double>(3) * s + vt * c,
state.at<double>(4),
DBL_EPSILON,
DBL_EPSILON);
noise = W * m_ctrl_noise * W.t();
return func;
}
virtual cv::Mat observeFunc(const cv::Mat& state, const cv::Mat& measure, cv::Mat& jacobian, cv::Mat& noise)
{
const double c = cos(state.at<double>(2) + m_gps_offset(1)), s = sin(state.at<double>(2) + m_gps_offset(1));
cv::Mat func = (cv::Mat_<double>(2, 1) <<
state.at<double>(0) + m_gps_offset(0) * c,
state.at<double>(1) + m_gps_offset(0) * s);
jacobian = (cv::Mat_<double>(2, 5) <<
1, 0, -m_gps_offset(0) * s, 0, 0,
0, 1, m_gps_offset(0) * c, 0, 0);
noise = m_gps_noise;
return func;
}
double m_ctrl_noise;
cv::Mat m_gps_noise;
cv::Vec2d m_gps_offset;
double m_prev_time;
};
// GPS localization using 'cv::KalmanFilter' in OpenCV
// - Same state and models with 'GPSLocalizerCx'. Please compare both implementation how to apply prediction and correction.
class GPSLocalizerCv : public GPSLocalizer, public cv::KalmanFilter
{
public:
GPSLocalizerCv()
{
init(5, 2, 1, CV_64F); // The dimension of state, measurement, and control
statePre = cv::Mat::zeros(5, 1, CV_64F);
statePost = cv::Mat::zeros(5, 1, CV_64F);
errorCovPre = cv::Mat::eye(5, 5, CV_64F);
errorCovPost = cv::Mat::eye(5, 5, CV_64F);
transitionMatrix = cv::Mat::eye(3, 3, CV_64F);
processNoiseCov = cv::Mat::eye(3, 3, CV_64F);
measurementMatrix = cv::Mat::eye(2, 3, CV_64F);
measurementNoiseCov = cv::Mat::eye(2, 2, CV_64F);
m_ctrl_noise = 1;
m_prev_time = -1;
}
virtual bool applyGPS(const cv::Vec2d& gps, double timestamp)
{
double dt = 0;
if (m_prev_time > 0) dt = timestamp - m_prev_time;
if (dt > DBL_EPSILON)
{
// Predict the state
const double vt = statePost.at<double>(3) * dt;
const double wt = statePost.at<double>(4) * dt;
const double c = cos(statePost.at<double>(2) + wt / 2), s = sin(statePost.at<double>(2) + wt / 2);
transitionMatrix = (cv::Mat_<double>(5, 5) <<
1, 0, -vt * s, dt * c, -vt * dt * s / 2,
0, 1, vt * c, dt * s, vt * dt * c / 2,
0, 0, 1, 0, dt,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1);
cv::Mat W = (cv::Mat_<double>(5, 1) <<
statePost.at<double>(3) * c - vt * s,
statePost.at<double>(3) * s + vt * c,
statePost.at<double>(4),
DBL_EPSILON,
DBL_EPSILON);
processNoiseCov = W * m_ctrl_noise * W.t();
predict();
statePre = (cv::Mat_<double>(5, 1) <<
statePost.at<double>(0) + vt * c,
statePost.at<double>(1) + vt * s,
statePost.at<double>(2) + wt,
statePost.at<double>(3),
statePost.at<double>(4));
}
m_prev_time = timestamp;
// Correct the state
const double c = cos(statePre.at<double>(2) + m_gps_offset(1)), s = sin(statePre.at<double>(2) + m_gps_offset(1));
measurementMatrix = (cv::Mat_<double>(2, 5) <<
1, 0, -m_gps_offset(0) * s, 0, 0,
0, 1, m_gps_offset(0) * c, 0, 0);
// 'measurementNoiseCov' is constant, so it is not necessary to calculate again.
correct(cv::Mat(gps));
cv::Mat expectation = (cv::Mat_<double>(2, 1) <<
statePre.at<double>(0) + m_gps_offset(0) * c,
statePre.at<double>(1) + m_gps_offset(0) * s);
temp5 = cv::Mat(gps) - expectation;
statePost = statePre + gain * temp5;
return true;
}
virtual cv::Vec3d getPose() const { return statePost.rowRange(0, 3); }
virtual cv::Vec2d getVelocity() const { return statePost.rowRange(3, 5); }
virtual bool setControlNoise(double noise)
{
m_ctrl_noise = noise;
return true;
}
virtual bool setGPSNoise(const cv::Vec2d& noise, const cv::Vec2d& offset = cv::Vec2d(0, 0))
{
measurementNoiseCov = 0;
measurementNoiseCov.at<double>(0, 0) = noise(0) * noise(0);
measurementNoiseCov.at<double>(1, 1) = noise(1) * noise(1);
m_gps_offset = offset;
return true;
}
protected:
double m_ctrl_noise;
cv::Vec2d m_gps_offset;
double m_prev_time;
};
int testGPSLocalization(GPSLocalizer* localizer, double gps_noise = 0.3, const cv::Vec2d gps_offset = cv::Vec2d(1, 0), double interval = 0.1, double velocity = 1)
{
if (localizer == nullptr) return -1;
if (!localizer->setGPSNoise({ gps_noise, gps_noise }, gps_offset)) return -1;
printf("| Time [sec] | GPS Data [m] | Pose [m] [deg] | Velocity [m/s] [deg/s] |\n");
printf("| ---------- | ------------ | -------------- | ---------------------- |\n");
for (double t = interval; t < 10; t += interval)
{
cv::Vec3d truth(velocity * t, 1, 0); // Going straight from (0, 1, 0)
// Simulate and apply noisy GPS data
cv::Vec2d gps(truth(0), truth(1));
gps(0) += gps_offset(0) * cos(truth(2) + gps_offset(1)) + cv::theRNG().gaussian(gps_noise);
gps(1) += gps_offset(0) * sin(truth(2) + gps_offset(1)) + cv::theRNG().gaussian(gps_noise);
if (!localizer->applyGPS(gps, t)) return -1;
// Print the current pose
cv::Vec3d pose = localizer->getPose();
cv::Vec2d velocity = localizer->getVelocity();
printf("| %.1f | %.3f, %.3f | %.3f, %.3f, %.1f | %.3f, %.1f |\n", t, gps(0), gps(1), pose(0), pose(1), cx::cvtRad2Deg(pose(2)), velocity(0), cx::cvtRad2Deg(velocity(1)));
}
return 0;
}
int main()
{
printf("\n### Test GPSLocalizerCx with cx::EKF\n");
GPSLocalizerCx localizer_cx;
if (testGPSLocalization(&localizer_cx) < 0) return -1;
printf("\n### Test GPSLocalizerCv with cv::KalmanFilter\n");
GPSLocalizerCv localizer_cv;
if (testGPSLocalization(&localizer_cv) < 0) return -1;
return 0;
}