PLC的ST语言实现IIR butterworth低通滤波器

参考 Butterworth Filter Design in C++ – The Code Hound

matlab代码，创建一个fc=0.1的4阶butterworth低通滤波器。

format long

[b,a] = butter(4,0.1,'low')

input1 = [1,2,3,1,2,3,1,2,3,0,0];
output = filter(b,a,input1)

过滤input1的结果为

output =

 Columns 1 through 5:

   3.123897691708262e-05   2.995734755404623e-04   1.454902769105549e-03   4.766674939805153e-03   1.193910293101060e-02

 Columns 6 through 10:

   2.475774743364613e-02   4.492688281346330e-02   7.394998706258429e-02   1.129536864512947e-01   1.626442203690692e-01

 Column 11:

   2.231700886131495e-01

从c++移植到PLC代码。Polynomial最大阶数为10，所以最好是在9阶下使用IIR滤波器。当然8阶就已经够高了，很容易发散了。

TYPE Complex :
STRUCT
    re : LREAL;
    im : LREAL;
END_STRUCT
END_TYPE
//----------------------------
TYPE PolyConstant :
(
	PolyMaxN := 10
);
END_TYPE
//----------------------------
TYPE Polynomial :
STRUCT
    c : ARRAY [0..PolyMaxN-1] OF Complex;
    n : DINT;
END_STRUCT
END_TYPE
//----------------------------
FUNCTION C_Add : Complex
VAR_INPUT  
    a, b : Complex;  
END_VAR  
C_Add.re := a.re + b.re;
C_Add.im := a.im + b.im;
//-----------------------------
FUNCTION C_Div : Complex
VAR_INPUT
    a, b : Complex; // a/b
END_VAR
VAR
    denominator : LREAL; // 分母的模长的平方
END_VAR
// 计算分母的模长的平方
denominator := b.re * b.re + b.im * b.im;

// 计算结果
C_Div.re := (a.re * b.re + a.im * b.im) / denominator;
C_Div.im := (a.im * b.re - a.re * b.im) / denominator;

//-------------------------------
FUNCTION C_Mul : Complex
VAR_INPUT  
    a, b : Complex;  
END_VAR
C_Mul.re := a.re * b.re - a.im * b.im;
C_Mul.im := a.re * b.im + a.im * b.re;

//-------------------------------
FUNCTION P_Poly : Polynomial
VAR_INPUT
    roots : Polynomial;
END_VAR
VAR
    i : DINT;
    factor : Polynomial;
    temp : Polynomial;
END_VAR
temp.c[0].re:=1;
temp.c[0].im:=0;
temp.n := 1;

factor.n := 2;
FOR i:=0 TO (roots.n-1) DO
    factor.c[0].re := -roots.c[i].re;
    factor.c[0].im := -roots.c[i].im;
    factor.c[1].re := 1.0;
    factor.c[1].im := 0.0;
    temp := P_PolyMul(temp,factor);
END_FOR

P_Poly.n := temp.n;
FOR i:=0 TO (temp.n-1) DO
    P_Poly.c[i] := temp.c[temp.n-i-1];
END_FOR

//--------------------------------
FUNCTION P_PolyMul : Polynomial
VAR_INPUT  
    p, q : Polynomial;  
END_VAR  
VAR
    n,i,j : DINT;
END_VAR
n := p.n + q.n - 1;
P_PolyMul.n := n;
FOR i:=0 TO p.n-1 DO
    FOR j:=0 TO q.n-1 DO
        P_PolyMul.c[i+j] := C_Add(P_PolyMul.c[i+j],C_Mul(p.c[i],q.c[j]));
	END_FOR
END_FOR

//---------------------------------
FUNCTION P_Real : Polynomial
VAR_INPUT  
    a : Polynomial;  
END_VAR  
VAR
    i : DINT;
END_VAR
FOR i := 0 TO a.n-1 DO
    P_Real.c[i].re := a.c[i].re;
END_FOR
P_Real.n := a.n;

//---------------------------------
FUNCTION P_RealSum : LREAL
VAR_INPUT  
    a : Polynomial;  
END_VAR  
VAR
    i : DINT;
END_VAR
FOR i := 0 TO a.n-1 DO
    P_RealSum := P_RealSum+ a.c[i].re;
END_FOR


//---------------------------------
FUNCTION_BLOCK IIR_Filter
VAR_INPUT
	inputVal : LREAL;
END_VAR
VAR_OUTPUT
	outputVal : LREAL;
END_VAR
VAR
	b : ARRAY [0..PolyMaxN-1] OF LREAL;
	a : ARRAY[0..PolyMaxN-1] OF LREAL;
    z : ARRAY[0..PolyMaxN-1] OF LREAL;
    ord : DINT;
	i : DINT;
END_VAR
z[0] := inputVal*a[0];
FOR i := 1 TO ord DO
    z[0] := z[0] - z[i]*a[i];
END_FOR
outputVal := z[0]*b[0];
FOR i := 1 TO ord DO
    outputVal := outputVal + z[i]*b[i];
END_FOR
FOR i := 0 TO ord -1 DO
    z[ord-i] := z[ord-(i+1)];
END_FOR

//----IIR_Filter.reset--------
METHOD reset
VAR_INPUT
	inputVal : LREAL := 0;
END_VAR
VAR
	i : DINT;
END_VAR
FOR i := 0 TO ord DO
	z[i] := inputVal;
END_FOR
outputVal := inputVal;

//------IIR_Filter.butter_low_init--------
METHOD butter_low_init
VAR_INPUT
    N : DINT := 4;
    fc : LREAL := 0.1;
    fs : LREAL := 2;
END_VAR
VAR
    i,k : DINT;
    theta : LREAL;
    pa : Polynomial;
    p : Polynomial;
    q : Polynomial;
    nume : Complex;
    deno : Complex;
    _fc : LREAL;
    Gain : LREAL;
    a : Polynomial;
    b : Polynomial;
END_VAR
VAR CONSTANT
    PI : LREAL := 3.141592653589793;
END_VAR
//0.init
pa.n := N;
p.n := N;
q.n := N;
FOR i:= 0 TO N-1 DO
    q.c[i].re := -1;
END_FOR

//1.Find poles of analog filter
FOR i := 0 TO N-1 DO
    k := i+1;
    theta := (2*k-1)*PI/(2*N);
    pa.c[i].re := -SIN(theta);
    pa.c[i].im :=  COS(theta);
END_FOR

//2.Scale Poles in frequency
_fc := fs/PI * TAN(PI*fc/fs);
FOR i:=0 TO pa.n-1 DO
    pa.c[i].re := pa.c[i].re*2*PI*_fc;
    pa.c[i].im := pa.c[i].im*2*PI*_fc;
END_FOR

//3. Find coeffs of digital filter poles and zeros in the z plane
FOR i:=0 TO N-1 DO
    nume.re := 1.0 + pa.c[i].re/(2*fs);
    nume.im :=       pa.c[i].im/(2*fs);
    deno.re := 1.0 - pa.c[i].re/(2*fs);
    deno.im :=     - pa.c[i].im/(2*fs);
    p.c[i] := C_Div(nume,deno);
END_FOR

a := P_Poly(p);
a := P_Real(a);

b := P_Poly(q);

Gain := P_RealSum(a) / P_RealSum(b);

FOR i:= 0 TO b.n-1 DO
    b.c[i].re := b.c[i].re*Gain;
END_FOR

//output
ord := a.n;
FOR i:= 0 TO a.n-1 DO
    THIS^.a[i] := a.c[i].re;
    THIS^.b[i] := b.c[i].re;
END_FOR



//----------------------------
PROGRAM main
VAR
    filter : IIR_Filter;
    init : BOOL;
    i : DINT;
    N : DINT := 4;
    fc : LREAL := 0.1;
END_VAR
VAR
    input : ARRAY [0..10] OF LREAL := [1,2,3,1,2,3,1,2,3];
    output : ARRAY[0..10] OF LREAL;
END_VAR
IF NOT init THEN
    init := TRUE;
    filter.butter_low_init(N,fc,2);
    filter.reset(0);
    FOR i:=0 TO 10 DO
        filter(inputVal:=input[i],outputVal=>output[i]);    
    END_FOR
END_IF

得出结果

比较matlab的结果，正确，误差不大。欢迎修改N和fc参数进行测试