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 )
{

// 슬립 처리 

}