Chinaunix
标题:
陀螺仪raw data 滤波
[打印本页]
作者:
shihyu
时间:
2016-08-09 02:07
标题:
陀螺仪raw data 滤波
本帖最后由 shihyu 于 2016-08-09 02:08 编辑
我想把 陀螺仪raw data 做滤波 , 每五笔做个平均
陀螺仪raw data 每笔数据有 x , y, z 三个轴的数值
第一次数值是前五笔(1~5)数据平均值
第二次数值是2~7 数据平均值
第三次数值是3~8 数据平均值
之后依此类推
陀螺仪raw data 是不断更新进来 , 五笔不断更新搬动数据 , 如果用 array 存放搬动数据感觉效率 , 会是用 linked list
因为代码要放在 stm32 板子上所以需要考量到效率问题 , 不知道怎么更新存放这些数据效率比较好?
作者:
xinglp
时间:
2016-08-09 08:07
Round Robin
作者:
bruceteen
时间:
2016-08-09 08:38
每五笔做个平均
第一次数值是前五笔(1~5)数据平均值
第二次数值是2~7 数据平均值 ------ 按照规律,应该是 2-6 吧
第三次数值是3~8 数据平均值 ------ 按照规律,应该是 3-7 吧
#include <stdio.h>
#include <string.h>
struct rawdata_buf
{
double x[5], y[5], z[5];
double sum[3];
size_t index;
};
void rawdata_buf_append( struct rawdata_buf* raw, double x, double y, double z )
{
raw->sum[0] += -raw->x[raw->index]+x;
raw->sum[1] += -raw->y[raw->index]+y;
raw->sum[2] += -raw->z[raw->index]+z;
raw->x[raw->index] = x;
raw->y[raw->index] = y;
raw->z[raw->index] = z;
raw->index = (raw->index+1)%5;
}
void rawdata_buf_average( struct rawdata_buf* raw, double* x, double* y, double* z )
{
#ifdef _DEBUG
for( size_t i=raw->index; i!=raw->index+5; ++i )
printf( "(%g,%g,%g)", raw->x[i%5], raw->y[i%5], raw->z[i%5] );
putchar( '\n' );
#endif
*x = raw->sum[0]/5;
*y = raw->sum[1]/5;
*z = raw->sum[2]/5;
}
int main( void )
{
double avgx, avgy, avgz;
struct rawdata_buf buf = { 0 };
rawdata_buf_append( &buf, 0,0,0 );
rawdata_buf_append( &buf, 1,1,1 );
rawdata_buf_append( &buf, 2,2,2 );
rawdata_buf_append( &buf, 3,3,3 );
rawdata_buf_append( &buf, 4,4,4 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
rawdata_buf_append( &buf, 5,5,5 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
rawdata_buf_append( &buf, 6,6,6 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
rawdata_buf_append( &buf, 7,7,7 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
rawdata_buf_append( &buf, 8,8,8 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
rawdata_buf_append( &buf, 9,9,9 );
rawdata_buf_average( &buf, &avgx, &avgy, &avgz );
printf( "avg = (%g,%g,%g)\n", avgx, avgy, avgz );
return 0;
}
复制代码
作者:
shihyu
时间:
2016-08-09 09:28
五筆是最新五筆 ,太舊數據就捨棄
作者:
karma303
时间:
2016-08-09 10:46
路过帮顶~~~
作者:
cokeboL
时间:
2016-08-09 10:57
float calc(float a[], int size, int newNum)
{
static bool inited = false;
static float sum = 0;
static int idx = 0;
if(!inited)
{
for(int i = 0; i < size; i++)
{
sum += a[i];
}
inited = true;
}
else
{
sum -= (a[idx] - newNum);
a[idx++] = newNum;
idx %= size;
}
return sum / size;
}
复制代码
欢迎光临 Chinaunix (http://bbs.chinaunix.net/)
Powered by Discuz! X3.2