低通滤波算法在加速度传感器应用中的作用

在现代系统中，加速度传感器越来越多的应用到系统中，在很多应用中需要考虑加速度传感器是不稳定性，用数字低通滤波技术可以过滤掉一些杂波，将有特征的加速度变化趋势提现出来。有着十分广泛的应用。如在计步器，移动检测等技术中广发使用该技术。以下是低通一个计步器的实现算法，通过三轴加速传感器的数据的提取，并进行低通滤波处理。从而判定人行走的状态：

#include "Counter.h"
#include "main.h"
#include "iodefine.h"
#include "LcdDisp.h"
#include "keydeal.h"
/***********************************************
* Copyright (c) 2007,ShenZhen Namtek CO.,LTD
* All rights reserved.
* 
* file name:Counter.c
* :
* recapitulation:
* 
* edition:1.0
* author: Dts
* create date: 2005-04-17
* modify:   2005-08-15
* 2005-09-06    dts modify calculate calorie and fat, use double data type.
* calculate time about 6.2ms
************************************************/


volatile StepCounter g_StepCounter;


static uint8_t ucSqrtResIndex;
static uint16_t uiSqrtResBuf[PHASE1BUF_MAX];
/* H(N)*/
static uint32_t ulLPFResultPrev;
static uint32_t ulLPFResultCurr;
/* I(n) */


static uint8_t  ucRiseFlagPrev;
static uint8_t ucRiseFlagCurr;
/* K(n) */


static uint32_t ulPeakHoldPrev;
static uint32_t ulPeakHoldCurr;
/* M */
static uint8_t ucValidPeakPrev;
static uint8_t ucValidPeakCurr;
/* N */


static uint8_t ucValidPeakIndex;
static uint8_t ucValidPeakIndex1s;
static uint8_t  ucValidPeakBuf[PHASE6BUF_MAX];
static uint8_t  ucValidPeakIn1S[ONE_SECOND_COUNT];


/* P */
static uint8_t ucValidHalfStepPrev;
static uint8_t ucValidHalfStepCurr;
static uint8_t g_ucValidIn5Second;
static uint8_t g_ucValidIn1Second;
static uint32_t  g_ulTotalValidHalfStep;
static uint32_t g_ulOldStep;
static uint32_t g_ulCurrentStep;


static uint32_t g_ulTotalPoint;
static uint32_t g_ulContinuePoint =(uint32_t) 0;
static uint16_t g_uiContinueNoStepPoint;


// static Function define 
static void StepCounter_InputPoint(uint16_t pt[3]);
static void CalculateProc(void);
static void   CounterParamInit(void);
static void ResetStepCounter(void);
static void CalculateProc(void);
static void StepCounter_Init(void);


#ifdef PCVER
static uint16_t SqrtInt_T(uint32_t M);
#endif 




/*======================================================================
 Name:        void  CounterParamInit(void)
 Purpose:    
 Parameter: NONE
 mode:      
 Return:     NONE
 Remarks:    step counter parament initlization
 Change Log:
                     Author                     Date                      Description
                   -------------------- -------- ------------------------------------------------
                    dts 2005-04-18 Create
zyf    2005-7-15           modified   
dts     2005-09-13 modify   
====================================================================== */
static void  CounterParamInit(void)
{
uint8_t ucLoop = (uint8_t)0;
/* sqrt data buffer*/
ucSqrtResIndex = (uint8_t)0;

for (ucLoop = (uint8_t)0; ucLoop < PHASE1BUF_MAX; ucLoop++)
{
uiSqrtResBuf[ucLoop] = (uint16_t)0;
}

/* LPF */
ulLPFResultPrev = (uint32_t)0;
ulLPFResultCurr = (uint32_t)0;
/* decide wave is rise or drop */
ucRiseFlagPrev = (uint8_t)0;
ucRiseFlagCurr = (uint8_t)0;
/* wave peak decide */
ulPeakHoldPrev =  (uint32_t)0;
ulPeakHoldCurr =  (uint32_t)0;
ucValidPeakPrev = (uint8_t)0;
ucValidPeakCurr = (uint8_t)0;

/* save wave peak data into buffer */
ucValidPeakIndex= (uint8_t)0;
ucValidPeakIndex1s = (uint8_t)0;
for (ucLoop = (uint8_t)0; ucLoop < (uint8_t)PHASE6BUF_MAX; ucLoop++)
{
ucValidPeakBuf[ucLoop] = (uint8_t)0;
}

for(ucLoop = (uint8_t)0; ucLoop < ONE_SECOND_COUNT; ucLoop++)
{
ucValidPeakIn1S[ucLoop] = (uint8_t)0;
}


/* valid peak count */
ucValidHalfStepPrev = (uint8_t)0;
ucValidHalfStepCurr= (uint8_t)0;
/* auto start flag */
g_StepCounter.AutoStartFlag = (uint8_t)0;
g_ucValidIn5Second = (uint8_t)0;
g_ucValidIn1Second = (uint8_t)0;
g_ulTotalPoint = (uint32_t)0;
g_ulContinuePoint = (uint32_t)START_POINT;
g_StepCounter.SampleReadyF = (uint8_t)0;
g_uiContinueNoStepPoint = PHASE6BUF_MAX;
if(g_ulTotalValidHalfStep & 1)
{
g_ulTotalValidHalfStep--;
}
return;
}
/*======================================================================
 Name:        void  StepCounter_Init(void)
 Purpose:    
 Parameter: NONE
 mode:      
 Return:     NONE
 Remarks:    step counter initlization
 Change Log:
                     Author                     Date                      Description
                   -------------------- -------- ------------------------------------------------
                    dts 2005-04-18 Create
zyf    2005-7-15           modified   
dts     2005-09-13 modify   
====================================================================== */
static void StepCounter_Init(void)
{
/* step counter start up from 16HZ sample */
CounterParamInit();
g_StepCounter.CompensateState = COMPENSATE_START_AT_16HZ;
g_ulTotalValidHalfStep = (uint32_t)0;
g_System_Control.SamplingState = SAMPLE_STOP;
g_ulOldStep = (uint32_t)0;
g_ulCurrentStep = (uint32_t)0;
return;
}
static void ResetStepCounter()
{
CounterParamInit();
g_ulTotalValidHalfStep = (uint32_t)0;
g_StepCounter.AutoStartFlag=(uint8_t)0;
g_ulOldStep = (uint32_t)0;
g_ulCurrentStep = (uint32_t)0;

}
/*======================================================================
 Name: StepCounter_InputPoint(unsigned short pt[3])
 Purpose: calculate steps.
 Parameter: input X,Y,Z axis AD Sampling data
       
 Return:
 Remarks:     
 Change Log:
 Author: dts
 Date: 5/31/2005
====================================================================== */
static void StepCounter_InputPoint(uint16_t pt[3])
{
const static uint16_t coefficient[] =
{
1, 10, 45, 120, 210, 252
};
uint8_t j;
uint8_t   k;
uint8_t ucVal; 
uint8_t   ucValidSumPeak;
int16_t   i;
uint32_t point[3];
uint32_t ulDiffOfPeak;
    uint32_t   ulSum;
uint32_t ulTp;
uint32_t ulTp1;
uint32_t ulTp2;
g_ulTotalPoint++;
/* phase1 process */
point[0] = (uint32_t)((pt[0] > 512) ? (pt[0] - (uint16_t)512) : ((uint16_t)512 - pt[0]));
point[1] = (uint32_t)((pt[1] > 512) ? (pt[1] - (uint16_t)512) : ((uint16_t)512 - pt[1]));
point[2] = (uint32_t)((pt[2] > 512) ? (pt[2] - (uint16_t)512) : ((uint16_t)512 - pt[2]));
ulTp = point[0] * point[0] + point[1] * point[1] + point[2] * point[2];
// ulTp=ulTp*100; 
#ifdef PCVER
uiSqrtResBuf[ucSqrtResIndex] = SqrtInt_T(ulTp);
#else
uiSqrtResBuf[ucSqrtResIndex] = SqrtInt(ulTp);
#endif
/* G(n) */
ucSqrtResIndex ++;
if (ucSqrtResIndex == (uint8_t)PHASE1BUF_MAX)
{
ucSqrtResIndex = (uint8_t)0;
}
if (g_ulTotalPoint >= (uint32_t)PHASE1BUF_MAX)
{
g_ulContinuePoint++;
ulSum = (uint32_t)0;
for (i = 0; i <5; i++)
{
j = (uint8_t)((uint8_t)((uint8_t)(ucSqrtResIndex + (uint8_t)10) - (uint8_t)i) % PHASE1BUF_MAX);
k = (uint8_t)((uint8_t)((uint8_t)i + ucSqrtResIndex) % PHASE1BUF_MAX);
ulTp1 = (uint32_t)(uiSqrtResBuf[j] + uiSqrtResBuf[k]);
ulTp2 = (uint32_t)coefficient[i];
ulSum += ulTp1 * ulTp2;
} 
k = (uint8_t)((uint8_t)((uint8_t)5 + ucSqrtResIndex) % PHASE1BUF_MAX);
ulSum +=  ( uint32_t )uiSqrtResBuf[k] * (uint32_t)252;
/* H(n), phase2 process */
ulLPFResultPrev = ulLPFResultCurr;
ulLPFResultCurr = ulSum;
ucRiseFlagPrev = ucRiseFlagCurr;
if (ulLPFResultPrev > ulLPFResultCurr)
{
ucRiseFlagCurr = (uint8_t)0;
}
else
{
ucRiseFlagCurr = (uint8_t)1;
}
/* I(n), phase3 process */
if (ucRiseFlagPrev == ucRiseFlagCurr)
{
ucVal = (uint8_t)0;
}
else
{
ucVal = (uint8_t)1;
}
/* phase4 process */
ulPeakHoldPrev = ulPeakHoldCurr;
    if (ucVal == 1) 
{
ulPeakHoldCurr = ulLPFResultPrev;
if (ulPeakHoldCurr > ulPeakHoldPrev)
{
ulDiffOfPeak = ulPeakHoldCurr - ulPeakHoldPrev;
}
else
{
ulDiffOfPeak = ulPeakHoldPrev - ulPeakHoldCurr;
}
}
else
{
ulPeakHoldCurr = ulPeakHoldPrev;
ulDiffOfPeak = (uint32_t)0;
}
/* L(n) */
ucValidPeakPrev = ucValidPeakCurr;
ucValidPeakCurr = (uint8_t)(( ulDiffOfPeak < SMALL_NOISE_DATA_DISCARD ) ? 0 : 1); 
/* M(n) */
g_ucValidIn5Second -= ucValidPeakBuf[ucValidPeakIndex];  
g_ucValidIn5Second += ucValidPeakCurr;
g_ucValidIn1Second -= ucValidPeakIn1S[ucValidPeakIndex1s];
g_ucValidIn1Second += ucValidPeakCurr;
ucValidPeakIn1S[ucValidPeakIndex1s] = ucValidPeakCurr;
ucValidPeakBuf[ucValidPeakIndex] = ucValidPeakCurr;
ucValidPeakIndex ++;
ucValidPeakIndex1s++;
if(ucValidPeakIndex1s == ONE_SECOND_COUNT)
{
ucValidPeakIndex1s = (uint8_t)0;
}
if (ucValidPeakIndex == (uint8_t)PHASE6BUF_MAX)
{
ucValidPeakIndex = (uint8_t)0;
}
if(g_ulContinuePoint >  START1S)
{
if(g_ucValidIn1Second < 2)
{
//reset buffer//
g_ulContinuePoint = (uint32_t)0;
ucValidPeakIndex = (uint8_t)0;
for( i  = (int16_t)0; i < (uint8_t)PHASE6BUF_MAX; i++)
{
ucValidPeakBuf[i] = (uint8_t)0;
}
g_ucValidIn5Second = (uint8_t)0;
g_ucValidIn1Second = (uint8_t)0;
for( i = (int16_t)0; i < (uint8_t)ONE_SECOND_COUNT; i++)
{
ucValidPeakIn1S[i] = (uint8_t)0;
}


}


}
if (g_ulTotalPoint >= (uint32_t)START_POINT)
{
/* N(n) */
ucValidSumPeak = g_ucValidIn5Second;
/* O(n)  P(n) */
ucValidHalfStepPrev = ucValidHalfStepCurr;
if ((ucValidSumPeak > VALID_PEAK_IN_ASSIGN_SECOND) && (g_ulContinuePoint >= 112) )
{
ucValidHalfStepCurr = ucValidPeakCurr;
}
else
{
ucValidHalfStepCurr = (uint8_t)0;
}

/* Q(n)*/
if (ucValidHalfStepPrev  == (uint8_t)0)
{
g_uiContinueNoStepPoint++;
}
else
{

g_uiContinueNoStepPoint = (uint16_t)0;
}
if ((g_uiContinueNoStepPoint > CONTINUE_POINT_NO_STEP) && (ucValidHalfStepCurr == (uint8_t)1))
{

if ( g_StepCounter.CompensateState != COMPENSATE_FINISHED)
{
g_StepCounter.CompensateState = COMPENSATE_FINISHED;
ucValidSumPeak += (uint8_t)((uint16_t)11 * (uint16_t)ucValidSumPeak / g_uiContinueNoStepPoint);

}
g_ulTotalValidHalfStep += (uint32_t)ucValidSumPeak;
if(GetTransferFlag() == 1)
{
CrlTransferFlag();
SendMessage(DATA_MODULE, MSG_TRANSFER_THIS_HOUR, NULL);
}

}
          if( ucValidHalfStepCurr==1)   // step count, please calculate step and display on LCD
{
g_ulTotalValidHalfStep++; 
if ( g_ulTotalValidHalfStep & 1 )   //when g_ulTotalValidHalfStep is even 
{         


g_ulCurrentStep = (uint32_t)((g_ulTotalValidHalfStep + (uint32_t)1)>>1);
//must judge the step count  range 0---99999
if(g_StepCounter.Step >= MAX_STEP_COUNT)
{
g_StepCounter.Step =(uint32_t) MAX_STEP_COUNT;



}else
{
ulTp = g_StepCounter.Step;
g_StepCounter.Step += (g_ulCurrentStep - g_ulOldStep);


if(g_StepCounter.Step >MAX_STEP_COUNT)
{
g_StepCounter.Step = MAX_STEP_COUNT;
ulTp = g_StepCounter.Step - ulTp;
}else
{
ulTp = (g_ulCurrentStep - g_ulOldStep);
}
g_StepCounter.WeekStep += ulTp;
}
if(g_System_Control.SystemState == MAIN_STATE )
{
g_System_Control.LCDUpdateFlag = (uint8_t)1;
}

}else if((g_uiContinueNoStepPoint > CONTINUE_POINT_NO_STEP) && (ucValidHalfStepCurr == (uint8_t)1))
{
// compensate is add even the odd peak cout is calculate step// dts add on 09/12/05

g_ulCurrentStep = g_ulTotalValidHalfStep >> 1;
if(g_StepCounter.Step >= MAX_STEP_COUNT)
{
g_StepCounter.Step =(uint32_t) MAX_STEP_COUNT;

}else
{
ulTp = g_StepCounter.Step;
g_StepCounter.Step += (g_ulCurrentStep - g_ulOldStep);
if(g_StepCounter.Step >MAX_STEP_COUNT)
{
g_StepCounter.Step = MAX_STEP_COUNT;
ulTp = g_StepCounter.Step - ulTp;

}else
{
ulTp = (g_ulCurrentStep - g_ulOldStep);
}
g_StepCounter.WeekStep += ulTp;
}
if(g_System_Control.SystemState == MAIN_STATE )
{
g_System_Control.LCDUpdateFlag = (uint8_t)1;
}

}
else
{

}
g_ulOldStep = g_ulCurrentStep;


}  

}
}


}




void StepCounterProc(uint8_t MsgID, uint8_t Param)
{
switch(MsgID)
{
case INIT_MODULE:
StepCounter_Init();
break;
case MSG_RESET_STEP_COUNTER:
ResetStepCounter();
break;
case MSG_CALCULATE_STEP:
CalculateProc();
break;
case MSG_PARAM_RESET:
CounterParamInit();
break;
case MSG_START_16_HZ:
CounterParamInit();
g_System_Control.SamplingState = SAMPLE_16HZ;
IENR2.BIT.IENTFH = (uint8_t)1;
g_StepCounter.CompensateState =  COMPENSATE_START_AT_16HZ;
break;
case MSG_STOP_STEP_COUNTER:
g_System_Control.SamplingState = SAMPLE_STOP;
CounterParamInit();
break;
default:
break;

}
}
//======================================================================
// Name:        void CalculateProc(void)
// Purpose:     step counter calculate handler
// Parameter: NONE
//
// Return:    
// Remarks:
// Change Log:
//                     Author                     Date                      Description
//                    -------------------- -------- ------------------------------------------------
//                    Dts     2006-06-23             Create
//======================================================================
static void CalculateProc(void)
{
uint32_t pt[3];
   uint32_t ulTp1;
uint16_t uiTp,uiTp1;
static uint16_t uiPrevSqrtVal = (uint16_t)0;
if(g_System_Control.SamplingState == SAMPLE_16HZ)
{
StepCounter_InputPoint((uint16_t*)&g_StepCounter.SampleBuf[0]);
if((g_uiContinueNoStepPoint > TEN_SECONDS_NO_STEP) && (g_ucValidIn1Second < 2))// 10 seconds
{
  uiPrevSqrtVal = (uint16_t)0;
g_System_Control.SamplingState = SAMPLE_1HZ;
CounterParamInit();
g_uiLCD_Open = (uint16_t)0;
g_StepCounter.CompensateState = COMPENSATE_FINISHED;
}else
{
;
}
}
else if(g_System_Control.SamplingState== SAMPLE_1HZ)
{
pt[0] = (uint32_t)(g_StepCounter.SampleBuf[0] > 512 ? (g_StepCounter.SampleBuf[0] - (uint16_t)512) : ((uint16_t)512 - g_StepCounter.SampleBuf[0]));
pt[1] = (uint32_t)(g_StepCounter.SampleBuf[1] > 512 ? (g_StepCounter.SampleBuf[1] - (uint16_t)512) : ((uint16_t)512 - g_StepCounter.SampleBuf[1]));
pt[2] = (uint32_t)(g_StepCounter.SampleBuf[2] > 512 ? (g_StepCounter.SampleBuf[2] - (uint16_t)512) : ((uint16_t)512 - g_StepCounter.SampleBuf[2]));
ulTp1 = pt[0] * pt[0] + pt[1] * pt[1] + pt[2] * pt[2];
#ifdef PCVER
uiTp1 = SqrtInt_T(ulTp1);
#else
uiTp1 = SqrtInt(ulTp1);
#endif
if (uiPrevSqrtVal == 0) 
{
uiPrevSqrtVal = uiTp1;
}
uiTp = uiTp1 > uiPrevSqrtVal ? uiTp1 - uiPrevSqrtVal : uiPrevSqrtVal - uiTp1;
uiPrevSqrtVal = uiTp1;
if (uiTp > START_DIFF) 
{
CounterParamInit();
IENR2.BIT.IENTFH = (uint8_t)1;
g_System_Control.SamplingState = SAMPLE_16HZ;
g_StepCounter.CompensateState = COMPENSATE_FROM_1HZ_TO_16HZ;
g_StepCounter.AutoStartFlag = (uint8_t)1;
}

}else
{
;
}


return ;
}


#ifdef PCVER
static uint16_t SqrtInt_T(uint32_t M) 
{
    int8_t  i;
uint16_t N; 
uint16_t tmp, ttp;      


if(M > 0)
{               
    
N = (uint16_t)0;
tmp = (uint16_t)0;

M <<= (uint32_t)12;
for (i = (uint8_t)10; i>0; i--)     
   { 
       N <<=(uint16_t) 1;             
       tmp <<= (uint16_t)2; 
       tmp += (uint16_t)(M >> 30);    
       ttp = N; 
       ttp = (ttp<<1)+1; 
       M <<= (uint32_t)2; 
       if (tmp >= ttp)       
       { 
           tmp -= ttp; 
           N ++; 
       } 
   } 
}else
{
N = (uint16_t)0;
}
return N;
}
#endif

更详细相关代码获得，技术交流欢迎加群：590411159