最小二乘法

#define Sample_Num 20

float value_buf[Sample_Num]={0}；

static int cnt = 0;

if(cnt >= Sample_Num)

cnt = 0;

//更新滑动窗口数组

if(cnt

value_buf[cnt] = (flaot)(angle);

//窗口滑动++

cnt++;

float Steer_analyse(void)

{

static float Slope=0；

static float X_Sum=Sample_Num*(Sample_Num-1)/2;

float Y_Sum=0,XX_Sum=0,YY_Sum=0,XY_Sum=0;

/***********************最小二乘法算斜率*********************/

for(int x=Sample_Num-1;x>=0;x--)

{

XY_Sum += (Sample_Num-1-x)*value_buf[x];//X*Y的和

Y_Sum += value_buf[x]; //y的和

XX_Sum += x*x; //x的平方和

}

Slope = (Sample_Num*XY_Sum-X_Sum*Y_Sum)

/(Sample_Num*XX_Sum-X_Sum*X_Sum);

//(采样个数乘以X*Y的和 - x的和乘以y的和)除以(采样个数乘以

//x的平方和 - x和的平方

return Slope；

}

#define Sample_Num 20

float value_buf[Sample_Num]={0}；

static int cnt = 0;

if(cnt >= Sample_Num)

cnt = 0;

//更新滑动窗口数组

if(cnt

value_buf[cnt] = (flaot)(angle);

//窗口滑动++

cnt++;

float Steer_analyse(void)

{

static float Slope=0；

static float X_Sum=Sample_Num*(Sample_Num-1)/2;

float Y_Sum=0,XX_Sum=0,YY_Sum=0,XY_Sum=0;

/***********************最小二乘法算斜率*********************/

for(int x=Sample_Num-1;x>=0;x--)

{

XY_Sum += (Sample_Num-1-x)*value_buf[x];//X*Y的和

Y_Sum += value_buf[x]; //y的和

XX_Sum += x*x; //x的平方和

}

Slope = (Sample_Num*XY_Sum-X_Sum*Y_Sum)

/(Sample_Num*XX_Sum-X_Sum*X_Sum);

//(采样个数乘以X*Y的和 - x的和乘以y的和)除以(采样个数乘以

//x的平方和 - x和的平方

return Slope；

}