- 超声波测到的数据会有一点毛刺,先用中值滤波,可以很好去掉突变太大的数据.为什么不用均值滤波呢,均值滤波对数据的实时性影响比较大.
- 对中值滤波后的数据做一定的统计,计算出数据的变化量和离散程度.我能想到的就是前后数据的差值,若干个数据的方差. 差值能直接反应数据的突变,方差能够强烈的反应数据的波动.
- 对数据进行判断,差值太大的不能要,方差太大的也不能要.这都说明超声波受到了明显的干扰:前方突然出现遮挡.干扰过后再等一下下,防止有连续的干扰出现.
- 开始重新记录数据,最后高度的变化就是前后数据的差值的积分.
- 优点:能实现厘米级定高
- 缺点:在刚好出现越障碍物时,如果出现在高度变化就测量不到了。这时就要再结合,加速度计和气压计做拟合了
sonarAltRaw = UltralSonicRanging_GetDistance();//原始超声波数据
sonarAltRaw = applySonarAltMedianFilter(sonarAltRaw);//中值滤波,过滤数据毛刺
if (sonar_available)
{
//float cosTiltAngle = getCosTiltAngle();//倾角
//sonarAlt = sonarAlt * cosTiltAngle;
/*int32_t boarStable = getSonarErrSum();*/
if ((ABS(sonarVariance.error) < 10))//正常时前后差不大于10,没遮挡时方差不大于10
{
sonarAltStableCnt++;
//这时候的相对高度是稳定的。
if (millis() - sonarAlt_timeRecord > 100)//数据稳定后等一下
{
if (!sonarAltDt)//没记录过数据
{
sonarAltDt = 1;
sonarAlt_last = sonarAltRaw;//记录第一个
sonarStartAlt = sonarAltRaw;//记录第一个稳定的高度
}
else
{
sonarAlt_dt = (sonarAltRaw - sonarAlt_last);//数据变化
sonarAlt += sonarAlt_dt;//积分获得位移
sonarAlt_last = sonarAltRaw;
sonarAltDtStable = 1;
/*sonarAlt /= 10;*/
}
}
}
else//检测到下方有东西突然出现
{
sonarAltDt = 0;
sonarAlt_timeRecord = millis();//开始计时
sonarAltDtStable = 0;
sonarAltStableCnt = 0;//有一次不稳定都清零
}
if (sonarAltStableCnt > 1000)//超声波测距长时间稳定
{
//高度信息是从开始高度的变化开始积分的,即:高度 = 测量高度 - 起始高度
sonarAltStableCnt = 0;
sonarAlt = sonarAltRaw - sonarStartAlt;//修正积分误差
}
}
int32_t applySonarAltMedianFilter(int32_t newSonarReading)//中值滤波对脉冲噪声有良好的滤除作用
{
static int32_t sonarFilterSamples[10];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;
if (newSonarReading < SONAR_OUT_OF_RANGE) // only accept samples that are in range
{
nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == 10) {
nextSampleIndex = 0;
medianFilterReady = true;
}
sonarFilterSamples[currentFilterSampleIndex] = newSonarReading;
currentFilterSampleIndex = nextSampleIndex;
}
if (medianFilterReady)
return quickMedianFilter5(sonarFilterSamples);
else
return newSonarReading;
}
int32_t quickMedianFilter5(int32_t * v)
{
int32_t p[5];
QMF_COPY(p, v, 5);
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
QMF_SORT(p[1], p[2]);
return p[2];
}
Variancec_s sonarVariance = {
.average = 0,
.dataCnt = 0,
.sum = 0,
.sum2=0,
.variance=0
};
void varianceCalculate(int32_t newInput, Variancec_s *_variance)//求数据方差
{
if (_variance->dataCnt < VARIANCE_LEN)//数据还不足
{
_variance->storage[_variance->dataCnt] = newInput;
_variance->dataCnt++;
}
else
{
_variance->sum = 0;
for (uint8_t i=0; i < VARIANCE_LEN; i++)//数据积分
{
_variance->sum += _variance->storage[i];
}
_variance->average = _variance->sum / VARIANCE_LEN;//数据平均值
_variance->sum2 = 0;
for (uint8_t i = 0; i < VARIANCE_LEN; i++)//数据平均差积分
{
_variance->sum2 += ((_variance->storage[i]- _variance->average)* (_variance->storage[i] - _variance->average));
}
_variance->variance = _variance->sum2 / VARIANCE_LEN;
for (uint8_t i = 0; i < (VARIANCE_LEN-1); i++)
{
_variance->storage[i] = _variance->storage[i + 1];//扔掉第一个数据
}
_variance->error = newInput - _variance->storage[VARIANCE_LEN - 1];
_variance->storage[VARIANCE_LEN-1] = newInput;//新数据插入到最后一个
}
}