PLC的ST语言实现用于编码器跟踪的FIR整数滤波器

    机器人用编码器进行物件跟踪时容易抖动,传送带本身不稳定,或者编码器跳变本身就容易抖动. 可以用普通FIR滤波器进行滤波,但是一般编码器反馈32位值,超过32位后溢出会造成跳变,而且浮点型滤波器输入很大的值会降低精度,虽然可以变基来解决问题,但是不优美.

    最近把FIR滤波器移植到plc的st语言时发现整数滤波器可以完美解决这个问题,因为整数FIR滤波器可以保证一个冲击响应积分出来还是1,即可以把编码器的delta作为输入,输出的速度可以直接数字积分加起来得到滤波后的信号.

    FIR滤波因为相位延迟是线性的,得到的速度可以乘上延迟系数,可以补偿相位延迟

FUNCTION_BLOCK DintFirFilter
VAR_INPUT
	inputVal : DINT;
END_VAR
VAR_OUTPUT
	outputVal : DINT;
	maxVal : DINT;
	minVal : DINT;
	limitCount : DINT;
END_VAR
VAR
	size : UDINT;
	w : ARRAY [0..127] OF DINT;
	v : ARRAY[0..127] OF DINT;
	vi : UDINT;
	i : UDINT;
	i_vi : UDINT;
END_VAR
VAR CONSTANT
	gainTarget : LREAL := 10000;
	inputLimit : DINT := 10000;
	//gainTarget * inputLimit < MAX_DINT32 -> overflow safe
END_VAR
//整数FIR滤波将10000作为增益目标,对于int32来说,输入10000以内的值是比较安全,不会超出int32范围的
maxVal := MAX(inputVal,maxVal);
minVal := MIN(inputVal,minVal);
IF inputVal > inputLimit THEN
	inputVal := inputLimit;
	limitCount := limitCount + 1;
ELSIF inputVal < -inputLimit THEN
	inputVal := -inputLimit;
	limitCount := limitCount + 1;
END_IF
v[vi] := inputVal;
outputVal := 0;
FOR i := 0 TO size-1 DO
	i_vi := (vi+i+1) MOD size;
	outputVal:= outputVal + w[i]*v[i_vi];
END_FOR
vi := vi+1;
IF vi >=size THEN
	vi := 0;
END_IF

//---------------------------------------------
METHOD barthannwinInit
VAR_INPUT
	n : UDINT;
	Fc : LREAL;
END_VAR
VAR
	x : LREAL;
	h : LREAL;
	n_1 : LREAL;
	i : UDINT;
	sum : LREAL;
	w : ARRAY[0..127] OF LREAL;
END_VAR
(*
barthannwin 把首尾的0去掉了
*)
IF n<=1 THEN
	size := 1;
	w[0] := TO_DINT(gainTarget);
	RETURN;
ELSIF n=2 THEN
	size := 2;
	w[0] := TO_DINT(gainTarget/2);
	w[1] := TO_DINT(gainTarget/2);
	RETURN;
ELSIF n>128 THEN
	n := 128;
END_IF
size := n;
n_1 := TO_LREAL(size+1);   // N-1 = n+2-1 = size + 1
sum := 0;
FOR i:=0 TO size-1 DO
	x := ABS( TO_LREAL(i+1)/(n_1) - 0.5);
	w[i] := 0.62 - 0.48*x + 0.38*COS(2.0*MATH.PI*x);
	h := SINC(2*Fc*( TO_LREAL(i+1) - (n_1)*0.5));
	w[i] := h*w[i];
	sum := sum + w[i];
END_FOR
FOR i:=0 TO size-1 DO
	w[i] := gainTarget*w[i]/sum;
END_FOR
sum := 0;
FOR i:=0 TO size-1 DO
	sum := sum + w[i];
	b[i] := TO_DINT(sum);
END_FOR
sum := 0;
FOR i:=size-1 TO 1 BY -1 DO
	b[i] := b[i] - b[i-1];
	sum := sum + b[i];
END_FOR
sum := sum + b[0];
WHILE sum<>gainTarget DO
	i := 0;
	sum := sum/i;
END_WHILE

reset();



//----------------------------------------
METHOD reset
VAR_INPUT
END_VAR
VAR
	i : UDINT;
END_VAR

FOR i := 0 TO size-1 DO
	v[i] := 0;
END_FOR
vi := 0;
outputVal := 0;
maxVal := 0;
minVal := 0;
limitCount := 0;


//---------------------------------------
METHOD SINC : LREAL
VAR_INPUT
	x : LREAL;
END_VAR

IF x = 0 THEN
	SINC := 1;
ELSE
	SINC := SIN(Math.PI*x) / (Math.PI*x);
END_IF

附上用来验证的matlab代码

%离散化正确的barthannwin 整数FIR滤波器
%N=17,fc=0.2 =>>  16  88  0 -384   -428    787   2891   4060   2891    787   -428   -384      0     88     16
N = 17;
fc = 0.2;

hn = 0:N-1;
h = sinc(2*fc*(hn-(N-1)/2));

arrn = 0:N-1;
x = abs((arrn./(N-1)) - 0.5);
bb = 0.62 - 0.48.*x + 0.38*cos(2*pi.*x);
bh = bb.*h;
b=bh;

b = 10000*b./sum(b);

len = length(b);

ss = 0;
sb = [];
for i=1:len-1
  ss = ss + b(i);
  sb = [sb round(ss)];
endfor

ib = [];
for i=1:len-2
  ib = [ib sb(i+1) - sb(i)];
endfor
b
ib = ib'
length(ib)

[hh,ww]=freqz(ib,1,512,2);
freqz_plot(ww,hh,'false');