滤波器调试完成

1、主体函数如下：

FILTER FLT={3.2,0,4.0,0,0.001,0.543};
float CBMS::Filter(FILTER *flt,float input)
{
	flt->nowP = flt->lastP + flt->Q;
	flt->kg = flt->nowP/(flt->nowP + flt->R);
	flt->out = flt->out + flt->kg * (input - flt->out);
	flt->lastP = (1-flt->kg) * flt->nowP;
	
	return flt->out;
}

2、滤波效果，快速收敛：

3、参数影响：初始输出out需要给定控制趋势。
4、初值与当前值会影响收敛速度，往大了给，不建议太小，如0.1这种，建议1以上。
5、Q和R对收敛速度也有影响，需要在使用时调试。