implementing function in 'struct' using C language(not C++)

Posted 2012.02.07 17:55

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 2012.02.14 2012.02.07 2011.12.30 2011.11.25 2011.11.07
« PREV : 1 : ··· : 15 : 16 : 17 : 18 : 19 : 20 : 21 : 22 : 23 : ··· : 37 : NEXT »