간단한 칼만 필터(Kalman Filter) 소스 코드와 사용 예제

칼만 필터(Kalman filter)는 노이즈가 포함된 측정치로 부터 실체 상태를 추정(推定, estimation)하는 도구다. 본 포스트의 칼만 필터 소스는 한 타겟의 위치를 추정하는 용도로 만들어 졌다. 

위키피디아 페이지(https://en.wikipedia.org/wiki/Kalman_filter)와 “Human Target Tracking in Multistatic Ultra-Wideband Radar” 논문 등을 참조하여 칼만 필터의 공식을 구현하였다.

F,H,Q,R matrix는 아래와 같은 값으로 정하여 사용하였다.

칼만 필터 c++ 소스는 아래와 같다. 

#include <malloc.h>

#include <Eigen/Dense>

__declspec(align(16)) class CKalmanFilter

{

private:

BOOL mFirst;

Eigen::MatrixXd mX;

Eigen::Matrix4d mP;

Eigen::Matrix4d mF;

Eigen::MatrixXd mH;

Eigen::Matrix2d mR; // measurement noise covariance

Eigen::Matrix4d mQ; // process noise covariance

public:

void* operator new(size_t i)

{

return _mm_malloc(i, 16);

}

void operator delete(void* p)

{

_mm_free(p);

}

CKalmanFilter(double process_var, double measure_var, double T)

{

Eigen::MatrixXd x(4, 1);

Eigen::MatrixXd h(2, 4);

mFirst = TRUE;

mX = x;

mX << 0, 0, 0, 0;

mP = Eigen::Matrix4d::Identity();

mF = Eigen::Matrix4d::Identity();

mF(0, 1) = T;

mF(2, 3) = T;

mH = h;

mH << 1, 0, 0, 0,

0, 0, 1, 0;

mR << measure_var, 0, 0, measure_var;

mQ = Eigen::Matrix4d::Zero();

mQ(1, 1) = pow(T, 2)*process_var;

mQ(3, 3) = pow(T, 2)*process_var;

}

~CKalmanFilter()

{

}

void filtering(double measure[2], double correct[2])

{

Eigen::MatrixXd Zkm1(2, 1);

Eigen::MatrixXd Zk(2, 1);

Eigen::MatrixXd S;

Eigen::MatrixXd K;

if (mFirst)

{

mX << measure[0], 0, measure[1], 0;

mFirst = FALSE;

}

Zk(0, 0) = measure[0];

Zk(1, 0) = measure[1];

// Prediction of state estimation :

// X(k|k-1) = F * X(k|k?1)

mX = mF*mX;

// Prediction of error covariance:

//P( k|k?1) = F*P(k?1|k?1)*FT + Q(k)

mP = mF*mP*mF.transpose() + mQ;

//Prediction of observation

//z(k|k?1) = H*x(k|k?1)

Zkm1 = (mH*mX);

//Compute innovation covariance:

//S(k) = H*P(k|k?1)*HT + R(k)

S = mH*mP*mH.transpose() + mR;

//Kalman’s gain:

//K(k) = P(k|k?1)*HT*invS(k)

K = mP*mH.transpose()*S.inverse();

//  State estimation update:

// x^(k|k) = x?(k|k?1) + K(k)*[z(k) ? z(k|k?1)]

mX = mX + (K*(Zk - Zkm1));

// Error covariance update:

//P(k|k) = [I ? K(k)*H]*P(k|k?1)

mP = (Eigen::Matrix4d::Identity() - (K*mH))*mP;

correct[0] = mX(0, 0);

correct[1] = mX(2, 0);

}

};

matrix 연산을 위해 Eigen 라이브러리를 사용하였으며, class 의 operator new/delete는 visual c/c++에서 C4316 warning을 피하기 위해 추가했다. (https://stackoverflow.com/questions/20104815/warning-c4316-object-allocated-on-the-heap-may-not-be-aligned-16).

Class 생성자 파라메터

• double process_var - process noise variance
• double measure_var - measurement noise variance
• double T: 위치 측정 시간 간격 Δt seconds.

filtering 함수 파라메터

• double measure[2] - measured location data, measure[0]=x, measure[1]=y
• double correct[2] -  estimated location data, x=correct[0], y=correct[1]

아래 비디오는 위 칼만 필터 소스를 사용해 만든 예제이며, 마우스 위치를 측정 데이터(붉은색 선)로 사용하여 위치를 추정(녹색 선)한다. 

위 예제에서 사용된 칼만 필터는 아래와 같이 사용 되었다.

• 생성

CKalmanFilter *kalmanfilter = NULL;

kalmanfilter = new CKalmanFilter(9.0, 1.0, 0.01);

• 필터링

double measure[2] = { 0, };

double correct[2] = { 0, };

measure[0] = (double)x;

measure[1] = (double)y;

kalmanfilter->filtering(measure, correct);

[관련 포스트]

쉽게 설명한 파티클 필터(particle filter) 동작 원리와 예제
Eigen 라이브러리 matrix 사용법