Computer Vision
Kalman filtering for SLAM 연습
maetel
2010. 1. 15. 11:55
1차원 SLAM을 위한 Kalman filter 간단 예제
console:
2차원 SLAM을 위한 Kalman filter 간단 예제
void cvGEMM(const CvArr* src1, const CvArr* src2, double alpha, const CvArr* src3, double beta, CvArr* dst, int tABC=0)
define cvMatMulAdd(src1, src2, src3, dst ) cvGEMM(src1, src2, 1, src3, 1, dst, 0 )define cvMatMul(src1, src2, dst ) cvMatMulAdd(src1, src2, 0, dst)¶
define cvMatMulAdd(src1, src2, src3, dst ) cvGEMM(src1, src2, 1, src3, 1, dst, 0 )define cvMatMul(src1, src2, dst ) cvMatMulAdd(src1, src2, 0, dst)¶
// 1-D SLAM with Kalman Filter
// VIP lab, Sogang University
// 2010-01-14
// ref. Probabilistic Robotics 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
#include <cmath>
using namespace std;
#define num_landmarks 10
#define num_dim (num_landmarks + 2)
#define step 100
#define width 1000
#define height 200
int main (int argc, char * const argv[]) {
srand(time(NULL));
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[num_landmarks];// = { 200, 600, 400 }; // x-position
for( int n = 0; n < num_landmarks; n++ )
{
landmark[n] = width * uniform_random();
}
// set the initial state
double rob_pos = 25.0; // initial robot position
double rob_vel = 10.0; // initial robot velocity
// set the initial covariance of the state
double rob_pos_cov = 0.01; // covariance of noise to robot position
double rob_vel_cov = 0.01; // covariance of noise to robot velocity
double obs_cov = 900; // covarriance of noise to measurement of landmarks
double xGroundTruth, vGroundTruth;
xGroundTruth = rob_pos;
vGroundTruth = rob_vel;
IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
cvZero(iplImg);
cvNamedWindow("SLAM-1d", 0);
// H matrix
int Hrow = num_landmarks;
int Hcol = num_landmarks + 2;
CvMat* H = cvCreateMat(Hrow, Hcol, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int row = 0; row < Hrow; row++)
{
cvmSet(H, row, 0, -1.0);
cvmSet(H, row, row+2, 1.0);
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H ; uncertainty of control
CvMat* Q = cvCreateMat(Hrow, Hrow, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
int Grow = num_landmarks + 2;
int Gcol = Grow;
CvMat* G = cvCreateMat(Grow, Gcol, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous position
cvmSet(G, 0, 1, 1.0); // velocity
for (int row = 1; row < Grow; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to H ; uncertainty of observation
CvMat* R = cvCreateMat(Grow, Grow, CV_64FC1); // 5x5
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_pos_cov);
cvmSet(R, 1, 1, rob_vel_cov);
displayMatrix(R, "R matrix");
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state to be predicted
CvMat* u = cvCreateMat(1, 1, CV_64FC1); // control vector
cvmSet(u, 0, 0, 1.0); // set u(0,0) to 1.0, the constant velocity here
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, num_landmarks, CV_64FC1); // K matrix // Kalman gain
CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))
CvMat* obs = cvCreateMat(num_landmarks, 1, CV_64FC1); // observation for each landmark
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_pos + sqrt(rob_pos_cov)*gaussian_random()); // set mu(0,0) to "rob_pos"
cvmSet(mu, 1, 0, rob_vel + sqrt(rob_vel_cov)*gaussian_random()); // set mu(0,0) to "rob_vel"
for(int n = 0; n < num_landmarks; n++)
{
// cvmSet(mu, n+2, 0, landmark[n] + sqrt(obs_cov)*gaussian_random());
cvmSet(mu, n+2, 0, landmark[n]);
}
displayMatrix(mu, "mu vector");
// initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_cov));
displayMatrix(sigma, "sigma matrix");
// matrices to be used in calculation
CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1); // 5x5
cvTranspose(G, Gt); // transpose(G) -> Gt
CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1); // 5x5 * 5x5
CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 5x5
CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 5xnum_landmarks
cvTranspose(H, Ht); // transpose(H) -> Ht
CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1); // 5x5 * 5xnum_landmarks
CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 5x5 * 5xnum_landmarks
CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // num_landmarksx1
CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 5x1
CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 5xnum_landmarks * num_landmarksx5
CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 5x5 identity matrix
cvSetIdentity(I);
CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 5x5
for (int t = 0; t < step; t++)
{
cout << endl << "step " << t << endl;
cvZero(iplImg);
// predict
// predict the state (ref. L2, KF algorithm, 42p)
cvMatMul(G, mu, mu_p); // G * mu -> mu_p
// displayMatrix(mu_p, "mu_p vector");
// predict the covariance of the state (ref. L3, KF algorithm, 42p)
cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
// displayMatrix(sigma_p, "sigma_p matrix");
// estimate Kalman gain (ref. L4, KF algorithm, 42p)
cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
// displayMatrix(HsigmaHtplusQ, "H*sigma*Ht + Q matrix");
cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
displayMatrix(invGain, "invGain matrix");
cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
// displayMatrix(K, "K matrix");
// measure
xGroundTruth += vGroundTruth;
cvZero(rob_ground);
cvmSet(rob_ground, 0, 0, xGroundTruth);
cvmSet(rob_ground, 1, 0, vGroundTruth);
for( int n = 0; n < num_landmarks; n++ )
{
cvmSet(rob_ground, n + 2, 0, landmark[n]);
}
for(int n = 0; n < num_landmarks; n++)
{
double rn = sqrt(obs_cov) * gaussian_random();
cvmSet(delta, n, 0, rn);
}
// displayMatrix(delta, "delta vector; measurement noise");
displayMatrix(rob_ground, "rob_ground");
cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
cvAdd(Hx, delta, z); // Hx + delta -> z
displayMatrix(z, "z vector");
// update the state with Kalman gain (ref. L5, KF algorithm, 42p)
cvMatMul(H, mu_p, Hmu); // H * mu_p -> Hmu
cvSub(z, Hmu, miss); // z - Hmu -> miss
cvMatMul(K, miss, adjust); // K * miss -> adjust
cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
displayMatrix(mu, "mu vector");
// update the coariance of the state (ref. L6, KF algorith, 42p)
cvMatMul(K, H, KH); // K * H -> KH
cvSub(I, KH, change); // I - KH -> change
cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
displayMatrix(sigma, "sigma matrix");
// result in console
cout << "landmarks = " << landmark[0] << setw(10) << landmark[1] << setw(10) << landmark[2] << setw(10) << endl;
cout << "robot position = " << cvmGet(mu, 0, 0) << endl;
// cout << "measurement = " << cvmGet(z,0,0) << setw(10) << cvmGet(z,1,0) << setw(10) << cvmGet(z,2,0) << endl;
for( int n = 0; n < num_landmarks; n++ )
{
cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
}
cout << "observation = " << cvmGet(obs,0,0) << setw(10) << cvmGet(obs,1,0) << setw(10) << cvmGet(obs,2,0) << endl;
cout<< "estimation = " << cvmGet(mu,2,0) << setw(10) << cvmGet(mu,3,0) << setw(10) << cvmGet(mu,4,0) << endl;
// result in image
// ground truth of robot position
cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(height/2)), 1, cvScalar(100, 0, 255));
// robot position, purple
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(height/2)), 3, cvScalar(255, 0, 100));
// uncertainty of robot position, purple line
cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
for( int index = 0; index < num_landmarks; index++ )
{
// landmarks, white
cvCircle(iplImg, cvPoint(cvRound(landmark[index]), cvRound(height/2)), 3, cvScalarAll(255));
// observation, yellow
// cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,index,0)), cvRound(height/2)), 2, cvScalar(0, 200, 255));
// estimation, green
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0)), cvRound(height/2)), 2, cvScalar(50, 255, 0));
// uncertainty of estimation, green line
cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);
}
cvShowImage("SLAM-1d", iplImg);
cvWaitKey(0);
}
cvWaitKey();
return 0;
}
// VIP lab, Sogang University
// 2010-01-14
// ref. Probabilistic Robotics 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
#include <cmath>
using namespace std;
#define num_landmarks 10
#define num_dim (num_landmarks + 2)
#define step 100
#define width 1000
#define height 200
int main (int argc, char * const argv[]) {
srand(time(NULL));
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[num_landmarks];// = { 200, 600, 400 }; // x-position
for( int n = 0; n < num_landmarks; n++ )
{
landmark[n] = width * uniform_random();
}
// set the initial state
double rob_pos = 25.0; // initial robot position
double rob_vel = 10.0; // initial robot velocity
// set the initial covariance of the state
double rob_pos_cov = 0.01; // covariance of noise to robot position
double rob_vel_cov = 0.01; // covariance of noise to robot velocity
double obs_cov = 900; // covarriance of noise to measurement of landmarks
double xGroundTruth, vGroundTruth;
xGroundTruth = rob_pos;
vGroundTruth = rob_vel;
IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
cvZero(iplImg);
cvNamedWindow("SLAM-1d", 0);
// H matrix
int Hrow = num_landmarks;
int Hcol = num_landmarks + 2;
CvMat* H = cvCreateMat(Hrow, Hcol, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int row = 0; row < Hrow; row++)
{
cvmSet(H, row, 0, -1.0);
cvmSet(H, row, row+2, 1.0);
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H ; uncertainty of control
CvMat* Q = cvCreateMat(Hrow, Hrow, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
int Grow = num_landmarks + 2;
int Gcol = Grow;
CvMat* G = cvCreateMat(Grow, Gcol, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous position
cvmSet(G, 0, 1, 1.0); // velocity
for (int row = 1; row < Grow; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to H ; uncertainty of observation
CvMat* R = cvCreateMat(Grow, Grow, CV_64FC1); // 5x5
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_pos_cov);
cvmSet(R, 1, 1, rob_vel_cov);
displayMatrix(R, "R matrix");
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state to be predicted
CvMat* u = cvCreateMat(1, 1, CV_64FC1); // control vector
cvmSet(u, 0, 0, 1.0); // set u(0,0) to 1.0, the constant velocity here
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, num_landmarks, CV_64FC1); // K matrix // Kalman gain
CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))
CvMat* obs = cvCreateMat(num_landmarks, 1, CV_64FC1); // observation for each landmark
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_pos + sqrt(rob_pos_cov)*gaussian_random()); // set mu(0,0) to "rob_pos"
cvmSet(mu, 1, 0, rob_vel + sqrt(rob_vel_cov)*gaussian_random()); // set mu(0,0) to "rob_vel"
for(int n = 0; n < num_landmarks; n++)
{
// cvmSet(mu, n+2, 0, landmark[n] + sqrt(obs_cov)*gaussian_random());
cvmSet(mu, n+2, 0, landmark[n]);
}
displayMatrix(mu, "mu vector");
// initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_cov));
displayMatrix(sigma, "sigma matrix");
// matrices to be used in calculation
CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1); // 5x5
cvTranspose(G, Gt); // transpose(G) -> Gt
CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1); // 5x5 * 5x5
CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 5x5
CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 5xnum_landmarks
cvTranspose(H, Ht); // transpose(H) -> Ht
CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1); // 5x5 * 5xnum_landmarks
CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // num_landmarksxnum_landmarks
CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 5x5 * 5xnum_landmarks
CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // num_landmarksx5 * 5x1
CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // num_landmarksx1
CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 5x1
CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 5xnum_landmarks * num_landmarksx5
CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 5x5 identity matrix
cvSetIdentity(I);
CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 5x5
for (int t = 0; t < step; t++)
{
cout << endl << "step " << t << endl;
cvZero(iplImg);
// predict
// predict the state (ref. L2, KF algorithm, 42p)
cvMatMul(G, mu, mu_p); // G * mu -> mu_p
// displayMatrix(mu_p, "mu_p vector");
// predict the covariance of the state (ref. L3, KF algorithm, 42p)
cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
// displayMatrix(sigma_p, "sigma_p matrix");
// estimate Kalman gain (ref. L4, KF algorithm, 42p)
cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
// displayMatrix(HsigmaHtplusQ, "H*sigma*Ht + Q matrix");
cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
displayMatrix(invGain, "invGain matrix");
cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
// displayMatrix(K, "K matrix");
// measure
xGroundTruth += vGroundTruth;
cvZero(rob_ground);
cvmSet(rob_ground, 0, 0, xGroundTruth);
cvmSet(rob_ground, 1, 0, vGroundTruth);
for( int n = 0; n < num_landmarks; n++ )
{
cvmSet(rob_ground, n + 2, 0, landmark[n]);
}
for(int n = 0; n < num_landmarks; n++)
{
double rn = sqrt(obs_cov) * gaussian_random();
cvmSet(delta, n, 0, rn);
}
// displayMatrix(delta, "delta vector; measurement noise");
displayMatrix(rob_ground, "rob_ground");
cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
cvAdd(Hx, delta, z); // Hx + delta -> z
displayMatrix(z, "z vector");
// update the state with Kalman gain (ref. L5, KF algorithm, 42p)
cvMatMul(H, mu_p, Hmu); // H * mu_p -> Hmu
cvSub(z, Hmu, miss); // z - Hmu -> miss
cvMatMul(K, miss, adjust); // K * miss -> adjust
cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
displayMatrix(mu, "mu vector");
// update the coariance of the state (ref. L6, KF algorith, 42p)
cvMatMul(K, H, KH); // K * H -> KH
cvSub(I, KH, change); // I - KH -> change
cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
displayMatrix(sigma, "sigma matrix");
// result in console
cout << "landmarks = " << landmark[0] << setw(10) << landmark[1] << setw(10) << landmark[2] << setw(10) << endl;
cout << "robot position = " << cvmGet(mu, 0, 0) << endl;
// cout << "measurement = " << cvmGet(z,0,0) << setw(10) << cvmGet(z,1,0) << setw(10) << cvmGet(z,2,0) << endl;
for( int n = 0; n < num_landmarks; n++ )
{
cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
}
cout << "observation = " << cvmGet(obs,0,0) << setw(10) << cvmGet(obs,1,0) << setw(10) << cvmGet(obs,2,0) << endl;
cout<< "estimation = " << cvmGet(mu,2,0) << setw(10) << cvmGet(mu,3,0) << setw(10) << cvmGet(mu,4,0) << endl;
// result in image
// ground truth of robot position
cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(height/2)), 1, cvScalar(100, 0, 255));
// robot position, purple
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(height/2)), 3, cvScalar(255, 0, 100));
// uncertainty of robot position, purple line
cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
for( int index = 0; index < num_landmarks; index++ )
{
// landmarks, white
cvCircle(iplImg, cvPoint(cvRound(landmark[index]), cvRound(height/2)), 3, cvScalarAll(255));
// observation, yellow
// cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,index,0)), cvRound(height/2)), 2, cvScalar(0, 200, 255));
// estimation, green
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0)), cvRound(height/2)), 2, cvScalar(50, 255, 0));
// uncertainty of estimation, green line
cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);
}
cvShowImage("SLAM-1d", iplImg);
cvWaitKey(0);
}
cvWaitKey();
return 0;
}
console:
2차원 SLAM을 위한 Kalman filter 간단 예제
// 2-D SLAM with Kalman Filter
// VIP lab, Sogang University
// 2010-01-18
// ref. Probabilistic Robotics 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
using namespace std;
#define num_landmarks 20
#define num_dim ( 2 * (num_landmarks + 2) )
#define step 200
#define width 800
#define height 600
// uniform random number generator
double uniform_random(void) {
return (double) rand() / (double) RAND_MAX;
}
// Gaussian random number generator
double gaussian_random(void) {
static int next_gaussian = 0;
static double saved_gaussian_value;
double fac, rsq, v1, v2;
if(next_gaussian == 0) {
do {
v1 = 2.0 * uniform_random() - 1.0;
v2 = 2.0 * uniform_random() - 1.0;
rsq = v1 * v1 + v2 * v2;
}
while(rsq >= 1.0 || rsq == 0.0);
fac = sqrt(-2.0 * log(rsq) / rsq);
saved_gaussian_value = v1 * fac;
next_gaussian = 1;
return v2 * fac;
}
else {
next_gaussian = 0;
return saved_gaussian_value;
}
}
void displayMatrix(CvMat *mat, char *title = NULL) {
if(title) cout << title << endl;
for(int iR = 0; iR < mat->rows; iR++) {
for(int iC = 0; iC < mat->cols; iC++) {
printf("%.2f ", cvmGet(mat, iR, iC));
}
printf("\n");
}
printf("\n");
return;
}
void draw2DEllipseFromCovariance
(CvMat* cov, CvPoint* cnt, IplImage *iplImg, CvScalar* curveColor /*= cvScalarAll(255)*/, CvScalar* centerColor /*= cvScalarAll(128) */, int thickness /*= 1*/)
{
if(NULL == cov || 2 != cov->rows || 2 != cov->cols) {
printf("covariance matrix is not 2x2 !! \n");
exit(0);
}
double eigenvalues[6], eigenvectors[36];
float ev1, ev2, vx, vy, angle;
CvSize axes;
CvMat evals = cvMat(1, 2, CV_64F, eigenvalues), evecs = cvMat(2, 2, CV_64F, eigenvectors);
cvSVD(cov, &evals, &evecs, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
ev1 = cvmGet(&evals, 0, 0); ev2 = cvmGet(&evals, 0, 1);
if( ev1 < 0 && ev2 < 0 ) {
ev1 = -ev1;
ev2 = -ev2;
}
if( ev1 < ev2 ) {
float tmp = ev1;
ev1 = ev2;
ev2 = tmp;
}
if( ev1 <= 0 || ev2 <= 0 ) {
printf("COV Eigenvalue is negativ or zero(!)\n");
exit(0);
}
// calc angle
angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI);
axes = cvSize(cvRound(sqrt(ev1)), cvRound(sqrt(ev2)));
(float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI);
cvEllipse(iplImg, *cnt, axes, angle, 0, 360, *curveColor, thickness);
cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y - 1), cvPoint(cnt->x + 2, cnt->y + 1), *centerColor, 1);
cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y + 1), cvPoint(cnt->x + 2, cnt->y - 1), *centerColor, 1);
}
int main (int argc, char * const argv[]) {
srand(time(NULL));
// set the initial state
double rob_x = width * 0.1; // robot's initial x-position
double rob_y = height * 0.4; // robot's initial y-position
double rob_vx = 10.0; // robot's initial x-velocity
double rob_vy = 10.0; // robot's initial y-velocity
// set the initial covariance of the state uncertainty
double rob_px_cov = 0.01; // covariance of noise to robot's x-position
double rob_py_cov = 0.01; // covariance of noise to robot's y-position
double rob_vx_cov = 0.01; // covariance of noise to robot's x-velocity
double rob_vy_cov = 0.01; // covariance of noise to robot's y-velocity
// set the initial covariance of the measurement noise
double obs_x_cov = 900; // covarriance of noise to x-measurement of landmarks
double obs_y_cov = 900; // covarriance of noise to y-measurement of landmarks
// ground truth of state
double xGroundTruth = rob_x;
double yGroundTruth = rob_y;
double vxGroundTruth = rob_vx;
double vyGroundTruth = rob_vy;
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[2*num_landmarks];
for( int n = 0; n < num_landmarks; n++ )
{
landmark[2*n] = width * uniform_random();
landmark[2*n+1] = height * uniform_random();
}
IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
cvZero(iplImg);
cvNamedWindow("SLAM-2d");
// H matrix, measurement matrix
CvMat* H = cvCreateMat(2*num_landmarks, num_dim, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int r = 0; r < num_landmarks; r++)
{
cvmSet(H, 2*r, 0, -1.0); // robot's x-position
cvmSet(H, 2*r, 2*r+4, 1.0); // landmark's x-position
cvmSet(H, 2*r+1, 1, -1.0); // robot's y-position
cvmSet(H, 2*r+1, 2*r+5, 1.0); // landmarks's y-position
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H; uncertainty of control
CvMat* Q = cvCreateMat(2*num_landmarks, 2*num_landmarks, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_x_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
CvMat* G = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous x-position
cvmSet(G, 0, 2, 1.0); // x-velocity
cvmSet(G, 1, 1, 1.0); // previous y-position
cvmSet(G, 1, 3, 1.0); // y-velocity
for (int row = 2; row < G->rows; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to G; uncertainty of observation
CvMat* R = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_px_cov);
cvmSet(R, 1, 1, rob_py_cov);
cvmSet(R, 2, 2, rob_vx_cov);
cvmSet(R, 3, 3, rob_vy_cov);
displayMatrix(R, "R matrix");
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // ground truth of state
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be predicted
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, 2*num_landmarks, CV_64FC1); // K matrix // Kalman gain
CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))
CvMat* obs = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // observation for each landmark
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_x); // set mu(0,0) to "rob_x"
cvmSet(mu, 1, 0, rob_y); // set mu(1,0) to "rob_y"
cvmSet(mu, 2, 0, rob_vx); // set mu(2,0) to "rob_vx"
cvmSet(mu, 3, 0, rob_vy); // set mu(3,0) to "rob_vy"
for (int n = 0; n < 2*num_landmarks; n++)
{
cvmSet(mu, n+4, 0, landmark[n]); // set mu(4,0) to "landmark[0]", ...
}
displayMatrix(mu, "mu vector");
/* // initialize "sigma" matrix
cvmSet(sigma, 0, 0, rob_px_cov);
cvmSet(sigma, 1, 1, rob_py_cov);
cvmSet(sigma, 2, 2, rob_vx_cov);
cvmSet(sigma, 3, 3, rob_vy_cov);
for (int r = 4; r < sigma->rows; r=r*2)
{
cvmSet(sigma, r, r, obs_x_cov);
cvmSet(sigma, r+1, r+1, obs_y_cov);
}
*/ // initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_x_cov));
displayMatrix(sigma, "sigma matrix");
// matrices to be used in calculation
CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1);
CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1);
cvTranspose(G, Gt); // transpose(G) -> Gt
CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1);
CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 10x10
CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 10x6
cvTranspose(H, Ht); // transpose(H) -> Ht
CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1); // 10x10 * 10x6
CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 10x10 * 10x6
CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // 6x10 * 10x1
CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // 6x1
CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 10x1
CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 10x6 * 6x10
CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 10x10 identity matrix
cvSetIdentity(I); // does not seem to be working properly
CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 10x10
CvPoint trajectory[step];
CvPoint robot_ground[step];
int frame = int(0.9*step);
for (int t = 0; t < step; t++)
{
cout << endl << "step " << t << endl;
cvZero(iplImg);
// predict
// predict the state (ref. L2, KF algorithm, 42p)
cvMatMul(G, mu, mu_p); // G * mu -> mu_p
// predict the covariance of the state (ref. L3, EKF algorithm, 42p)
cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
// estimate Kalman gain (ref. L4, EKF algorithm, 42p)
cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
displayMatrix(K, "K matrix");
// measure
// set ground truths
if ( xGroundTruth >= width || xGroundTruth <= 0)
{
vxGroundTruth = - vxGroundTruth;
}
if ( yGroundTruth >= height || yGroundTruth <= 0 )
{
vyGroundTruth = - vyGroundTruth;
}
xGroundTruth += vxGroundTruth;
yGroundTruth += vyGroundTruth;
cvZero(rob_ground);
cvmSet(rob_ground, 0, 0, xGroundTruth);
cvmSet(rob_ground, 1, 0, yGroundTruth);
cvmSet(rob_ground, 2, 0, vxGroundTruth);
cvmSet(rob_ground, 3, 0, vyGroundTruth);
robot_ground[t] = cvPoint(cvRound(xGroundTruth),cvRound(yGroundTruth));
for (int dim = 0; dim < 2*num_landmarks; dim++)
{
cvmSet(rob_ground, dim+4, 0, landmark[dim]);
}
displayMatrix(rob_ground, "rob_ground");
// set measurement noise
for(int n = 0; n < num_landmarks; n++)
{
double rn_x = sqrt(obs_x_cov) * gaussian_random();
double rn_y = sqrt(obs_y_cov) * gaussian_random();
cvmSet(delta, 2*n, 0, rn_x);
cvmSet(delta, 2*n+1, 0, rn_y);
}
// displayMatrix(delta, "delta vector; measurement noise");
// define z, measurement, vector
cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
cvAdd(Hx, delta, z); // Hx + delta -> z
displayMatrix(z, "z vector");
// observation relative to robot's position
for( int n = 0; n < 2*num_landmarks; n++ )
{
cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
}
// update the state with Kalman gain (ref. L5, EKF algorithm, 42p)
cvMatMul(H, mu, Hmu); // H * mu -> Hmu
cvSub(z, Hmu, miss); // z - Hmu -> miss
cvMatMul(K, miss, adjust); // K * miss -> adjust
cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
displayMatrix(mu, "mu vector");
trajectory[t] = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
// update the covariance of the state
cvMatMul(K, H, KH); // K * H -> KH
cvSub(I, KH, change); // I - KH -> change
cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
displayMatrix(sigma, "sigma matrix");
// result in console
cout << "robot position: " << "px = " << cvmGet(mu, 0, 0) << " py = " << cvmGet(mu, 1, 0) << endl;
for (int n = 0; n < num_landmarks; n++)
{
cout << setw(10) << "landmark" << n+1 << " (" << landmark[2*n] << ", " << landmark[2*n+1] << ") "
<< setw(10) << "observation" << n+1 << " (" << cvmGet(obs,2*n,0) << ", " << cvmGet(obs,2*n+1,0) << ") "
<< setw(10) << "estimation" << n+1 << " (" << cvmGet(mu,4+2*n,0) << ", " << cvmGet(mu,4+2*n+1,0) << ") " << endl;
}
CvMat* local_uncertain = cvCreateMat(2, 2, CV_64FC1);
CvMat* map_uncertain [num_landmarks];
for (int n = 0; n < num_landmarks; n++)
{
map_uncertain [n] = cvCreateMat(2, 2, CV_64FC1);
}
cvmSet(local_uncertain, 0, 0, cvmGet(sigma,0,0));
cvmSet(local_uncertain, 0, 1, cvmGet(sigma,0,1));
cvmSet(local_uncertain, 1, 0, cvmGet(sigma,1,0));
cvmSet(local_uncertain, 1, 1, cvmGet(sigma,1,1));
displayMatrix(local_uncertain, "local_uncertain");
for (int n = 0; n < num_landmarks; n++)
{
cvmSet(map_uncertain[n], 0, 0, cvmGet(sigma,n+4,n+4));
cvmSet(map_uncertain[n], 0, 1, cvmGet(sigma,n+4,n+5));
cvmSet(map_uncertain[n], 1, 0, cvmGet(sigma,n+5,n+4));
cvmSet(map_uncertain[n], 1, 1, cvmGet(sigma,n+5,n+5));
displayMatrix(map_uncertain[n], "map_uncertain");
}
// result in image
// ground truth of robot position, red
cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(cvmGet(rob_ground,1,0))), 2, cvScalar(60, 0, 255), 2);
// estimated robot position, purple
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(cvmGet(mu,1,0))), 5, cvScalar(255, 0, 100), 2);
// uncertainty of robot position, purple line
// cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
// cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
CvPoint local = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
CvScalar local_center_color = cvScalar(255, 0, 100);
CvScalar local_curve_color = cvScalarAll(128);
draw2DEllipseFromCovariance(local_uncertain, &local, iplImg, &local_center_color, &local_curve_color, 1);
for( int index = 0; index < num_landmarks; index++ )
{
// landmarks, white
cvCircle(iplImg, cvPoint(cvRound(landmark[2*index]), cvRound(landmark[2*index+1])), 4, cvScalarAll(255), 2);
// observation, yellow
// cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,2*index,0)), cvRound(cvmGet(obs,2*index+1,0))), 4, cvScalar(0, 200, 255), 1);
// estimation, green
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0))), 3, cvScalar(50, 255, 0), 1);
// uncertainty of estimation, green line
// cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
// cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);
CvPoint observed = cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0)));
CvScalar observed_center_color = cvScalar(50, 255, 0);
CvScalar observed_curve_color = cvScalar(50, 255, 0);
draw2DEllipseFromCovariance(map_uncertain[index], &observed, iplImg, &observed_center_color, &observed_curve_color, 1);
}
for( int p = 1; p <= t; p++ )
{
cvLine(iplImg, robot_ground[p-1], robot_ground[p], cvScalar(60, 0, 255), 1);
cvLine(iplImg, trajectory[p-1], trajectory[p], cvScalar(255, 0, 100), 1);
}
if ( t == frame )
{
cvSaveImage("2D SLAM test.bmp", iplImg);
}
cvShowImage("SLAM-2d", iplImg);
cvWaitKey(100);
}
cout << endl << endl << "process finished" << endl;
cvWaitKey();
return 0;
}
// VIP lab, Sogang University
// 2010-01-18
// ref. Probabilistic Robotics 42p
#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <iomanip>
using namespace std;
#define num_landmarks 20
#define num_dim ( 2 * (num_landmarks + 2) )
#define step 200
#define width 800
#define height 600
// uniform random number generator
double uniform_random(void) {
return (double) rand() / (double) RAND_MAX;
}
// Gaussian random number generator
double gaussian_random(void) {
static int next_gaussian = 0;
static double saved_gaussian_value;
double fac, rsq, v1, v2;
if(next_gaussian == 0) {
do {
v1 = 2.0 * uniform_random() - 1.0;
v2 = 2.0 * uniform_random() - 1.0;
rsq = v1 * v1 + v2 * v2;
}
while(rsq >= 1.0 || rsq == 0.0);
fac = sqrt(-2.0 * log(rsq) / rsq);
saved_gaussian_value = v1 * fac;
next_gaussian = 1;
return v2 * fac;
}
else {
next_gaussian = 0;
return saved_gaussian_value;
}
}
void displayMatrix(CvMat *mat, char *title = NULL) {
if(title) cout << title << endl;
for(int iR = 0; iR < mat->rows; iR++) {
for(int iC = 0; iC < mat->cols; iC++) {
printf("%.2f ", cvmGet(mat, iR, iC));
}
printf("\n");
}
printf("\n");
return;
}
void draw2DEllipseFromCovariance
(CvMat* cov, CvPoint* cnt, IplImage *iplImg, CvScalar* curveColor /*= cvScalarAll(255)*/, CvScalar* centerColor /*= cvScalarAll(128) */, int thickness /*= 1*/)
{
if(NULL == cov || 2 != cov->rows || 2 != cov->cols) {
printf("covariance matrix is not 2x2 !! \n");
exit(0);
}
double eigenvalues[6], eigenvectors[36];
float ev1, ev2, vx, vy, angle;
CvSize axes;
CvMat evals = cvMat(1, 2, CV_64F, eigenvalues), evecs = cvMat(2, 2, CV_64F, eigenvectors);
cvSVD(cov, &evals, &evecs, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
ev1 = cvmGet(&evals, 0, 0); ev2 = cvmGet(&evals, 0, 1);
if( ev1 < 0 && ev2 < 0 ) {
ev1 = -ev1;
ev2 = -ev2;
}
if( ev1 < ev2 ) {
float tmp = ev1;
ev1 = ev2;
ev2 = tmp;
}
if( ev1 <= 0 || ev2 <= 0 ) {
printf("COV Eigenvalue is negativ or zero(!)\n");
exit(0);
}
// calc angle
angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI);
axes = cvSize(cvRound(sqrt(ev1)), cvRound(sqrt(ev2)));
(float)(180 - atan2(eigenvectors[2], eigenvectors[3]) * 180 / CV_PI);
cvEllipse(iplImg, *cnt, axes, angle, 0, 360, *curveColor, thickness);
cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y - 1), cvPoint(cnt->x + 2, cnt->y + 1), *centerColor, 1);
cvLine(iplImg, cvPoint(cnt->x - 1, cnt->y + 1), cvPoint(cnt->x + 2, cnt->y - 1), *centerColor, 1);
}
int main (int argc, char * const argv[]) {
srand(time(NULL));
// set the initial state
double rob_x = width * 0.1; // robot's initial x-position
double rob_y = height * 0.4; // robot's initial y-position
double rob_vx = 10.0; // robot's initial x-velocity
double rob_vy = 10.0; // robot's initial y-velocity
// set the initial covariance of the state uncertainty
double rob_px_cov = 0.01; // covariance of noise to robot's x-position
double rob_py_cov = 0.01; // covariance of noise to robot's y-position
double rob_vx_cov = 0.01; // covariance of noise to robot's x-velocity
double rob_vy_cov = 0.01; // covariance of noise to robot's y-velocity
// set the initial covariance of the measurement noise
double obs_x_cov = 900; // covarriance of noise to x-measurement of landmarks
double obs_y_cov = 900; // covarriance of noise to y-measurement of landmarks
// ground truth of state
double xGroundTruth = rob_x;
double yGroundTruth = rob_y;
double vxGroundTruth = rob_vx;
double vyGroundTruth = rob_vy;
// ground truth of num_landmarks landmarks in the world coordinate
double landmark[2*num_landmarks];
for( int n = 0; n < num_landmarks; n++ )
{
landmark[2*n] = width * uniform_random();
landmark[2*n+1] = height * uniform_random();
}
IplImage *iplImg = cvCreateImage(cvSize(width, height) , 8, 3);
cvZero(iplImg);
cvNamedWindow("SLAM-2d");
// H matrix, measurement matrix
CvMat* H = cvCreateMat(2*num_landmarks, num_dim, CV_64FC1);
cvZero(H); // initialize H matrix
// set H matrix
for (int r = 0; r < num_landmarks; r++)
{
cvmSet(H, 2*r, 0, -1.0); // robot's x-position
cvmSet(H, 2*r, 2*r+4, 1.0); // landmark's x-position
cvmSet(H, 2*r+1, 1, -1.0); // robot's y-position
cvmSet(H, 2*r+1, 2*r+5, 1.0); // landmarks's y-position
}
displayMatrix(H, "H matrix");
// Q matrix ; covariance of noise to H; uncertainty of control
CvMat* Q = cvCreateMat(2*num_landmarks, 2*num_landmarks, CV_64FC1);
cvZero(Q); // initialize Q matrix
// set Q matrix
for (int row = 0; row < Q->rows; row++)
{
cvmSet(Q, row, row, obs_x_cov);
}
displayMatrix(Q, "Q matrix");
// G matrix // transition
CvMat* G = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(G); // initialize G matrix
// set G matrix
cvmSet(G, 0, 0, 1.0); // previous x-position
cvmSet(G, 0, 2, 1.0); // x-velocity
cvmSet(G, 1, 1, 1.0); // previous y-position
cvmSet(G, 1, 3, 1.0); // y-velocity
for (int row = 2; row < G->rows; row++)
{
cvmSet(G, row, row, 1.0); // constance of velocity
}
displayMatrix(G, "G matrix");
// R matrix ; covariance of noise to G; uncertainty of observation
CvMat* R = cvCreateMat(num_dim, num_dim, CV_64FC1);
cvZero(R); // initialize R matrix
// set R matrix
cvmSet(R, 0, 0, rob_px_cov);
cvmSet(R, 1, 1, rob_py_cov);
cvmSet(R, 2, 2, rob_vx_cov);
cvmSet(R, 3, 3, rob_vy_cov);
displayMatrix(R, "R matrix");
CvMat* rob_ground = cvCreateMat(num_dim, 1, CV_64FC1); // ground truth of state
CvMat* mu = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be estimated
CvMat* mu_p = cvCreateMat(num_dim, 1, CV_64FC1); // state vector to be predicted
CvMat* sigma = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* sigma_p = cvCreateMat(num_dim, num_dim, CV_64FC1); // covariance to be updated
CvMat* z = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // measurement vector
CvMat* K = cvCreateMat(num_dim, 2*num_landmarks, CV_64FC1); // K matrix // Kalman gain
CvMat* delta = cvCreateMat(z->rows, 1, CV_64FC1); // measurement noise (ref. 42p: (3.5))
CvMat* obs = cvCreateMat(2*num_landmarks, 1, CV_64FC1); // observation for each landmark
// initialize "mu" vector
cvmSet(mu, 0, 0, rob_x); // set mu(0,0) to "rob_x"
cvmSet(mu, 1, 0, rob_y); // set mu(1,0) to "rob_y"
cvmSet(mu, 2, 0, rob_vx); // set mu(2,0) to "rob_vx"
cvmSet(mu, 3, 0, rob_vy); // set mu(3,0) to "rob_vy"
for (int n = 0; n < 2*num_landmarks; n++)
{
cvmSet(mu, n+4, 0, landmark[n]); // set mu(4,0) to "landmark[0]", ...
}
displayMatrix(mu, "mu vector");
/* // initialize "sigma" matrix
cvmSet(sigma, 0, 0, rob_px_cov);
cvmSet(sigma, 1, 1, rob_py_cov);
cvmSet(sigma, 2, 2, rob_vx_cov);
cvmSet(sigma, 3, 3, rob_vy_cov);
for (int r = 4; r < sigma->rows; r=r*2)
{
cvmSet(sigma, r, r, obs_x_cov);
cvmSet(sigma, r+1, r+1, obs_y_cov);
}
*/ // initialize "sigma" matrix <-- This is the most critical point in tuning
cvSetIdentity(sigma, cvRealScalar(obs_x_cov));
displayMatrix(sigma, "sigma matrix");
// matrices to be used in calculation
CvMat* Hx = cvCreateMat(H->rows, mu->cols, CV_64FC1);
CvMat* Gt = cvCreateMat(G->cols, G->rows, CV_64FC1);
cvTranspose(G, Gt); // transpose(G) -> Gt
CvMat* sigmaGt = cvCreateMat(sigma->rows, G->rows, CV_64FC1);
CvMat* GsigmaGt = cvCreateMat(G->rows, G->rows, CV_64FC1); // 10x10
CvMat* Ht = cvCreateMat(H->cols, H->rows, CV_64FC1); // 10x6
cvTranspose(H, Ht); // transpose(H) -> Ht
CvMat* sigmaHt = cvCreateMat(sigma->rows, H->rows, CV_64FC1); // 10x10 * 10x6
CvMat* HsigmaHt = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* HsigmaHtplusQ = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* invGain = cvCreateMat(H->rows, H->rows, CV_64FC1); // 6x6
CvMat* sigmapHt = cvCreateMat(sigma_p->rows, Ht->cols, CV_64FC1); // 10x10 * 10x6
CvMat* Hmu = cvCreateMat(H->rows, mu->cols, CV_64FC1); // 6x10 * 10x1
CvMat* miss = cvCreateMat(Hmu->rows, 1, CV_64FC1); // 6x1
CvMat* adjust = cvCreateMat(mu->rows, 1, CV_64FC1); // 10x1
CvMat* KH = cvCreateMat(K->rows, H->cols, CV_64FC1); // 10x6 * 6x10
CvMat* I = cvCreateMat(KH->rows, KH->cols, CV_64FC1); // 10x10 identity matrix
cvSetIdentity(I); // does not seem to be working properly
CvMat* change = cvCreateMat(I->rows, I->cols, CV_64FC1); // 10x10
CvPoint trajectory[step];
CvPoint robot_ground[step];
int frame = int(0.9*step);
for (int t = 0; t < step; t++)
{
cout << endl << "step " << t << endl;
cvZero(iplImg);
// predict
// predict the state (ref. L2, KF algorithm, 42p)
cvMatMul(G, mu, mu_p); // G * mu -> mu_p
// predict the covariance of the state (ref. L3, EKF algorithm, 42p)
cvMatMul(sigma, Gt, sigmaGt); // sigma * Gt -> sigmaGt
cvMatMul(G, sigmaGt, GsigmaGt); // G * sigmaGt -> GsigmaGt
cvAdd(GsigmaGt, R, sigma_p); // GsigmaGt + R -> sigma_p
// estimate Kalman gain (ref. L4, EKF algorithm, 42p)
cvMatMul(sigma_p, Ht, sigmaHt); // sigma_p * Ht -> sigmaHt
cvMatMul(H, sigmaHt, HsigmaHt); // H * sigmaHt -> HsigmaHt
cvAdd(HsigmaHt, Q, HsigmaHtplusQ); // HsigmaHt + Q -> HsigmaHtplusQ
cvInvert(HsigmaHtplusQ, invGain); // inv(HsigmaHtplusQ) -> invGain
cvMatMul(sigma_p, Ht, sigmapHt); // sigma_p * Ht -> sigmapHt
cvMatMul(sigmapHt, invGain, K); // sigmapHt * invGain -> K
displayMatrix(K, "K matrix");
// measure
// set ground truths
if ( xGroundTruth >= width || xGroundTruth <= 0)
{
vxGroundTruth = - vxGroundTruth;
}
if ( yGroundTruth >= height || yGroundTruth <= 0 )
{
vyGroundTruth = - vyGroundTruth;
}
xGroundTruth += vxGroundTruth;
yGroundTruth += vyGroundTruth;
cvZero(rob_ground);
cvmSet(rob_ground, 0, 0, xGroundTruth);
cvmSet(rob_ground, 1, 0, yGroundTruth);
cvmSet(rob_ground, 2, 0, vxGroundTruth);
cvmSet(rob_ground, 3, 0, vyGroundTruth);
robot_ground[t] = cvPoint(cvRound(xGroundTruth),cvRound(yGroundTruth));
for (int dim = 0; dim < 2*num_landmarks; dim++)
{
cvmSet(rob_ground, dim+4, 0, landmark[dim]);
}
displayMatrix(rob_ground, "rob_ground");
// set measurement noise
for(int n = 0; n < num_landmarks; n++)
{
double rn_x = sqrt(obs_x_cov) * gaussian_random();
double rn_y = sqrt(obs_y_cov) * gaussian_random();
cvmSet(delta, 2*n, 0, rn_x);
cvmSet(delta, 2*n+1, 0, rn_y);
}
// displayMatrix(delta, "delta vector; measurement noise");
// define z, measurement, vector
cvMatMul(H, rob_ground, Hx); // H * rob_ground -> Hx
cvAdd(Hx, delta, z); // Hx + delta -> z
displayMatrix(z, "z vector");
// observation relative to robot's position
for( int n = 0; n < 2*num_landmarks; n++ )
{
cvmSet(obs, n, 0, cvmGet(mu,0,0) + cvmGet(z,n,0));
}
// update the state with Kalman gain (ref. L5, EKF algorithm, 42p)
cvMatMul(H, mu, Hmu); // H * mu -> Hmu
cvSub(z, Hmu, miss); // z - Hmu -> miss
cvMatMul(K, miss, adjust); // K * miss -> adjust
cvAdd(mu_p, adjust, mu); // mu_p + adjust -> mu
displayMatrix(mu, "mu vector");
trajectory[t] = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
// update the covariance of the state
cvMatMul(K, H, KH); // K * H -> KH
cvSub(I, KH, change); // I - KH -> change
cvMatMul(change, sigma_p, sigma); // change * sigma_p -> sigma
displayMatrix(sigma, "sigma matrix");
// result in console
cout << "robot position: " << "px = " << cvmGet(mu, 0, 0) << " py = " << cvmGet(mu, 1, 0) << endl;
for (int n = 0; n < num_landmarks; n++)
{
cout << setw(10) << "landmark" << n+1 << " (" << landmark[2*n] << ", " << landmark[2*n+1] << ") "
<< setw(10) << "observation" << n+1 << " (" << cvmGet(obs,2*n,0) << ", " << cvmGet(obs,2*n+1,0) << ") "
<< setw(10) << "estimation" << n+1 << " (" << cvmGet(mu,4+2*n,0) << ", " << cvmGet(mu,4+2*n+1,0) << ") " << endl;
}
CvMat* local_uncertain = cvCreateMat(2, 2, CV_64FC1);
CvMat* map_uncertain [num_landmarks];
for (int n = 0; n < num_landmarks; n++)
{
map_uncertain [n] = cvCreateMat(2, 2, CV_64FC1);
}
cvmSet(local_uncertain, 0, 0, cvmGet(sigma,0,0));
cvmSet(local_uncertain, 0, 1, cvmGet(sigma,0,1));
cvmSet(local_uncertain, 1, 0, cvmGet(sigma,1,0));
cvmSet(local_uncertain, 1, 1, cvmGet(sigma,1,1));
displayMatrix(local_uncertain, "local_uncertain");
for (int n = 0; n < num_landmarks; n++)
{
cvmSet(map_uncertain[n], 0, 0, cvmGet(sigma,n+4,n+4));
cvmSet(map_uncertain[n], 0, 1, cvmGet(sigma,n+4,n+5));
cvmSet(map_uncertain[n], 1, 0, cvmGet(sigma,n+5,n+4));
cvmSet(map_uncertain[n], 1, 1, cvmGet(sigma,n+5,n+5));
displayMatrix(map_uncertain[n], "map_uncertain");
}
// result in image
// ground truth of robot position, red
cvCircle(iplImg, cvPoint(cvRound(cvmGet(rob_ground,0,0)), cvRound(cvmGet(rob_ground,1,0))), 2, cvScalar(60, 0, 255), 2);
// estimated robot position, purple
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,0,0)), cvRound(cvmGet(mu,1,0))), 5, cvScalar(255, 0, 100), 2);
// uncertainty of robot position, purple line
// cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,0,0))-sqrt(cvmGet(sigma,0,0)), cvRound(height/2)),
// cvPoint(cvRound(cvmGet(mu,0,0))+sqrt(cvmGet(sigma,0,0)), cvRound(height/2)), cvScalar(255, 0, 100), 1);
CvPoint local = cvPoint(cvRound(cvmGet(mu,0,0)),cvRound(cvmGet(mu,1,0)));
CvScalar local_center_color = cvScalar(255, 0, 100);
CvScalar local_curve_color = cvScalarAll(128);
draw2DEllipseFromCovariance(local_uncertain, &local, iplImg, &local_center_color, &local_curve_color, 1);
for( int index = 0; index < num_landmarks; index++ )
{
// landmarks, white
cvCircle(iplImg, cvPoint(cvRound(landmark[2*index]), cvRound(landmark[2*index+1])), 4, cvScalarAll(255), 2);
// observation, yellow
// cvCircle(iplImg, cvPoint(cvRound(cvmGet(obs,2*index,0)), cvRound(cvmGet(obs,2*index+1,0))), 4, cvScalar(0, 200, 255), 1);
// estimation, green
cvCircle(iplImg, cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0))), 3, cvScalar(50, 255, 0), 1);
// uncertainty of estimation, green line
// cvLine(iplImg, cvPoint(cvRound(cvmGet(mu,index+2,0))-sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)),
// cvPoint(cvRound(cvmGet(mu,index+2,0))+sqrt(cvmGet(sigma,index+2,0)), cvRound(height/2)), cvScalar(50, 255, 0), 1);
CvPoint observed = cvPoint(cvRound(cvmGet(mu,4+2*index,0)), cvRound(cvmGet(mu,4+2*index+1,0)));
CvScalar observed_center_color = cvScalar(50, 255, 0);
CvScalar observed_curve_color = cvScalar(50, 255, 0);
draw2DEllipseFromCovariance(map_uncertain[index], &observed, iplImg, &observed_center_color, &observed_curve_color, 1);
}
for( int p = 1; p <= t; p++ )
{
cvLine(iplImg, robot_ground[p-1], robot_ground[p], cvScalar(60, 0, 255), 1);
cvLine(iplImg, trajectory[p-1], trajectory[p], cvScalar(255, 0, 100), 1);
}
if ( t == frame )
{
cvSaveImage("2D SLAM test.bmp", iplImg);
}
cvShowImage("SLAM-2d", iplImg);
cvWaitKey(100);
}
cout << endl << endl << "process finished" << endl;
cvWaitKey();
return 0;
}