돛단배

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)¶

// 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;
}

// 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;
}

References

[1] J. Civera, A.J. Davison, and J.M.M Montiel. Inverse depth parametrization for monocular SLAM. IEEE Trans. on Robotics, 24(5), 2008.

[2] J. Civera, Andrew Davison, and J. Montiel. Inverse Depth to Depth Conversion for Monocular SLAM. In IEEE Int. Conf. on Robotics and Automation, pages 2778 –2783, April 2007.

[3] A. J. Davison. Real-time simultaneous localisation and mapping with a single camera. In Int. Conf. on Computer Vision, volume 2, pages 1403–1410, Nice, October 2003.

[4] Andrew J. Davison. Active search for real-time vision. Int. Conf. on Computer Vision, 1:66–73, 2005.

[5] Andrew J. Davison, Ian D. Reid, Nicholas D. Molton, and Olivier Stasse. MonoSLAM: Real-time single camera SLAM. Trans. on Pattern Analysis and Machine Intelligence, 29(6):1052–1067, June 2007.

[6] Ethan Eade and Tom Drummond. Scalable monocular SLAM. IEEE Int. Conf. on Computer Vision and Pattern Recognition, 1:469–476, 2006.

[7] Thomas Lemaire and Simon Lacroix. Monocular-vision based SLAM using line segments. In IEEE Int. Conf. on Robotics and Automation, pages 2791–2796, Rome, Italy, 2007.

[8] Nicholas Molton, Andrew J. Davison, and Ian Reid. Locally planar patch features for real-time structure from motion. In British Machine Vision Conference, 2004.

[9] J. Montiel, J. Civera, and A. J. Davison. Unified inverse depth parametrization for monocular SLAM. In Robotics: Science and Systems, Philadelphia, USA, August 2006.

[10] L. M. Paz, P. Pini´es, J. Tard´os, and J. Neira. Large scale 6DOF SLAM with stereo-in-hand. IEEE Trans. on Robotics, 24(5), 2008.

[11] J. Sol`a, Andr´e Monin, Michel Devy, and T. Vidal-Calleja. Fusing monocular information in multi-camera SLAM. IEEE Trans. on Robotics, 24(5):958–968, 2008.

[12] Joan Sol`a. Towards Visual Localization, Mapping and Moving Objects Tracking by a Mobile Robot: a Geometric and Probabilistic Approach. PhD thesis, Institut National Polytechnique de Toulouse, 2007.

[13] Joan Sol`a, Andr´e Monin, and Michel Devy. BiCamSLAM: Two times mono is more than stereo. In IEEE Int. Conf. on Robotics and Automation, pages 4795–4800, Rome, Italy, April 2007.

[14] Joan Sol`a, Andr´e Monin, Michel Devy, and Thomas Lemaire. Undelayed initialization in bearing only SLAM. In IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pages 2499–2504, Edmonton, Canada, 2005.

[15] Joan Sol`a, Teresa Vidal-Calleja, and Michel Devy. Undelayed initialization of line segments in monocular SLAM. In IEEE Int. Conf. on Intelligent Robots and Systems, Saint Louis, USA, 2009. To appear.

OS: 32bit Windows Vista Home Basic K Service Pack 1
processor: Intel(R) Core(TM)2 Duo CPU T5750 @ 2.00GHz 2.00 GHz
RAM: 2.00GM

Microsoft Visual Studio 2005 버전 8.050727.867 (vsvista.050727-8600
Microsoft.Net Framework 버전 2.0.50727 서비스 팩

// 3-D Particle filter algorithm + Computer Vision exercise
// : object tracking - contour tracking
// lym, VIP Lab, Sogang Univ.
// 2009-11-23
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations & Canny edge detection
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles
#define D 3 // dimension of the state

// 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;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 3; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}

double distanceEuclidean(CvPoint2D64f a, CvPoint2D64f b)
{
    double d2 = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y);
    return sqrt(d2);
}

// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(3, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 3, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(3, 3, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(3, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
//    return exp( -0.5 * cvmGet(dist, 0, 0) );
//    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
}

// likelihood based on normal distribution (ref. 14p eqn(2.3))
double likelihood(double distance, double standardDeviation) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * distance*distance / variance ) / sqrt(2 * PI * variance);
}

int main (int argc, char * const argv[]) {
   
    srand(time(NULL));
   
    IplImage *iplOriginalColor; // image to be captured
    IplImage *iplOriginalGrey; // grey-scale image of "iplOriginalColor"
    IplImage *iplEdge; // image detected by Canny edge algorithm
    IplImage *iplImg; // resulting image to show tracking process   
    IplImage *iplEdgeClone;

    int hours, minutes, seconds;
    double frame_rate, Codec, frame_count, duration;
    char fnVideo[200], titleOriginal[200], titleEdge[200], titleResult[200];
   
    sprintf(titleOriginal, "original");
    sprintf(titleEdge, "Edges by Canny detector");
//    sprintf(fnVideo, "E:/AVCHD/BDMV/STREAM/00092.avi");   
    sprintf(fnVideo, "/Users/lym/Documents/VIP/2009/Nov/volleyBall.mov");
    sprintf(titleResult, "3D Particle filter for contour tracking");
   
    CvCapture *capture = cvCaptureFromAVI(fnVideo);
   
    // stop the process if capture is failed
    if(!capture) { printf("Can NOT read the movie file\n"); return -1; }
   
    frame_rate = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
//    Codec = cvGetCaptureProperty( capture, CV_CAP_PROP_FOURCC );
    frame_count = cvGetCaptureProperty( capture, CV_CAP_PROP_FRAME_COUNT);
   
    duration = frame_count/frame_rate;
    hours = duration/3600;
    minutes = (duration-hours*3600)/60;
    seconds = duration-hours*3600-minutes*60;
   
    //  stop the process if grabbing is failed
    //    if(cvGrabFrame(capture) == 0) { printf("Can NOT grab a frame\n"); return -1; }
   
    cvSetCaptureProperty(capture, CV_CAP_PROP_POS_FRAMES, 0); // go to frame #0
    iplOriginalColor = cvRetrieveFrame(capture);
    iplOriginalGrey = cvCreateImage(cvGetSize(iplOriginalColor), 8, 1);
    iplEdge = cvCloneImage(iplOriginalGrey);
    iplEdgeClone = cvCreateImage(cvSize(iplOriginalColor->width, iplOriginalColor->height), 8, 3);
    iplImg = cvCreateImage(cvSize(iplOriginalColor->width, iplOriginalColor->height), 8, 3);   
   
    int width = iplOriginalColor->width;
    int height = iplOriginalColor->height;
   
    cvNamedWindow(titleOriginal);
    cvNamedWindow(titleEdge);
   
    cout << "image width : height = " << width << "  " << height << endl;
    cout << "# of frames = " << frame_count << endl;   
    cout << "capture finished" << endl;   
   
   
    // set the system   
   
    // set the process noise
    // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(D, D, CV_64FC1);
    // assume the transition noise
    for (int row = 0; row < D; row++)
    {   
        for (int col = 0; col < D; col++)
        {
            cvmSet(transition_noise, row, col, 0.0);
        }
    }
    cvmSet(transition_noise, 0, 0, 3.0);
    cvmSet(transition_noise, 1, 1, 3.0);
    cvmSet(transition_noise, 2, 2, 0.3);
   
    // set the measurement noise
/*
    // covariance of Gaussian noise to measurement
     CvMat* measurement_noise = cvCreateMat(D, D, CV_64FC1);
     // initialize the measurement noise
     for (int row = 0; row < D; row++)
     {   
        for (int col = 0; col < D; col++)
        {
            cvmSet(measurement_noise, row, col, 0.0);
        }
     }
     cvmSet(measurement_noise, 0, 0, 5.0);
     cvmSet(measurement_noise, 1, 1, 5.0);
     cvmSet(measurement_noise, 2, 2, 5.0); 
 */
    double measurement_noise = 2.0; // standrad deviation of Gaussian noise to measurement
   
    CvMat* state = cvCreateMat(D, 1, CV_64FC1);    // state of the system to be estimated   
//    CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); // measurement of states
   
    // declare particles
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(D, 1, CV_64FC1);
        pp[n] = cvCreateMat(D, 1, CV_64FC1);
        pu[n] = cvCreateMat(D, 1, CV_64FC1);   
        v[n] = cvCreateMat(D, 1, CV_64FC1);   
        vu[n] = cvCreateMat(D, 1, CV_64FC1);           
    }   
   
    // initialize the state and particles    
    for (int n = 0; n < N; n++)
    {
        cvmSet(state, 0, 0, 258.0); // center-x
        cvmSet(state, 1, 0, 406.0); // center-y       
        cvmSet(state, 2, 0, 38.0); // radius   
       
//        cvmSet(state, 0, 0, 300.0); // center-x
//        cvmSet(state, 1, 0, 300.0); // center-y       
//        cvmSet(state, 2, 0, 38.0); // radius       
       
        cvmSet(pb[n], 0, 0, cvmGet(state,0,0)); // center-x
        cvmSet(pb[n], 1, 0, cvmGet(state,1,0)); // center-y
        cvmSet(pb[n], 2, 0, cvmGet(state,2,0)); // radius
       
        cvmSet(v[n], 0, 0, 2 * uniform_random()); // center-x
        cvmSet(v[n], 1, 0, 2 * uniform_random()); // center-y
        cvmSet(v[n], 2, 0, 0.1 * uniform_random()); // radius       
       
        w[n] = (double) 1 / (double) N; // equally weighted particle
    }
   
    // initialize the image window
    cvZero(iplImg);   
    cvNamedWindow(titleResult);
   
    cout << "start filtering... " << endl << endl;
   
    float aperture = 3,     thresLow = 50,     thresHigh = 110;   
//    float aperture = 3,     thresLow = 80,     thresHigh = 110;   
    // for each frame
    int frameNo = 0;   
    while(frameNo < frame_count && cvGrabFrame(capture)) {
        // retrieve color frame from the movie "capture"
        iplOriginalColor = cvRetrieveFrame(capture);        
        // convert color pixel values of "iplOriginalColor" to grey scales of "iplOriginalGrey"
        cvCvtColor(iplOriginalColor, iplOriginalGrey, CV_RGB2GRAY);               
        // extract edges with Canny detector from "iplOriginalGrey" to save the results in the image "iplEdge" 
        cvCanny(iplOriginalGrey, iplEdge, thresLow, thresHigh, aperture);

        cvCvtColor(iplEdge, iplEdgeClone, CV_GRAY2BGR);
       
        cvShowImage(titleOriginal, iplOriginalColor);
        cvShowImage(titleEdge, iplEdge);

//        cvZero(iplImg);
       
        cout << "frame # " << frameNo << endl;
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < D; row++)
            {
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0)
                            + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
            if ( cvmGet(pp[n],2,0) < 2) { cvmSet(pp[n],2,0,0.0); }
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))),
//             cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
            cvCircle(iplEdgeClone, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvRound(cvmGet(pp[n],2,0)), CV_RGB(255, 255, 0));
//            cvCircle(iplImg, cvPoint(iplImg->width *0.5, iplImg->height * 0.5), 100, CV_RGB(255, 255, 0), -1);
//            cvSaveImage("a.bmp", iplImg);

            double cX = cvmGet(pp[n], 0, 0); // predicted center-y of the object
            double cY = cvmGet(pp[n], 1, 0); // predicted center-x of the object
            double cR = cvmGet(pp[n], 2, 0); // predicted radius of the object           

            if ( cR < 0 ) { cR = 0; }
           
            // measure
            // search measurements
            CvPoint2D64f direction [8]; // 8 searching directions
            // define 8 starting points in each direction
            direction[0].x = cX + cR;    direction[0].y = cY;      // East
            direction[2].x = cX;        direction[2].y = cY - cR; // North
            direction[4].x = cX - cR;    direction[4].y = cY;      // West
            direction[6].x = cX;        direction[6].y = cY + cR; // South
            int cD = cvRound( cR/sqrt(2.0) );
            direction[1].x = cX + cD;    direction[1].y = cY - cD; // NE
            direction[3].x = cX - cD;    direction[3].y = cY - cD; // NW
            direction[5].x = cX - cD;    direction[5].y = cY + cD; // SW
            direction[7].x = cX + cD;    direction[7].y = cY + cD; // SE       
           
            CvPoint2D64f search [8];    // searched point in each direction         
            double scale = 0.4;
            double scope [8]; // scope of searching
   
            for ( int i = 0; i < 8; i++ )
            {
//                scope[2*i] = cR * scale;
//                scope[2*i+1] = cD * scale;
                scope[i] = 6.0;
            }
           
            CvPoint d[8];
            d[0].x = 1;        d[0].y = 0; // E
            d[1].x = 1;        d[1].y = -1; // NE
            d[2].x = 0;        d[2].y = 1; // N
            d[3].x = 1;        d[3].y = 1; // NW
            d[4].x = 1;        d[4].y = 0; // W
            d[5].x = 1;        d[5].y = -1; // SW
            d[6].x = 0;        d[6].y = 1; // S
            d[7].x = 1;        d[7].y = 1; // SE           
           
            int count = 0; // number of measurements
            double dist_sum = 0;
           
            for (int i = 0; i < 8; i++) // for 8 directions
            {
                double dist = scope[i] * 1.5;
                for ( int range = 0; range < scope[i]; range++ )
                {
                    int flag = 0;
                    for (int turn = -1; turn <= 1; turn += 2) // reverse the searching direction
                    {
                        search[i].x = direction[i].x + turn * range * d[i].x;
                        search[i].y = direction[i].y + turn * range * d[i].y;
                       
//                        cvCircle(iplImg, cvPoint(cvRound(search[i].x), cvRound(search[i].y)), 2, CV_RGB(0, 255, 0), -1);
//                        cvShowImage(titleResult, iplImg);
//                        cvWaitKey(100);

                        // detect measurements   
//                        CvScalar s = cvGet2D(iplEdge, cvRound(search[i].y), cvRound(search[i].x));
                        unsigned char s = CV_IMAGE_ELEM(iplEdge, unsigned char, cvRound(search[i].y), cvRound(search[i].x));
//                        if ( s.val[0] > 200 && s.val[1] > 200 && s.val[2] > 200 ) // bgr color               
                        if (s > 250) // bgr color                           
                        { // when the pixel value is white, that means a measurement is detected
                            flag = 1;
                            count++;
//                            cvCircle(iplEdgeClone, cvPoint(cvRound(search[i].x), cvRound(search[i].y)), 3, CV_RGB(200, 0, 255));
//                            cvShowImage("3D Particle filter", iplEdgeClone);
//                            cvWaitKey(1);
/*                            // get measurement
                            cvmSet(measurement, 0, 0, search[i].x);
                            cvmSet(measurement, 1, 0, search[i].y);   
                            double dist = distance(measurement, pp[n]);
*/                            // evaluate the difference between predictions of the particle and measurements
                            dist = distanceEuclidean(search[i], direction[i]);
                            break; // break for "turn"
                        } // end if
                    } // for turn
                    if ( flag == 1 )
                    { break; } // break for "range"
                } // for range
               
                dist_sum += dist; // for all searching directions of one particle 

            } // for i direction
           
            double dist_avg; // average distance of measurements from the one particle "n"
//            cout << "count = " << count << endl;
            dist_avg = dist_sum / 8;
//            cout << "dist_avg = " << dist_avg << endl;
           
//            estimate likelihood with "dist_avg"
            like[n] = likelihood(dist_avg, measurement_noise);
//            cout << "likelihood of particle-#" << n << " = " << like[n] << endl;
            like_sum += like[n];   
        } // for n particle
//        cout << "sum of likelihoods of N particles = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
        double state_r = 0.0;
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // center-x of the object
            state_y += w[n] * cvmGet(pp[n], 1, 0); // center-y of the object
            state_r += w[n] * cvmGet(pp[n], 2, 0); // radius of the object           
        }
        if (state_r < 0) { state_r = 0; }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
        cvmSet(state, 2, 0, state_r);
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation: (x,y,r) = " << cvmGet(state,0,0) << ",  " << cvmGet(state,1,0)
        << ",  " << cvmGet(state,2,0) << endl;
        cvCircle(iplEdgeClone, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0)) ),
                 cvRound(cvmGet(state,2,0)), CV_RGB(255, 0, 0), 1);

        cvShowImage(titleResult, iplEdgeClone);
        cvWaitKey(1);

   
        // update particles       
        cout << endl << "updating particles" << endl;
        double a[N]; // portion between particles
       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if ( uniform_random() < a[k] )               
                {
                    pselected = k;
                    break;
                }
            }
//            cout << "p " << n << " => " << pselected << "  ";       
           
            // retain the selection 
            for (int row = 0; row < D; row++)
            {
                cvmSet(pu[n], row, 0, cvmGet(pp[pselected],row,0));
                cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
            }
        }
       
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < D; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cout << endl << endl ;
       
//      cvShowImage(titleResult, iplImg);  
//        cvWaitKey(1000);       
        cvWaitKey(1);       
        frameNo++;
    }
   
    cvWaitKey();   
   
    return 0;
}

http://wiki.answers.com/Q/What_is_a_feedback_system
A feedback system, in general engineering terms, is a system whose output if fed back to the input, and depending on the output, your input is adjusted so as to reach a steady-state. In colloquial language, you adjust your input based on the output of your system so as to achieve a certain end, like minimizing disturbance, cancelling echo (in a speech system) and so on.

http://en.wikipedia.org/wiki/Three_sigma_rule
In statistics, the 68-95-99.7 rule, or three-sigma rule, or empirical rule, states that for a normal distribution, nearly all values lie within 3 standard deviations of the mean.

// 1-D Particle filter algorithm exercise
// 2009-11-06
// ref. Probabilistic Robotics: 98p

#include <iostream>
#include <cstdlib> //defining RAND_MAX
#include <ctime> //time as a random seed
#include <cmath>
using namespace std;

#define PI 3.14159265
#define N 10 //number of particles

////////////////////////////////////////////////////////////////////////////
// random number generators written by kyu
double uniform_random(void) {
   
    return (double) rand() / (double) RAND_MAX;
   
}

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;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}

////////////////////////////////////////////////////////////////////////////


int main (int argc, char * const argv[]) {
   
    double groundtruth[] = {0.5, 2.0, 3.5, 5.0, 7.0, 8.0, 10.0};
    double measurement[] = {0.4, 2.1, 3.2, 5.3, 7.4, 8.1, 9.6};
    double transition_noise = 0.3; // covariance of Gaussian noise to control
    double measurement_noise = 0.3; // covariance of Gaussian noise to measurement
   
    double x[N]; // N particles
    double x_p[N]; // predicted particles
    double state; // estimated state with particles
    double x_u[N]; // temporary variables for updating particles
    double v[N]; // velocity
    double v_u[N]; // temporary variables for updating velocity   
    double m[N]; // measurement
    double l[N]; // likelihood
    double lsum; // sum of likelihoods
    double w[N]; // weight of each particle
    double a[N]; // portion between particles
   
    double grn[N]; // Gaussian random number
    double urn[N]; // uniform random number
   
    srand(time(NULL));        
   
    // initialize particles
    for (int n = 0; n < N; n++)
    {
        x[n] = 0.0;
        v[n] = 0.0;
        w[n] = (double)1/(double)N;           
    }
   
    int step = 7;
    for (int t = 0; t < step; t++)
    {
        cout << "step " << t << endl;       
        // measure
        m[t] = measurement[t];
       
        cout << "groundtruth = " << groundtruth[t] << endl;
        cout << "measurement = " << measurement[t] << endl;       
       
        lsum = 0;
        for (int n = 0; n < N; n++)
        {
            // predict
            grn[n] = gaussian_random();
            x_p[n] = x[n] + v[n] + transition_noise * grn[n];
//            cout << grn[n] << endl;
           
            // estimate likelihood between measurement and each prediction
            l[n] = normal_distribution(m[t], measurement_noise, x_p[n]); // ref. 14p eqn(2.3)
            lsum += l[n];
        }
//            cout << "lsum = " << lsum << endl;       
       
        // estimate states       
        state = 0;
        for (int n = 0; n < N; n++)
        {
            w[n] = l[n] / lsum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state += w[n] * x_p[n]; // estimate the state with particles
        }   
        cout << "estimation = " << state << endl;
       
        // update       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int select;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    select = k;
                    break;
                }
            }
            cout << "select" << n << "= " << select << "  ";       
            // retain the selection 
            x_u[n] = x_p[select];
            v_u[n] = x_p[select] - x[select];
        }
        cout << endl << endl;
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            x[n] = x_u[n];
            v[n] = v_u[n];
//            cout << "v" << n << "= " << v[n] << "  ";   
        }
    }
   
    return 0;
}

// 2-D Particle filter algorithm exercise
// 2009-11-10
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

// 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;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void set_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
//    cout << "check measurement type3 = " << mtype[3] << endl; // just to check

    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
//    return exp( -0.5 * cvmGet(dist, 0, 0) );
//    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
   
}



/*
struct particle
{
    double weight; // weight of a particle
    CvMat* loc_p = cvCreateMat(2, 1, CV_64FC1); // previously estimated position of a particle
    CvMat* loc = cvCreateMat(2, 1, CV_64FC1); // currently estimated position of a particle
    CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); // estimated velocity of a particle
    cvSub(loc, loc_p, velocity); // loc - loc_p -> velocity
};
*/


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    int width = 400; // width of image window
    int height = 400; // height of image window   
   
    IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);
    cvZero(iplImg);
   
    cvNamedWindow("ParticleFilter-2d", 0);
   
   
     // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //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, 6);      
   
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //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, 2);  
   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);
    // declare particles
/*    particle pb[N]; // N estimated particles
    particle pp[N]; // N predicted particles       
    particle pu[N]; // temporary variables for updating particles       
*/
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize particles and the state
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
            cvmSet(state, row, 0, 0.0);           
        }
    }
   
    // set the system   
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }
   
    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // set groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        set_groundtruth(groundtruth, gx, gy);

        // set measurement types
        double c1 = 1.0, c2 = 4.0;   
        // measured point 1
        cvmSet(measurement[0], 0, 0, gx + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[0], 1, 0, gy + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 2
        cvmSet(measurement[1], 0, 0, gx + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
        cvmSet(measurement[1], 1, 0, gy + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
        // measured point 3 // clutter noise
        cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
        cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

        // count the number of measurements       
        int count = count_detections(); // number of detections
        cout << "# of measured points = " << count << endl;
   
        // get measurement           
        for (int index = 0; index < count; index++)
        {
            cout << "measurement #" << index << " : "
                << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(200, 0, 255), 1);
           
        }
       
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
               
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
           
            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));

           
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
///            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
       
///            cout << "likelihood of #" << n << " = " << like[n] << endl;
           
            like_sum += like[n];
        }
       
///        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
   
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
///            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
   
        // update particles       
        cout << endl << "updating particles" << endl;
        double urn[N]; // uniform random number
        double a[N]; // portion between particles

        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            urn[n] = uniform_random();
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if (urn[n] < a[k] )
                {
                    pselected = k;
                    break;
                }
            }
///            cout << "particle " << n << " => " << pselected << "  ";       
            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value
           
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
   
        }
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 4, cvScalarAll(255), 1);
       
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-2d", iplImg);
        cvWaitKey(1000);   
       
       
       
       
    } // for "t"
   
   
    cvWaitKey();   
   
    return 0;
}

// 2-D Particle filter algorithm exercise
// lym, VIP Lab, Sogang Univ.
// 2009-11-23
// ref. Probabilistic Robotics: 98p

#include <OpenCV/OpenCV.h> // matrix operations
#include <iostream>
#include <cstdlib> // RAND_MAX
#include <ctime> // time as a random seed
#include <cmath>
#include <algorithm>
using namespace std;

#define PI 3.14159265
#define N 100 //number of particles

int width = 400; // width of image window
int height = 400; // height of image window   
IplImage *iplImg = cvCreateImage(cvSize(width, height), 8, 3);

// 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;
    }
}

double normal_distribution(double mean, double standardDeviation, double state) {
   
    double variance = standardDeviation * standardDeviation;
   
    return exp(-0.5 * (state - mean) * (state - mean) / variance ) / sqrt(2 * PI * variance);
}
////////////////////////////////////////////////////////////////////////////

// set groundtruth
void get_groundtruth (CvMat* groundtruth, double x, double y)
{
    cvmSet(groundtruth, 0, 0, x); // x-value
    cvmSet(groundtruth, 1, 0, y); // y-value
   
    cout << "groundtruth = " << cvmGet(groundtruth,0,0) << "  " << cvmGet(groundtruth,1,0) << endl;
    cvCircle(iplImg, cvPoint(cvRound(cvmGet(groundtruth,0,0)), cvRound(cvmGet(groundtruth,1,0))), 2, cvScalarAll(255), 2);   
}


// count the number of detections in measurement process
int count_detections (void)
{
    // set cases of measurement results
    double mtype[4];
    mtype [0] = 0.0;
    mtype [1] = 0.5;
    mtype [2] = mtype[1] + 0.3;
    mtype [3] = mtype[2] + 0.2;
    //    cout << "check measurement type3 = " << mtype[3] << endl; // just to check
   
    // select a measurement case
    double mrn = uniform_random();       
    int type = 1;
    for ( int k = 0; k < 3; k++ )
    {   
        if ( mrn < mtype[k] )
        {
            type = k;
            break;
        }
    }
    return type;
}

// get measurements
int get_measurement (CvMat* measurement[], CvMat* measurement_noise, double x, double y)
{
    // set measurement types
    double c1 = 1.0, c2 = 4.0;   
    // measured point 1
    cvmSet(measurement[0], 0, 0, x + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[0], 1, 0, y + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 2
    cvmSet(measurement[1], 0, 0, x + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
    cvmSet(measurement[1], 1, 0, y + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
    // measured point 3 // clutter noise
    cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
    cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value       

    // count the number of measured points   
    int number = count_detections(); // number of detections
    cout << "# of measured points = " << number << endl;

    // get measurement           
    for (int index = 0; index < number; index++)
    {
        cout << "measurement #" << index << " : "
        << cvmGet(measurement[index],0,0) << "  " << cvmGet(measurement[index],1,0) << endl;
       
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(measurement[index],0,0)), cvRound(cvmGet(measurement[index],1,0))), 4, CV_RGB(255, 0, 255), 2);           
    }

    return number;
}


// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
    double distance2 = 0;
    double differance = 0;
    for (int u = 0; u < 2; u++)
    {
        differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
        distance2 += differance * differance;
    }
    return sqrt(distance2);
}


// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
   
    CvMat* diff = cvCreateMat(2, 1, CV_64FC1);
    cvSub(sample, mean, diff); // sample - mean -> diff
    CvMat* diff_t = cvCreateMat(1, 2, CV_64FC1);
    cvTranspose(diff, diff_t); // transpose(diff) -> diff_t
    CvMat* cov_inv = cvCreateMat(2, 2, CV_64FC1);
    cvInvert(covariance, cov_inv); // transpose(covariance) -> cov_inv
    CvMat* tmp = cvCreateMat(2, 1, CV_64FC1);
    CvMat* dist = cvCreateMat(1, 1, CV_64FC1);
    cvMatMul(cov_inv, diff, tmp); // cov_inv * diff -> tmp   
    cvMatMul(diff_t, tmp, dist); // diff_t * tmp -> dist
   
    double likeliness = exp( -0.5 * cvmGet(dist, 0, 0) );
    double bound = 0.0000001;
    if ( likeliness < bound )
    {
        likeliness = bound;
    }
    return likeliness;
    //    return exp( -0.5 * cvmGet(dist, 0, 0) );
    //    return max(0.0000001, exp(-0.5 * cvmGet(dist, 0, 0)));   
}


int main (int argc, char * const argv[]) {
   
    srand(time(NULL));

    // set the system   
    CvMat* state = cvCreateMat(2, 1, CV_64FC1);    // state of the system to be estimated
    CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states   
    CvMat* measurement [3]; // measurement of states
    for (int k = 0; k < 3; k++) // 3 types of measurement
    {
        measurement[k] = cvCreateMat(2, 1, CV_64FC1);
    }   

    // declare particles
    CvMat* pb [N]; // estimated particles
    CvMat* pp [N]; // predicted particles
    CvMat* pu [N]; // temporary variables to update a particle
    CvMat* v[N]; // estimated velocity of each particle
    CvMat* vu[N]; // temporary varialbe to update the velocity   
    double w[N]; // weight of each particle
    for (int n = 0; n < N; n++)
    {
        pb[n] = cvCreateMat(2, 1, CV_64FC1);
        pp[n] = cvCreateMat(2, 1, CV_64FC1);
        pu[n] = cvCreateMat(2, 1, CV_64FC1);   
        v[n] = cvCreateMat(2, 1, CV_64FC1);   
        vu[n] = cvCreateMat(2, 1, CV_64FC1);           
    }   
   
    // initialize the state and particles
    for (int n = 0; n < N; n++)
    {
        w[n] = (double) 1 / (double) N; // equally weighted
        for (int row=0; row < 2; row++)
        {
            cvmSet(state, row, 0, 0.0);   
            cvmSet(pb[n], row, 0, 0.0);
            cvmSet(v[n], row, 0, 15 * uniform_random());
        }
    }
   
    // set the process noise
    // covariance of Gaussian noise to control
    CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 3.0
    cvmSet(transition_noise, 0, 1, 0.0);
    cvmSet(transition_noise, 1, 0, 0.0);
    cvmSet(transition_noise, 1, 1, 6);      
   
    // set the measurement noise
    // covariance of Gaussian noise to measurement
    CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
    cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 2.0
    cvmSet(measurement_noise, 0, 1, 0.0);
    cvmSet(measurement_noise, 1, 0, 0.0);
    cvmSet(measurement_noise, 1, 1, 2);  
   
    // initialize the image window
    cvZero(iplImg);   
    cvNamedWindow("ParticleFilter-3d", 0);

    cout << "start filtering... " << endl << endl;
    int step = 30; //30; // timestep
   
    for (int t = 0; t < step; t++) // for "step" steps
    {
//        cvZero(iplImg);
        cout << "step " << t << endl;
       
        // get the groundtruth
        double gx = 10 * t;
        double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
        get_groundtruth(groundtruth, gx, gy);
        // get measurements
        int count = get_measurement(measurement, measurement_noise, gx, gy);
       
        double like[N]; // likelihood between measurement and prediction
        double like_sum = 0; // sum of likelihoods
       
        for (int n = 0; n < N; n++) // for "N" particles
        {
            // predict
            double prediction;
            for (int row = 0; row < 2; row++)
            {
                prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
                cvmSet(pp[n], row, 0, prediction);
            }
//            cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);           
//            cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));
           
            // evaluate measurements
            double range = (double) width; // range to search measurements for each particle
//            cout << "range of distances = " << range << endl;
            int mselected;
            for (int index = 0; index < count; index++)
            {
                double d = distance(measurement[index], pp[n]);
               
                if ( d < range )
                {
                    range = d;
                    mselected = index; // selected measurement
                }
            }
//            cout << "selected measurement # = " << mselected << endl;
            like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);   
//            cout << "likelihood of #" << n << " = " << like[n] << endl;           
            like_sum += like[n];
        }
//        cout << "sum of likelihoods = " << like_sum << endl;
       
        // estimate states       
        double state_x = 0.0;
        double state_y = 0.0;
        // estimate the state with predicted particles
        for (int n = 0; n < N; n++) // for "N" particles
        {
            w[n] = like[n] / like_sum; // update normalized weights of particles           
//            cout << "w" << n << "= " << w[n] << "  ";               
            state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
            state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
        }
        cvmSet(state, 0, 0, state_x);
        cvmSet(state, 1, 0, state_y);       
       
        cout << endl << "* * * * * *" << endl;       
        cout << "estimation = " << cvmGet(state,0,0) << "  " << cvmGet(state,1,0) << endl;
        cvCircle(iplImg, cvPoint(cvRound(cvmGet(state,0,0)), cvRound(cvmGet(state,1,0))), 4, CV_RGB(0, 255, 200), 2);
       
        // update particles       
        cout << endl << "updating particles" << endl;
        double a[N]; // portion between particles
       
        // define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
        a[0] = w[0];
        for (int n = 1; n < N; n++)
        {
            a[n] = a[n - 1] + w[n];
//            cout << "a" << n << "= " << a[n] << "  ";           
        }
//        cout << "a" << N << "= " << a[N] << "  " << endl;           
       
        for (int n = 0; n < N; n++)
        {   
            // select a particle from the distribution
            int pselected;
            for (int k = 0; k < N; k++)
            {
                if ( uniform_random() < a[k] )               
                {
                    pselected = k;
                    break;
                }
            }
//            cout << "p " << n << " => " << pselected << "  ";       

            // retain the selection 
            cvmSet(pu[n], 0, 0, cvmGet(pp[pselected],0,0)); // x-value
            cvmSet(pu[n], 1, 0, cvmGet(pp[pselected],1,0)); // y-value               
            cvSub(pp[pselected], pb[pselected], vu[n]); // pp - pb -> vu
        }
       
        // updated each particle and its velocity
        for (int n = 0; n < N; n++)
        {
            for (int row = 0; row < 2; row++)
            {
                cvmSet(pb[n], row, 0, cvmGet(pu[n],row,0));
                cvmSet(v[n], row , 0, cvmGet(vu[n],row,0));
            }
        }
        cout << endl << endl ;
       
        cvShowImage("ParticleFilter-3d", iplImg);
        cvWaitKey(1000);   
       
    } // for "t"
   
    cvWaitKey();   
   
    return 0;
}

head and face modeling
map building
pervasive computing
real-time detection

RTV4HCI: A Historical Overview.
- Real-Time Algorithms: From Signal Processing to Computer Vision.
- Recognition of Isolated Fingerspelling Gestures Using Depth Edges.
- Appearance-Based Real-Time Understanding of Gestures Using Projected Euler Angles.
- Flocks of Features for Tracking Articulated Objects.
- Static Hand Posture Recognition Based on Okapi-Chamfer Matching.
- Visual Modeling of Dynamic Gestures Using 3D Appearance and Motion Features.
- Head and Facial Animation Tracking Using Appearance-Adaptive Models and Particle Filters.
- A Real-Time Vision Interface Based on Gaze Detection -- EyeKeys.
- Map Building from Human-Computer Interactions.
- Real-Time Inference of Complex Mental States from Facial Expressions and Head Gestures.
- Epipolar Constrained User Pushbutton Selection in Projected Interfaces.
- Vision-Based HCI Applications.
- The Office of the Past.
- MPEG-4 Face and Body Animation Coding Applied to HCI.
- Multimodal Human-Computer Interaction.
- Smart Camera Systems Technology Roadmap.
- Index.

The goal of research in real-time vision for human-computer interaction is to develop algorithms and systems that sense and perceive humans and human activity, in order to enable more natural, powerful, and effective computer interfaces.

Computers in the Human Interaction Loop (CHIL)

perceptual interfaces
multimodal interfaces
post-WIMP(windows, icons, menus, pointer) interfaces

implicit user awareness or explicit user control

The user interface
- the software and devices that implement a particular model (or set of models) of HCI

Computer vision technologies must ultimately deliver a better "user experience".

B Shneiderman, Designing the User Interface: Strategies for Effective Human-Computer Interaction, Third Edition, Addison-Wesley, 1998.
: 1) time to learn 2) speed of performance 3) user error rates 4) retention over time 5) subjective satisfaction

- Presence and location (Face and body detection, head and body tracking)
- Identity (Face recognition, gait recognition)
- Expression (Facial feature tracking, expression modeling and analysis)
- Focus of attention (Head/face tracking, eye gaze tracking)
- Body posture and movement (Body modeling and tracking)
- Gesture (Gesture recognition, hand tracking)
- Activity (Analysis of body movement)

eg.
VIDEOPLACE (M W Krueger, Artificial Reality II, Addison-Wesley, 1991)
Magic Morphin Mirror / Mass Hallucinations (T Darrell et al., SIGGRAPH Visual Proc, 1997)

Principal Component Analysis (PCA)
Linear Discriminant Analysis (LDA)
Gabor Wavelet Networks (GWNs)
Active Appearance Models (AAMs)
Hidden Markov Models (HMMs)

Identix Inc.
Viisage Technology Inc.
Cognitec Systems


- MIT Medial Lab
ALIVE system (P Maes et al., The ALIVE system: wireless, full-body interaction with autonomous agents, ACM Multimedia Systems, 1996)
PFinder system (C R Wren et al., Pfinder: Real-time tracking of the human body, IEEE Trans PAMI, pp 780-785, 1997)
KidsRoom project (A Bobick et al., The KidsRoom: A perceptually-based interactive and immersive story environment, PRESENCE: Teleoperators and Virtual Environments, pp 367-391, 1999)

flocking behavior

http://www.movesinstitute.org/~kolsch/HandVu/HandVu.html

// 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;
}

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]

// 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;
}

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