利用加速度传感器模糊检测设备被人为移动的算法

项目需要做一个安全装置盒，防止人为移动，破坏。选用一款数字式3轴加速度传感器，原思路是通过加速度传感器的3轴加速度值进行二次积分获取设备的移动位置，通过位置判断设备是否被移动；通过瞬时加速度值来判断设备是否为强力破坏；
经过测试发现加速度本身噪声很大，二次积分累计误差很大，重力加速度值会根据位置不同3轴有偏差等诸多问题，不得不根据实际情况进行简化模型，加速度进行10次平均滤波（采样时间为100ms）；
将整个运动分成2种状态，稳定态与移动态，通过连续n次3个方向加速度微分值进行判断当前状态，假定n为5次判定，连续5次3个方向加速度微分值都小于设定阈值，则认为当前处于稳定状态，将最后一次加速度值作为当前重力加速度，随后进行重力加速度平均，若出现3个方向加速度微分值大于阈值，则当前处于移动态，使用以前重力加速度进行计算，开始进行两次积分，获取位置信息。
移动态时加速度与重力加速度进行比较，将两次变化小于3的值强制变成0，进一步滤掉微小误差。在人为移动装置时候很难出现人为匀速运动的情况，故在加速度为0的时候，将速度置为0。
最后根据位置变化输出设备被人为移动报警信息。

#include 
#include 
#include  
#include 
#include"libfahw.h"

#define BUF_SIZE            32
#define WEND_MAX            5
#define PWEND_MAX           5
#define WEND_CI             5
void stringtoxyz(char st[],int *a,int *b,int *c);
//基本思路 判断处于动态还是稳态 5次加速度值很接近表示稳态 稳态下累计计算g值 出现动态时以g值作为基本参考 计算二次积分 过程中计算稳态，稳态判定成功重新累计g值
//计算平均值时加速度
int ax2=0,ay2=0,az2=0;
//重力加速度
int gx=0,gy=0,gz=0;
//累计位置信息
int px=0,py=0,pz=0;
//累计速度信息
int vx=0,vy=0,vz=0;
//前一次加速度信息
int preax=0,preay=0,preaz=0;
//前一次速度信息
int prevx=0,prevy=0,prevz=0;
//前一次位置信息
int prepx=0,prepy=0,prepz=0;
//稳定计数值
int wendcnt=0;
//在稳态5次之后重新矫正重力加速度
void SteadystateCal()
{
    if((abs(preax-ax2)<=WEND_MAX&&abs(preay-ay2)<=WEND_MAX)&&abs(preaz-az2)<=WEND_MAX)
    {
        wendcnt++;
        if(wendcnt==WEND_CI)
        {
            gx = ax2;
            gy = ay2;
            gz = az2;
        }
        else if(wendcnt>WEND_CI)
        {
            gx = (gx+ax2)/2;
            gy = (gy+ay2)/2;
            gz = (gz+az2)/2;
        }
    }
    else 
    {
        wendcnt=0;
    }
}


int main (int argc, char* argv[])
{
    char position[BUF_SIZE];
    int board;
    int ax,ay,az;
    int i;
    int j=0;

    if ((board = boardInit()) < 0) {
        printf("Fail to init board\n");
        return -1;
    }
    system("modprobe adxl34x");
    system("modprobe adxl34x-i2c");


    for(i=0;i<10;i++)
    {
        usleep(100*1000);
        memset(position, 0, BUF_SIZE);
        if (adxl34xRead(position) > 0) 
        {
            stringtoxyz(position,&ax,&ay,&az);
            //去掉重力加速度
            ax2 = ax2+ax;
            ay2 = ay2+ay;
            az2 = az2+az;
        }
    }
    gx = ax2/i;
    gy = ay2/i;
    gz = az2/i;

    while(1)
    {
        ax2 = 0;
        ay2 = 0;
        az2 = 0;
        for(i=0;i<10;i++)
        {
            usleep(100*1000);
            memset(position, 0, BUF_SIZE);
            if (adxl34xRead(position) > 0) 
            {
                stringtoxyz(position,&ax,&ay,&az);
                //去掉重力加速度
                ax2 = ax2+ax;
                ay2 = ay2+ay;
                az2 = az2+az;
            }
        }
        ax2 = ax2/10;
        ay2 = ay2/10;
        az2 = az2/10;
        SteadystateCal();

        //计算水平方向位移 时间为1s
        if(wendcntif(abs(ax2-gx)0;
            }
            else
            {
                ax2 = ax2-gx;
            }
            if(abs(ay2-gy)0;
            }
            else
            {
                ay2 = ay2-gy;
            }
            if(abs(az2-gz)0;
            }
            else
            {
                az2 = az2-gz;
            }
            if(ax2!=0)
            {
                vx = vx + preax + (ax2-preax)/2;
                preax = ax2;
                px = px + prevx + (vx-prevx)/2;
                prevx = vx;
            }
            else
            {
                vx = 0;
                preax = ax2;
                prevx = vx;
            }

            if(ay2!=0)
            {
                vy = vy + preay + (ay2-preay)/2;
                preay = ay2;
                py = py + prevy + (vy-prevy)/2;
                prevy = vy;
            }           
            else
            {
                vy = 0;
                preay = ay2;
                prevy = vy;
            }
            if(az2!=0)
            {
                vz = vz + preaz + (az2-preaz)/2;
                preaz = az2;
                pz = pz + prevz + (vz-prevz)/2;
                prevz = vz;
            }
            else
            {
                vz = 0;
                preaz = az2;
                prevz = vz;
            }       
        }
        if(j>5)
        {
            if((abs(prepx-px)>PWEND_MAX||abs(prepy-py)>PWEND_MAX)||abs(prepz-pz)>PWEND_MAX)
            {
                //写入报警标记及报警级别
                printf("position %d,%d,%d\n", px-prepx,py-prepy,pz-prepz);

                prepx = px;
                prepy = py;
                prepz = pz;
            }
            j=0;
        }
        else
        {
            j++;
        }



    }

    system("rmmod adxl34x-i2c");
    system("rmmod adxl34x");
    return 0;
}
void stringtoxyz(char st[],int *a,int *b,int *c)
{
    int i,j=0,k;
    //char st[30]="(78, 276, -78)";
    char sa[10],sb[10],sc[10];
    //sscanf("(78, 276, -78)","%s,%s,%s",sa,sb,sc);
    k=0;
    for(i=0;i<strlen(st);i++)
    {
        if(('0'<=st[i]&&st[i]<='9')||st[i]=='-')
        {
            if(k==0)
            {
                sa[j]=st[i];
            }
            else if(k==1)
            {
                sb[j]=st[i];
            }
            else if(k==2)
            {
                sc[j]=st[i];
            }
            j++;
        }
        else if(st[i]==','||st[i]==')')
        {
            if(k==0)
            {
                sa[j]='\0';
            }
            else if(k==1)
            {
                sb[j]='\0';
            }
            else if(k==2)
            {
                sc[j]='\0';
            }
            j=0;
            k++;
        }
    }
    *a = atoi(sa);
    *b = atoi(sb);
    *c = atoi(sc);
}