본문 바로가기

Enginius/C / C++

Variance Estimator

1. Variance Estimator 구조체
typedef struct _VarianceEstimator
{
// Data used of calculating variance
int array_size;
float *data_array;

// Samlping period
unsigned int period_ms;
unsigned long long tick_previous;
// Estimated Variance 
float variance; // var(x) = E(x^2) - (E(x))^2
// Function
float (*VarianceEstimate)(struct _VarianceEstimator* this, float input);
} VarianceEstimator;
void VarianceEstimator_constructor(VarianceEstimator* this, int size, int period_ms )
{
int i; // index
// Argument Handle
this->array_size = size;
this->period_ms = period_ms;
// Initialize Structure
this->data_array = (float *)malloc(size * sizeof(float));
if( this->data_array == 0 )
myprintf("[error] memory allocation fail \r\n");
else
{
for(i=0; i<size; i++)
this->data_array[i] = 0.0;
}
this->variance = 0;
this->tick_previous = g_Tick2;
// Linking function
this->VarianceEstimate = VarianceEstimate_define;
}
float VarianceEstimate_define(VarianceEstimator* this, float input)
{
int i; // indiex
int size; // array size
float average, square_average;
if( g_Tick2 - this->tick_previous > this->period_ms)
{// Every "period_ms" 
// 1. Store previous tick
this->tick_previous = g_Tick2;
size = this->array_size;
// 2. Calculate E(X) and E(X^2)
average = 0;
square_average = 0;
for(i=0; i<(this->array_size)-1; i++)
{
this->data_array[i] = this->data_array[i+1];
average = average + this->data_array[i+1];
square_average = square_average + this->data_array[i+1]*this->data_array[i+1];
}
this->data_array[size-1] = input;
// 2.a E(X)
average = average + input;
average = average / size;
// 2.b E(X^2)
square_average = square_average + input*input;
square_average = square_average / size;
// 3. Calculate variance: Var(X) = E(X^2) - E(X)^2
this->variance = square_average - average*average;
}
return this->variance;
}

2. Checking slip using motor currents
// Variance of R, L Motor Current 
VarianceEstimator RightMotorCurrVarianceEstimator, LeftMotorCurrVarianceEstimator;
unsigned long long tick_CurrSlipCheck = 0, tick_OverSpeed = 0;
void Initialize_CurrSlipCheck()
{
// Initialize Variance Estimator
VarianceEstimator_constructor(&RightMotorCurrVarianceEstimator, 50, 20); // 50ms period *20 size = 1sec 
VarianceEstimator_constructor(&LeftMotorCurrVarianceEstimator, 50, 20); // 50ms period *20 size = 1sec 
tick_CurrSlipCheck = g_Tick2;
}
unsigned char CurrSlipCheck(unsigned char flag)
{
unsigned char ret = 0, dontcheck = 0;
float RmotorCurr, LmotorCurr, MaxMotorCurrentVariace, RightMotorVariance, LeftMotorVariance;
// 1. Get Motor Current
RmotorCurr = g_RmotorCurr;
LmotorCurr = g_LmotorCurr;
// 2. Check slip only when the speed has been maintained over certain speed for certain period of time. 
if(g_Vcm < 10)
{
tick_OverSpeed = g_Tick2;
RmotorCurr = 0;
LmotorCurr = 0;
dontcheck = 1;
}
else if( g_Tick2 - tick_OverSpeed < 2000 )
{
RmotorCurr = 0;
LmotorCurr = 0;
dontcheck = 1;
}
// 3. Make Motor Current over 100.0 (Filtering out currens less then 100.0)
if(g_RmotorCurr < 100.0) RmotorCurr = 100.0;
if(g_LmotorCurr < 100.0) LmotorCurr = 100.0;
// 4. Calcualte variance of motor current. 
RightMotorVariance = RightMotorCurrVarianceEstimator.VarianceEstimate(&RightMotorCurrVarianceEstimator, RmotorCurr);
LeftMotorVariance = LeftMotorCurrVarianceEstimator.VarianceEstimate(&LeftMotorCurrVarianceEstimator, LmotorCurr);
// 5. Make variances under 300.0
if(RightMotorVariance > 300.0) RightMotorVariance = 300.0;
if(LeftMotorVariance > 300.0) LeftMotorVariance = 300.0;
// 6. Get Max current variance
if(RightMotorVariance > LeftMotorVariance) MaxMotorCurrentVariace = RightMotorVariance;
else    MaxMotorCurrentVariace = LeftMotorVariance;
// 7. Calculate elapsed time from the time when motor current's variance exceeds certain threshold. 
unsigned int tick_elapased ;
if( MaxMotorCurrentVariace > 200 && !dontcheck)
tick_elapased = g_Tick2 - tick_CurrSlipCheck;
else
{
tick_elapased = 0;
tick_CurrSlipCheck = g_Tick2;
}
// 8. Check slip by elapsed time (3sec)
if(tick_elapased > 2000)
{
ret = (unsigned char)(tick_elapased/1000);
}
// Debug
static unsigned long long tick_prev = 0;
if( g_Tick2 - tick_prev > 500 && flag)
{// print every 500ms
tick_prev = g_Tick2;
// RmotorCurr LmotorCurr RightMotorVariance LeftMotorVariance MaxMotorCurrentVariace tick_elapased
PrintF(RmotorCurr, 1); TAB; PrintF(LmotorCurr, 1);  TAB;
PrintF(RightMotorVariance, 1); TAB; PrintF(LeftMotorVariance, 1);  TAB;
PrintF(MaxMotorCurrentVariace, 1); TAB;
PrintI(tick_elapased); 
ENTER;
}
// 9. return elapsed slip time
return ret;
}

3. Usage
unsigned char CurrSlip;
CurrSlip = CurrSlipCheck(print_flag);
if(  CurrSlip )
{
// 슬립 처리 

 

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

console로 print할 때 주의사항  (0) 2012.03.26
linked list class  (0) 2012.03.23
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