10 using namespace alvar;
15 void filter_none(
double x,
double y,
double *fx,
double *fy) {
40 void filter_des(
double x,
double y,
double *fx,
double *fy) {
48 static bool init=
true;
55 cvmSet(sensor.
H, 0, 0, 1);
56 cvmSet(sensor.
H, 1, 1, 1);
58 cvSetIdentity(sensor.
R, cvScalar(10));
60 cvSetIdentity(kalman.
F);
61 cvmSet(kalman.
F, 0, 2, 1);
62 cvmSet(kalman.
F, 1, 3, 1);
64 cvmSet(kalman.
Q, 0, 0, 0.0001);
65 cvmSet(kalman.
Q, 1, 1, 0.0001);
66 cvmSet(kalman.
Q, 2, 2, 0.000001);
67 cvmSet(kalman.
Q, 3, 3, 0.000001);
69 cvSetIdentity(kalman.
P, cvScalar(100));
71 cvmSet(sensor.
z, 0, 0, x);
72 cvmSet(sensor.
z, 1, 0, y);
73 kalman.
predict_update(&sensor, (
unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
74 *fx = cvmGet(kalman.
x, 0, 0);
75 *fy = cvmGet(kalman.
x, 1, 0);
79 static bool init=
true;
83 for (
int i=0; i<2; i++) {
84 fa[i].setWindowSize(30);
92 virtual void h(CvMat *x_pred, CvMat *_z_pred) {
93 double x = cvmGet(x_pred, 0, 0);
94 double y = cvmGet(x_pred, 1, 0);
95 double dx = cvmGet(x_pred, 2, 0);
96 double dy = cvmGet(x_pred, 3, 0);
97 cvmSet(_z_pred, 0, 0, x);
98 cvmSet(_z_pred, 1, 0, y);
105 virtual void f(CvMat *_x, CvMat *_x_pred,
double dt) {
106 double x = cvmGet(_x, 0, 0);
107 double y = cvmGet(_x, 1, 0);
108 double dx = cvmGet(_x, 2, 0);
109 double dy = cvmGet(_x, 3, 0);
110 cvmSet(_x_pred, 0, 0, x + dt*dx);
111 cvmSet(_x_pred, 1, 0, y + dt*dy);
112 cvmSet(_x_pred, 2, 0, dx);
113 cvmSet(_x_pred, 3, 0, dy);
119 void filter_ekf(
double x,
double y,
double *fx,
double *fy) {
120 static bool init=
true;
126 cvSetIdentity(sensor.
R, cvScalar(100));
128 cvmSet(kalman.
Q, 0, 0, 0.001);
129 cvmSet(kalman.
Q, 1, 1, 0.001);
130 cvmSet(kalman.
Q, 2, 2, 0.01);
131 cvmSet(kalman.
Q, 3, 3, 0.01);
133 cvSetIdentity(kalman.
P, cvScalar(100));
135 cvmSet(sensor.
z, 0, 0, x);
136 cvmSet(sensor.
z, 1, 0, y);
137 kalman.
predict_update(&sensor, (
unsigned long)(cv::getTickCount() / cv::getTickFrequency() * 1000));
138 *fx = cvmGet(kalman.
x, 0, 0);
139 *fy = cvmGet(kalman.
x, 1, 0);
155 "No filter - Press any key to change",
159 "Double Exponential Smoothing",
169 static double dxx = 0.3;
170 static double dyy = 0.7;
171 xx += dxx; yy += dyy;
172 if ((xx >
res) || (xx < 0)) dxx = -dxx;
173 if ((yy >
res) || (yy < 0)) dyy = -dyy;
174 double rx = (rand()*20.0/RAND_MAX)-10.0;
175 double ry = (rand()*20.0/RAND_MAX)-10.0;
184 *x = xx + rx; *y = yy + ry;
187 int main(
int argc,
char *argv[])
192 filename = filename.substr(filename.find_last_of(
'\\') + 1);
193 std::cout <<
"SampleFilter" << std::endl;
194 std::cout <<
"============" << std::endl;
195 std::cout << std::endl;
196 std::cout <<
"Description:" << std::endl;
197 std::cout <<
" This is an example of how to use the 'FilterAverage', 'FilterMedian'," << std::endl;
198 std::cout <<
" 'FilterRunningAverage', 'FilterDoubleExponentialSmoothing', 'Kalman'" << std::endl;
199 std::cout <<
" 'KalmanEkf' and 'FilterArray' filtering classes. First the example" << std::endl;
200 std::cout <<
" shows unfiltered test data with outliers. The data is then filtered" << std::endl;
201 std::cout <<
" using the various filters. Press any key to cycle through the filters." << std::endl;
202 std::cout << std::endl;
203 std::cout <<
"Usage:" << std::endl;
204 std::cout <<
" " << filename << std::endl;
205 std::cout << std::endl;
206 std::cout <<
"Keyboard Shortcuts:" << std::endl;
207 std::cout <<
" any key: cycle through filters" << std::endl;
208 std::cout <<
" q: quit" << std::endl;
209 std::cout << std::endl;
212 IplImage *img = cvCreateImage(cvSize(
res,
res), IPL_DEPTH_8U, 3);
213 cvNamedWindow(
"SampleFilter");
215 cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0);
220 vector<CvPoint> tail;
225 cvPutText(img, filter_names[ii], cvPoint(3,
res - 10), &font, CV_RGB(255, 255, 255));
226 cvCircle(img, cvPoint(
int(x),
int(y)), 2, CV_RGB(0, 255, 255));
227 cvCircle(img, cvPoint(
int(x),
int(y)), 3, CV_RGB(255, 255, 255));
232 for (
size_t iii = 0; iii < tail.size(); iii++) {
233 cvCircle(img, tail[iii], 0, CV_RGB(255, 255, 0));
235 cvCircle(img, fp, 2, CV_RGB(255, 0, 255));
236 cvShowImage(
"SampleFilter", img);
246 cvReleaseImage(&img);
249 catch (
const std::exception &e) {
250 std::cout <<
"Exception: " << e.what() << endl;
253 std::cout <<
"Exception: unknown" << std::endl;
Extended Kalman Filter (EKF) sensor implementation.
void filter_kalman(double x, double y, double *fx, double *fy)
void filter_average(double x, double y, double *fx, double *fy)
void filter_running_average(double x, double y, double *fx, double *fy)
FilterDoubleExponentialSmoothing provides an weighted running average filter
Extended Kalman Filter (EKF) implementation.
char filter_names[nof_filters][64]
KalmanSensorOwn(int _n, int _m)
virtual void h(CvMat *x_pred, CvMat *_z_pred)
void get_measurement(double *x, double *y)
CvMat * predict_update(KalmanSensor *sensor, unsigned long tick)
Predict the Kalman state vector for the given time step and update the state using the Kalman gain...
This file implements multiple filters.
CvMat * P
The error covariance matrix describing the accuracy of the state estimate.
TFSIMD_FORCE_INLINE const tfScalar & y() const
void filters(double x, double y, double *fx, double *fy)
void filter_median(double x, double y, double *fx, double *fy)
CvMat * R
The covariance matrix for the observation noise.
Kalman sensor implementation.
FilterRunningAverage provides an weighted running average filter
void filter_ekf(double x, double y, double *fx, double *fy)
This file implements a versatile Kalman filter.
int main(int argc, char *argv[])
TFSIMD_FORCE_INLINE const tfScalar & x() const
Class for handling an array of filtered values.
CvMat * H
The matrix (m*n) mapping Kalman state vector into this sensor's measurements vector.
CvMat * F
The matrix (n*n) containing the transition model for the internal state.
FilterAverage provides an average filter
virtual double next(double y)
Update the value. All inherited classes need to update value in next().
CvMat * Q
The covariance matrix for the process noise.
CvMat * z
Latest measurement vector (m*1)
CvMat * x
The Kalman state vector (n*1)
void filter_des(double x, double y, double *fx, double *fy)
This file implements the ALVAR exception class.
void filter_array_average(double x, double y, double *fx, double *fy)
void filter_none(double x, double y, double *fx, double *fy)
virtual double next(double y)
Update the value. All inherited classes need to update value in next().
virtual double next(double y)
Update the value. All inherited classes need to update value in next().
virtual void f(CvMat *_x, CvMat *_x_pred, double dt)