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

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

#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

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

重力加速计低通滤波算法——安卓开发文档
重力为测量结果中的不变成分.因此在获取到三轴的加速度分量后,可以采用低通滤波的方式分离出重力分量,然后在原始值中减去重力.最后得到不含重力的手机加速度. 手机的x.y.z轴定义如图: 以下为安卓开发者 ...
高阶低通滤波算法_高/低算法
高阶低通滤波算法介绍在我以前的文章中,我谈到了各种数据库标识符策略,在设计数据库模型时需要注意. 我们得出的结论是,数据库序列非常方便,因为它们在大多数用例中既灵活又高效. 但是,即使具有缓存的序 ...
一阶RC低通滤波算法原理与实现
文章目录 1. 一阶低通滤波算法原理 2. 一阶滤波算法的特点 3. 基本算法的例程 4. 优化:减少乘.除的运算次数以提高运算速度 5. 改进:动态调整滤波系数动态调整滤波例程本文整理自网络,参 ...
利用FFT分析比较卡尔曼滤波算法、低通滤波算法、滑动平均滤波的频谱
1 卡尔曼滤波详见博客 https://blog.csdn.net/moge19/article/details/81750731 2 低通滤波 2.1 算法推导一阶RC滤波器的硬件电路如图: 图 ...
数字低通滤波算法及截止频率计算，pt1Filter，lpf
参考模电RC滤波计算公式来计算截止频率 #define M_PI_FLOAT 3.14159265358979323846ftypedef struct LpfFilter {float RC;//模 ...
实战低通滤波和卡尔曼滤波
目录背景所需硬件与软件理论基础一阶低通滤波卡尔曼滤波仿真验证实际验证总结背景滤波这个词对任何一个工科生都不会陌生,尤其是做控制或者信号方面的从业者和学生.我们不仅可以通过硬件滤波也 ...
opencv 高通滤波和低通滤波_一阶低通滤波原理详解
在汽车标定中,使用最多的滤波算法即低通滤波,很多朋友可能知道怎么标定,但是不清楚具体的原理,因此本文将介绍一阶低通滤波的原理.算法建模仿真和优缺点: 一阶滤波算法的原理一阶滤波,又叫一阶惯性滤波,或 ...
简述RC低通滤波原理
首先需要明确,低通指的是低频率通过. 然后上图: 上图是RC低通滤波电路的原理图,我们可以把电路稍微改动一下: 这样看起来更像一个分压电路. 由于电容C具有隔直通交的特性,所以输入的高频分量相当于经过 ...
一阶低通滤波器方程_一阶低通滤波原理详解
在汽车标定中,使用最多的滤波算法即低通滤波,很多朋友可能知道怎么标定,但是不清楚具体的原理,因此本文将介绍一阶低通滤波的原理.算法建模仿真和优缺点:一阶滤波算法的原理一阶滤波,又叫一阶惯性滤波,或一 ...

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

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

最新文章

热门文章