论坛 › C/C++ › 陀螺仪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, y, z;
        double sum;
        size_t index;
};
void rawdata_buf_append( struct rawdata_buf* raw, double x, double y, double z )
{
        raw->sum += -raw->x+x;
        raw->sum += -raw->y+y;
        raw->sum += -raw->z+z;
        raw->x = x;
        raw->y = y;
        raw->z = 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, raw->y, raw->z );
        putchar( '\n' );
#endif

        *x = raw->sum/5;
        *y = raw->sum/5;
        *z = raw->sum/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;
                }
                inited = true;
        }
        else
        {
                sum -= (a - newNum);
                a = newNum;
                idx %= size;
        }

        return sum / size;
}

查看完整版本: 陀螺仪raw data 滤波