Computer Vision
Kalman filter 연습
maetel
2009. 11. 5. 16:12
Kalman filter 연습 코딩
1차원 간단 예제
실행 결과:
2차원 연습
실행 결과:
1차원 간단 예제
// 1-D Kalman filter algorithm exercise
// VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p
#include <iostream>
using namespace std;
int main (int argc, char * const argv[]) {
double groundtruth[] = {1.0, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
double measurement[] = {1.0, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
double transition_noise = 0.1; // covariance of Gaussian noise to control
double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
double x = 0.0, v = 1.0; double cov = 0.5;
double x_p, c_p; // prediction of x and cov
double gain; // Kalman gain
double x_pre, m;
for (int t=0; t<7; t++)
{
// prediction
x_pre = x;
x_p = x + v;
c_p = cov + transition_noise;
m = measurement[t];
// update
gain = c_p / (c_p + measurement_noise);
x = x_p + gain * (m - x_p);
cov = ( 1 - gain ) * c_p;
v = x - x_pre;
cout << t << endl;
cout << "estimation = " << x << endl;
cout << "measurement = " << measurement[t] << endl;
cout << "groundtruth = " << groundtruth[t] << endl;
}
return 0;
}
// VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p
#include <iostream>
using namespace std;
int main (int argc, char * const argv[]) {
double groundtruth[] = {1.0, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
double measurement[] = {1.0, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
double transition_noise = 0.1; // covariance of Gaussian noise to control
double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
double x = 0.0, v = 1.0; double cov = 0.5;
double x_p, c_p; // prediction of x and cov
double gain; // Kalman gain
double x_pre, m;
for (int t=0; t<7; t++)
{
// prediction
x_pre = x;
x_p = x + v;
c_p = cov + transition_noise;
m = measurement[t];
// update
gain = c_p / (c_p + measurement_noise);
x = x_p + gain * (m - x_p);
cov = ( 1 - gain ) * c_p;
v = x - x_pre;
cout << t << endl;
cout << "estimation = " << x << endl;
cout << "measurement = " << measurement[t] << endl;
cout << "groundtruth = " << groundtruth[t] << endl;
}
return 0;
}
실행 결과:
0
estimation = 1
measurement = 1
groundtruth = 1
1
estimation = 2.05
measurement = 2.1
groundtruth = 2
2
estimation = 3.14545
measurement = 3.2
groundtruth = 3.5
3
estimation = 4.70763
measurement = 5.3
groundtruth = 5
4
estimation = 6.76291
measurement = 7.4
groundtruth = 7
5
estimation = 8.50584
measurement = 8.1
groundtruth = 8
6
estimation = 9.9669
measurement = 9.6
groundtruth = 10
logout
[Process completed]
estimation = 1
measurement = 1
groundtruth = 1
1
estimation = 2.05
measurement = 2.1
groundtruth = 2
2
estimation = 3.14545
measurement = 3.2
groundtruth = 3.5
3
estimation = 4.70763
measurement = 5.3
groundtruth = 5
4
estimation = 6.76291
measurement = 7.4
groundtruth = 7
5
estimation = 8.50584
measurement = 8.1
groundtruth = 8
6
estimation = 9.9669
measurement = 9.6
groundtruth = 10
logout
[Process completed]
2차원 연습
// 2-D Kalman filter algorithm exercise
// lym, VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
using namespace std;
int main (int argc, char * const argv[]) {
int step = 7;
IplImage *iplImg = cvCreateImage(cvSize(150, 150), 8, 3);
cvZero(iplImg);
cvNamedWindow("Kalman-2d", 0);
//ground truth of real states
double groundtruth[] = {10.0, 20.0, 35, 50.0, 70.0, 80.0, 100.0, //x-value
10.0, 20.0, 40.0, 55, 65, 80.0, 90.0}; //y-value
//measurement of observed states
double measurement_set[] = {10.0, 21, 32, 53, 74, 81, 96, //x-value
10.0, 19, 42, 56, 66, 78, 88}; //y-value
//covariance of Gaussian noise to control
// double transition_noise[] = { 0.1, 0.0,
// 0.0, 0.1 };
CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(transition_noise, 0, 0, 0.1); //set transition_noise(0,0) to 0.1
cvmSet(transition_noise, 0, 1, 0.0);
cvmSet(transition_noise, 1, 0, 0.0);
cvmSet(transition_noise, 1, 1, 0.1);
//covariance of Gaussian noise to measurement
// double measurement_noise[] = { 0.3, 0.0,
// 0.0, 0.2 };
CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(measurement_noise, 0, 0, 0.3); //set measurement_noise(0,0) to 0.3
cvmSet(measurement_noise, 0, 1, 0.0);
cvmSet(measurement_noise, 1, 0, 0.0);
cvmSet(measurement_noise, 1, 1, 0.2);
CvMat* state = cvCreateMat(2, 1, CV_64FC1); //states to be estimated
CvMat* state_p = cvCreateMat(2, 1, CV_64FC1); //states to be predicted
CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); //motion controls to change states
CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); //measurement of states
CvMat* cov = cvCreateMat(2, 2, CV_64FC1); //covariance to be updated
CvMat* cov_p = cvCreateMat(2, 2, CV_64FC1); //covariance to be predicted
CvMat* gain = cvCreateMat(2, 2, CV_64FC1); //Kalman gain to be updated
// temporary matrices to be used for estimation
CvMat* Kalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* invKalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* I = cvCreateMat(2,2,CV_64FC1);
cvSetIdentity(I); // does not seem to be working properly
// cvSetIdentity (I, cvRealScalar (1));
// check matrix
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
cout << cvmGet(I, i, j) << "\t";
}
cout << endl;
}
// set the initial state
cvmSet(state, 0, 0, 0.0); //x-value //set state(0,0) to 0.0
cvmSet(state, 1, 0, 0.0); //y-value //set state(1,0) to 0.0
// set the initital covariance of state
cvmSet(cov, 0, 0, 0.5); //set cov(0,0) to 0.5
cvmSet(cov, 0, 1, 0.0); //set cov(0,1) to 0.0
cvmSet(cov, 1, 0, 0.0); //set cov(1,0) to 0.0
cvmSet(cov, 1, 0, 0.4); //set cov(1,1) to 0.4
// set the initial control
cvmSet(velocity, 0, 0, 10.0); //x-direction //set velocity(0,0) to 1.0
cvmSet(velocity, 1, 0, 10.0); //y-direction //set velocity(0,0) to 1.0
for (int t=0; t<step; t++)
{
// retain the current state
CvMat* state_out = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvmSet(state_out, 0, 0, cvmGet(state,0,0));
cvmSet(state_out, 1, 0, cvmGet(state,1,0));
// predict
cvAdd(state, velocity, state_p); // state + velocity -> state_p
cvAdd(cov, transition_noise, cov_p); // cov + transition_noise -> cov_p
// measure
cvmSet(measurement, 0, 0, measurement_set[t]); //x-value
cvmSet(measurement, 1, 0, measurement_set[step+t]); //y-value
// estimate Kalman gain
cvAdd(cov_p, measurement_noise, Kalman); // cov_p + measure_noise -> Kalman
cvInvert(Kalman, invKalman); // inv(Kalman) -> invKalman
cvMatMul(cov_p, invKalman, gain); // cov_p * invKalman -> gain
// update the state
CvMat* err = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvSub(measurement, state_p, err); // measurement - state_p -> err
CvMat* adjust = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvMatMul(gain, err, adjust); // gain*err -> adjust
cvAdd(state_p, adjust, state); // state_p + adjust -> state
// update the covariance of states
CvMat* cov_up = cvCreateMat(2, 2, CV_64FC1); // temporary matrix
cvSub(I, gain, cov_up); // I - gain -> cov_up
cvMatMul(cov_up, cov_p, cov); // cov_up *cov_p -> cov
// update the control
cvSub(state, state_out, velocity); // state - state_p -> velocity
// result in colsole
cout << "step " << t << endl;
cout << "estimation = " << cvmGet(state,0,0) << setw(10) << cvmGet(state,1,0) << endl;
cout << "measurement = " << cvmGet(measurement,0,0) << setw(10) << cvmGet(measurement,1,0) << endl;
cout << "groundtruth = " << groundtruth[t] << setw(10) << groundtruth[t+step] << endl;
// result in image
cvCircle(iplImg, cvPoint(cvRound(groundtruth[t]), cvRound(groundtruth[t + step])), 3, cvScalarAll(255));
cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement,0,0)), cvRound(cvmGet(measurement,1,0))), 2, cvScalar(255, 0, 0));
cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 2, cvScalar(0, 0, 255));
cvShowImage("Kalman-2d", iplImg);
cvWaitKey(500);
}
cvWaitKey();
return 0;
}
// lym, VIP lab, Sogang University
// 2009-11-05
// ref. Probabilistic Robotics: 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
using namespace std;
int main (int argc, char * const argv[]) {
int step = 7;
IplImage *iplImg = cvCreateImage(cvSize(150, 150), 8, 3);
cvZero(iplImg);
cvNamedWindow("Kalman-2d", 0);
//ground truth of real states
double groundtruth[] = {10.0, 20.0, 35, 50.0, 70.0, 80.0, 100.0, //x-value
10.0, 20.0, 40.0, 55, 65, 80.0, 90.0}; //y-value
//measurement of observed states
double measurement_set[] = {10.0, 21, 32, 53, 74, 81, 96, //x-value
10.0, 19, 42, 56, 66, 78, 88}; //y-value
//covariance of Gaussian noise to control
// double transition_noise[] = { 0.1, 0.0,
// 0.0, 0.1 };
CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(transition_noise, 0, 0, 0.1); //set transition_noise(0,0) to 0.1
cvmSet(transition_noise, 0, 1, 0.0);
cvmSet(transition_noise, 1, 0, 0.0);
cvmSet(transition_noise, 1, 1, 0.1);
//covariance of Gaussian noise to measurement
// double measurement_noise[] = { 0.3, 0.0,
// 0.0, 0.2 };
CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(measurement_noise, 0, 0, 0.3); //set measurement_noise(0,0) to 0.3
cvmSet(measurement_noise, 0, 1, 0.0);
cvmSet(measurement_noise, 1, 0, 0.0);
cvmSet(measurement_noise, 1, 1, 0.2);
CvMat* state = cvCreateMat(2, 1, CV_64FC1); //states to be estimated
CvMat* state_p = cvCreateMat(2, 1, CV_64FC1); //states to be predicted
CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); //motion controls to change states
CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); //measurement of states
CvMat* cov = cvCreateMat(2, 2, CV_64FC1); //covariance to be updated
CvMat* cov_p = cvCreateMat(2, 2, CV_64FC1); //covariance to be predicted
CvMat* gain = cvCreateMat(2, 2, CV_64FC1); //Kalman gain to be updated
// temporary matrices to be used for estimation
CvMat* Kalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* invKalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* I = cvCreateMat(2,2,CV_64FC1);
cvSetIdentity(I); // does not seem to be working properly
// cvSetIdentity (I, cvRealScalar (1));
// check matrix
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
cout << cvmGet(I, i, j) << "\t";
}
cout << endl;
}
// set the initial state
cvmSet(state, 0, 0, 0.0); //x-value //set state(0,0) to 0.0
cvmSet(state, 1, 0, 0.0); //y-value //set state(1,0) to 0.0
// set the initital covariance of state
cvmSet(cov, 0, 0, 0.5); //set cov(0,0) to 0.5
cvmSet(cov, 0, 1, 0.0); //set cov(0,1) to 0.0
cvmSet(cov, 1, 0, 0.0); //set cov(1,0) to 0.0
cvmSet(cov, 1, 0, 0.4); //set cov(1,1) to 0.4
// set the initial control
cvmSet(velocity, 0, 0, 10.0); //x-direction //set velocity(0,0) to 1.0
cvmSet(velocity, 1, 0, 10.0); //y-direction //set velocity(0,0) to 1.0
for (int t=0; t<step; t++)
{
// retain the current state
CvMat* state_out = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvmSet(state_out, 0, 0, cvmGet(state,0,0));
cvmSet(state_out, 1, 0, cvmGet(state,1,0));
// predict
cvAdd(state, velocity, state_p); // state + velocity -> state_p
cvAdd(cov, transition_noise, cov_p); // cov + transition_noise -> cov_p
// measure
cvmSet(measurement, 0, 0, measurement_set[t]); //x-value
cvmSet(measurement, 1, 0, measurement_set[step+t]); //y-value
// estimate Kalman gain
cvAdd(cov_p, measurement_noise, Kalman); // cov_p + measure_noise -> Kalman
cvInvert(Kalman, invKalman); // inv(Kalman) -> invKalman
cvMatMul(cov_p, invKalman, gain); // cov_p * invKalman -> gain
// update the state
CvMat* err = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvSub(measurement, state_p, err); // measurement - state_p -> err
CvMat* adjust = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvMatMul(gain, err, adjust); // gain*err -> adjust
cvAdd(state_p, adjust, state); // state_p + adjust -> state
// update the covariance of states
CvMat* cov_up = cvCreateMat(2, 2, CV_64FC1); // temporary matrix
cvSub(I, gain, cov_up); // I - gain -> cov_up
cvMatMul(cov_up, cov_p, cov); // cov_up *cov_p -> cov
// update the control
cvSub(state, state_out, velocity); // state - state_p -> velocity
// result in colsole
cout << "step " << t << endl;
cout << "estimation = " << cvmGet(state,0,0) << setw(10) << cvmGet(state,1,0) << endl;
cout << "measurement = " << cvmGet(measurement,0,0) << setw(10) << cvmGet(measurement,1,0) << endl;
cout << "groundtruth = " << groundtruth[t] << setw(10) << groundtruth[t+step] << endl;
// result in image
cvCircle(iplImg, cvPoint(cvRound(groundtruth[t]), cvRound(groundtruth[t + step])), 3, cvScalarAll(255));
cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement,0,0)), cvRound(cvmGet(measurement,1,0))), 2, cvScalar(255, 0, 0));
cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 2, cvScalar(0, 0, 255));
cvShowImage("Kalman-2d", iplImg);
cvWaitKey(500);
}
cvWaitKey();
return 0;
}
실행 결과:
step 0
estimation = 10 10
measurement = 10 10
groundtruth = 10 10
step 1
estimation = 20.5 19.6263
measurement = 21 19
groundtruth = 20 20
step 2
estimation = 31.4545 35.5006
measurement = 32 42
groundtruth = 35 40
step 3
estimation = 47.0763 53.7411
measurement = 53 56
groundtruth = 50 55
step 4
estimation = 67.6291 69.0154
measurement = 74 66
groundtruth = 70 65
step 5
estimation = 85.0584 81.1424
measurement = 81 78
groundtruth = 80 80
step 6
estimation = 99.669 90.634
measurement = 96 88
groundtruth = 100 90
estimation = 10 10
measurement = 10 10
groundtruth = 10 10
step 1
estimation = 20.5 19.6263
measurement = 21 19
groundtruth = 20 20
step 2
estimation = 31.4545 35.5006
measurement = 32 42
groundtruth = 35 40
step 3
estimation = 47.0763 53.7411
measurement = 53 56
groundtruth = 50 55
step 4
estimation = 67.6291 69.0154
measurement = 74 66
groundtruth = 70 65
step 5
estimation = 85.0584 81.1424
measurement = 81 78
groundtruth = 80 80
step 6
estimation = 99.669 90.634
measurement = 96 88
groundtruth = 100 90