In Embedded Programming, such as "AVR" and "PIC", we often can only use C language only(not C++). But knowing how useful and convenient OOP, I googled about how to implement 'member function-like' feature using structure and linking function address. 

 This example is about implementing 1-D Kalman Filter. 

1. First, declare structure including necessary member variables and function. 
 typedef struct _KalmanFilter
{
// Kalman Filter Coefficient Define
float Q;
float R;
// Kalman1 Filter Variables
float x_hat_k;
float x_hat_k_minus;
float x_hat_k_bar;
float P_k_minus;
float P_k_bar;
float K_k;
// Kalman Filter Initial value
float P_k;
int FilterFlag;

// Input
float z_k;

float (*KalmanFiltering)(struct _KalmanFilter* this);
}KalmanFilter;

2. Second, define constructor-like function 
void KalmanFilter_constructor(KalmanFilter* this)
{
this->Q = 0.0001;
this->R = 0.0005;
// Kalman1 Filter Variables
this->x_hat_k = 0;
this->x_hat_k_minus = 0;
this->x_hat_k_bar = 0;
this->P_k_minus = 0;
this->P_k_bar = 0;
this->K_k = 0;
// Kalman Filter Initial value
this->P_k = 0;
this->FilterFlag = 0;
// Input
this->z_k = 0;
this->KalmanFiltering = KalmanFiltering_define;
return;
}

3. Third, define member function declared in the structure
float KalmanFiltering_define(KalmanFilter* this)
{
//Time update equations
this->x_hat_k_bar = this->x_hat_k_minus;
this->P_k_bar = this->P_k_minus + this->Q;

//Measurement undate equations
this->K_k = this->P_k_bar/(this->P_k_bar+this->R);
this->x_hat_k = this->x_hat_k_bar + this->K_k*(this->z_k - this->x_hat_k_bar);
this->P_k = (1-this->K_k)*this->P_k_bar;

//k -> k-1
this->x_hat_k_minus = this->x_hat_k;
this->P_k_minus = this->P_k;

if(!this->FilterFlag)
{//초기화
this->FilterFlag = 1;
return this->z_k;
}
return this->x_hat_k;
}

4. How to use 
// step 1. declare structure-type variables
KalmanFilter AccX_KalmanFilter;
KalmanFilter AccY_KalmanFilter;
KalmanFilter AccZ_KalmanFilter;

// step 2. Initialize structure
KalmanFilter_constructor(&AccX_KalmanFilter);
KalmanFilter_constructor(&AccY_KalmanFilter);
KalmanFilter_constructor(&AccZ_KalmanFilter);
 
while (1)
{
...
        // step 3.  Kalman Filter input setting 
        AccX_KalmanFilter.z_k = (float)g_ACC_X;
        AccY_KalmanFilter.z_k = (float)g_ACC_Y;
        AccZ_KalmanFilter.z_k = (float)g_ACC_Z; 

         // step 4. Kalman Filtering
        g_ACC_F_X = AccX_KalmanFilter.KalmanFiltering(&AccX_KalmanFilter);
        g_ACC_F_Y = AccY_KalmanFilter.KalmanFiltering(&AccY_KalmanFilter);
         g_ACC_F_Z = AccZ_KalmanFilter.KalmanFiltering(&AccZ_KalmanFilter);  
...
}

5. Filtered Result






 
저작자 표시 비영리 동일 조건 변경 허락
신고

'Enginius > C / C++' 카테고리의 다른 글

Variance Estimator  (0) 2012.02.19
using enum in switch()  (0) 2012.02.14
implementing function in 'struct' using C language(not C++)  (0) 2012.02.07
Listbox control 사용하기  (0) 2011.12.30
synchronization문제  (0) 2011.11.25
길찾기 프로그램 class documentation  (0) 2011.11.07
« PREV : 1 : ··· : 364 : 365 : 366 : 367 : 368 : 369 : 370 : 371 : 372 : ··· : 587 : NEXT »