칼만 필터(Kalman Filter)

칼만 필터는 부정확한 측정값(관찰, 혹은 예측값)으로부터 오차를 최소화 하는 추정치를 반복적으로 

계산하는 방법이다. 가우시안 잡음(Gaussian noise)인 경우에 최적의 추정량을 구할 수 있다.

Wan, Eric A., and Rudolph Van Der Merwe. "The unscented Kalman filter for nonlinear estimation." 

Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000. AS-SPCC. 

The IEEE 2000. IEEE, 2000.

칼만 필터 프로세스 방정식과 측정 방정식은 다음과 같다.

칼만 필터는 초기화 이후 예측(predict) 위 단계를 계속 반복하면서 추정치를 구한다.

측정 잡음의 공분산 R이 아주 크면, 칼만이득 K가 작아져서 다음 단계의 추정치를 계산할 때 현재의 측정값이 무시된다.

#include <cv.h>
#include <highgui.h>
#include <iostream>
 
using namespace std;
using namespace cv;
 
int main()
{
    // Greq Welch and Gary Bishop, "An Introduction to the Kalman Filter", 2006.
    // Estimating a Random Constant
 
    IplImage *dstImage = cvCreateImage(cvSize(512, 512), IPL_DEPTH_8U, 3);
    cvSet(dstImage, CV_RGB(255, 255, 255));
 
    IplImage *pImage = cvCreateImage(cvSize(512, 512), IPL_DEPTH_8U, 3);
    cvSet(pImage, CV_RGB(255, 255, 255));
 
    cvNamedWindow("dstImage");
    cvNamedWindow("pImage");
 
    CvRNG rng_state = cvRNG(0xffffffff);
 
    int t, count = 50;
    double x = -0.37727;
 
    int step = dstImage->width / count;
 
    CvMat *z = cvCreateMat(count, 1, CV_32FC1);
    cvRandArr(&rng_state, z, CV_RAND_NORMAL, cvRealScalar(x), cvRealScalar(0.1));
 
    ////////////////////////////////
    //        kalman filter       //
    ////////////////////////////////
    double Q = 1e-5; // process variance
    double R = (0.1) * (0.1); // estimate of measurement variance
    //double R = 1.0; // estimate of measurement variance
    //double R = 0.0001; // estimate of measurement variance
 
    CvMat *xhat = cvCreateMat(count, 1, CV_32FC1); // posteriori estimate of x
    CvMat *P = cvCreateMat(count, 1, CV_32FC1); // posteriori error estimate 
    CvMat *xhatminus = cvCreateMat(count, 1, CV_32FC1); // priori estimate of x
    CvMat *Pminus = cvCreateMat(count, 1, CV_32FC1); // priori error estimate
    CvMat *K = cvCreateMat(count, 1, CV_32FC1); // kalman gain
 
    // to access as an array in 1D matrix
    float *xhatA = xhat->data.fl;
    float *PA = P->data.fl;
    float *xhatminusA = xhatminus->data.fl;
    float *PminusA = Pminus->data.fl;
    float *KA = K->data.fl;
    float *zA = z->data.fl;
 
    xhatA[0] = 0.0;
    PA[0] = 1.0;
 
    for (t = 1; t < count; t++)
    {
        //predict
        xhatminusA[t] = xhatA[t - 1];
        PminusA[t] = PA[t - 1] + Q;
 
        //update
        KA[t] = PminusA[t] / (PminusA[t] + R);
        xhatA[t] = xhatminusA[t] + KA[t] * (zA[t] - xhatminusA[t]);
        PA[t] = (1 - KA[t]) * PminusA[t];
    }
 
 
    //drawing values
    double minVal, maxVal;
    cvMinMaxLoc(z, &minVal, &maxVal);
    double scale = dstImage->height / (maxVal - minVal);
    printf("observations, z : minVal = %f, maxVal = %f\n", minVal, maxVal);
 
    //drawing the truth value, x = -0.37727
    CvPoint pt1, pt2;
    pt1.x = 0;
    pt1.y = dstImage->height - cvRound(scale * x - scale*minVal);
 
    pt2.x = dstImage->width;
    pt2.y = dstImage->height - cvRound(scale * x - scale*minVal);
    cvLine(dstImage, pt1, pt2, CV_RGB(0, 0, 255), 2);
 
    //drawing the noisy measurements, observations, z
    for (t = 0; t < count; t++)
    {
        pt1.x = t*step;
        pt1.y = dstImage->height - cvRound(scale * zA[t] - scale * minVal);
        cvCircle(dstImage, pt1, 3, CV_RGB(255, 0, 0), 2);
    }
 
    //drawing the filter estimate, xhat
    pt1.x = 0;
    pt1.y = dstImage->height - cvRound(scale * xhatA[0] - scale * minVal);
 
    for (t = 1; t < count; t++)
    {
        pt2.x = t*step;
        pt2.y = dstImage->height - cvRound(scale * xhatA[t] - scale * minVal);
        cvLine(dstImage, pt1, pt2, CV_RGB(0, 255, 0), 2);
        pt1 = pt2;
    }
 
    //drawing the error covariance, P
 
    cvMinMaxLoc(P, &minVal, &maxVal);
    scale = pImage->height / (maxVal - minVal);
    printf("error covariance, P : minVal = %f, maxVal = %f\n", minVal, maxVal);
 
    pt1.x = 0;
    pt1.y = pImage->height - cvRound(scale * PA[0] - scale * minVal);
 
    for (t = 1; t < count; t++)
    {
        pt2.x = t*step;
        pt2.y = pImage->height - cvRound(scale * PA[t] - scale*minVal);
        //printf("PA[%d] = %f\n", t, PA[t]);
        cvLine(pImage, pt1, pt2, CV_RGB(255, 0, 0), 2);
        pt1 = pt2;
    }
 
    cvShowImage("dstImage", dstImage);
    cvShowImage("pImage", pImage);
 
    cvSaveImage("dstImage.jpg", dstImage);
    cvSaveImage("pImage.jpg", pImage);
    
    cvWaitKey(0);
 
    cvReleaseMat(&z);
    cvReleaseMat(&xhat);
    cvReleaseMat(&P);
    cvReleaseMat(&xhatminus);
    cvReleaseMat(&Pminus);
    cvReleaseMat(&K);
 
    cvReleaseImage(&dstImage);
    cvReleaseImage(&pImage);
}