超声波定高--过滤突然出现的障碍物

1. 超声波测到的数据会有一点毛刺,先用中值滤波,可以很好去掉突变太大的数据.为什么不用均值滤波呢,均值滤波对数据的实时性影响比较大.
2. 对中值滤波后的数据做一定的统计,计算出数据的变化量和离散程度.我能想到的就是前后数据的差值,若干个数据的方差. 差值能直接反应数据的突变,方差能够强烈的反应数据的波动.
3. 对数据进行判断,差值太大的不能要,方差太大的也不能要.这都说明超声波受到了明显的干扰:前方突然出现遮挡.干扰过后再等一下下,防止有连续的干扰出现.
4. 开始重新记录数据,最后高度的变化就是前后数据的差值的积分.
5. 优点：能实现厘米级定高
6. 缺点:在刚好出现越障碍物时，如果出现在高度变化就测量不到了。这时就要再结合，加速度计和气压计做拟合了

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;//新数据插入到最后一个
	}
}