간단한 칼만 필터(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);
[관련 포스트]
댓글
댓글 쓰기