一、PT1滤波器原理PT1滤波器也称为RC滤波器是一种广泛应用的低筒滤波器其原理很简单电容C与电阻R相结合形成一个RC双极环路在这个环路中电容和电阻之间存在恒定的电容-电阻耦合电路原理如图1。PT1滤波器算法实现二、PT2滤波器原理PT2、PT3滤波器也称2阶、3阶RC滤波器其电路原理如图2。三、代码示例C// inc/algothrim_pt.hpp#ifndef ALGOTHRIM_PT_INCLUDE_HPP#define ALGOTHRIM_PT_INCLUDE_HPPnamespace algothrim{class CPTfilter{public: CPTfilter();float runAlgothrim(const float f_valueIn);void initAlgothrim(const float f_valueIn, const float f_rateIn);private: float m_ratePt;float m_perValue;};}#endif // ALGOTHRIM_PT_INCLUDE_HPP// src/algothrim_pt.cpp#include ../inc/algothrim_pt.hppnamespace algothrim{CPTfilter::CPTfilter():m_ratePt(0.9), m_perValue(0.0){}float CPTfilter::runAlgothrim(const float f_valueIn){returnm_ratePt * f_valueIn (1- m_ratePt)* m_perValue;}void CPTfilter::initAlgothrim(const float f_valueIn, const float f_rateIn){//!init filter valueif(f_rateIn0.0||f_rateIn1.0){return;}m_ratePtf_rateIn;m_perValuef_valueIn;return;}}#include vector#include iostream#include inc/algothrim_pt.hppalgothrim::CPTfilter m_ptFilter;float l_ratePT{0.95};float l_initValue{0.0};intmain(){//!build x vector std::vectorfloatl_valueX, l_valueY;for(int ii_tempD1;ii_tempD200;ii_tempD){float l_ii_tempEii_tempD *0.1-10;l_valueX.push_back(l_ii_tempE * l_ii_tempE 3.5);}//!init filter m_ptFilter.initAlgothrim(l_ratePT, l_initValue);//!get run y vectorfor(int ii_T0;ii_Tl_valueX.size();ii_T){l_valueY.push_back(m_ptFilter.runAlgothrim(l_valueX[ii_T]));std::coutx_value:l_valueX[ii_T]std::endl;std::couty_value:m_ptFilter.runAlgothrim(l_valueX[ii_T])std::endl;}return-1;}matlabclassdef CIntegratorTrapezoidhandle properties value prevU end methods(Static)functionnewInstnewInstance()newInstCIntegratorTrapezoid;end end methodsfunctionobjCIntegratorTrapezoid()obj.value0;obj.prevU0;endfunctionintegrate(obj,u,delta_t)obj.valueobj.value (obj.prevU u)/2 * delta_t;obj.prevUu;endfunctioninit(obj)obj.value0;end end endclassdef CIntegratorCodeImplhandle % integrator according to C implementation properties value end methods(Static)functionnewInstnewInstance()newInstCIntegratorCodeImpl;end end methodsfunctionobjCIntegratorCodeImpl()obj.value0;endfunctionintegrate(obj,u,delta_t)obj.valueobj.value u*delta_t;endfunctioninit(obj)obj.value0;end end endclassdef CPt2Bilinearhandle % % PT2 element % s-domain(continous)transfer function: % W(s)Y(s)/ U(s)1/(T^2 * s^2 2* D * T * s 1)% % Where T istimeconstant % D is damping factor % % s-domain(continous)transferfunctionis converted to % z-domain(discrete)timetransferfunction% with the bilinear(tustin)approximation % s(2/ Ts)*(z -1)/(z 1)% where Ts is the samplingtime% % This leads to %(Ts^2*z^2 2*Ts^2*z Ts^2)/((4*T^2 4*D*T*Ts Ts^2)*z^2 (2*Ts^2 -8*T^2)*z 4*T^2 -4*D*T*Ts Ts^2)% % simplifying the expression by using a0, a1, a2, b0, b1, b2 % W(z)Y(z)/ U(z)(a0 * z^2 a1 * z a2)/(b0 * z^2 b1 * z b2)% where % % a0Ts^2 % a12*Ts^2 % a2Ts^2 % b04*T^2 4*D*T*Ts Ts^2 % b12*Ts^2 -8*T^2 % b24*T^2 -4*D*T*Ts Ts^2 % % expressing the transferfunctionW(z)%interms of polynomial of z^(-1)% where z^(-1)represents a discretetimedelay of Ts % % W(z)Y(Z)/ U(z)(a0 a1 * z^(-1) a2 * z^(-2))/(b0 b1 * z^(-1) b2 * z^(-2))% % expressing the input - output relationship: % Y(z)*(b0 b1 * z^(-1) b2 * z^(-2))U(z)*(a0 a1 * z^(-1) a2 * z^(-2))% % From the above, the discretetimeresponse to the signal U(z)is % % b0 * Y(z)a0 * U(z) a1 * U(z)* z^(-1) a2 * U(z)* z^(-2)- b1 * Y(z)* z^(-1)- b2 * Y(z)* z^(-2)% % As such, the discrete-time system, that implements the following % transferfunctionis: % % b0 * y(k)a0 * u(k) a1 * u(k-1) a2 * u(k-2)- b1 * y(k-1)- b2 * y(k-2)% % y(k)(a0 * u(k) a1 * u(k-1) a2 * u(k-2)- b1 * y(k-1)- b2 * y(k-2))/ b0 % properties m_T m_D m_in1;% u(k-1)m_in2;% u(k-2)m_out1;% y(k-1)m_out2;% y(k-2)end methodsfunctionobjCPt2Bilinear(T,D)obj.m_TT;obj.m_DD;obj.m_out10;obj.m_out20;obj.m_in10;obj.m_in20;endfunction[l_out]update(obj,u,delta_t)a0delta_t^2;a12*delta_t^2;a2delta_t^2;b04*obj.m_T^2 4*obj.m_D*obj.m_T*delta_t delta_t^2;b12*delta_t^2 -8*obj.m_T^2;b24*obj.m_T^2 -4*obj.m_D*obj.m_T*delta_t delta_t^2;l_out(a0 * u a1 * obj.m_in1 a2 * obj.m_in2 - b1 * obj.m_out1 - b2 * obj.m_out2)/ b0;obj.m_out2obj.m_out1;obj.m_out1l_out;obj.m_in2obj.m_in1;obj.m_in1u;end end endclassdef CPt2BwEulerhandle % % PT2 element % s-domain(continous)transfer function: % % W(s)Y(s)/ U(s)1/(T^2 * s^2 2* D * T * s 1)% % Where T istimeconstant % D is damping factor % % s-domain(continous)transferfunctionis converted to % z-domain(discrete)timetransferfunction% with the backward euler(rectangular)approximation % % s(z-1)/(Ts*z);% % where Ts is the samplingtime% % This leads to % % W(z)Y(z)/ U(z)(Ts^2*z^2)/((T^2 2*D*T*Ts Ts^2)*z^2 (-2*T^2 -2*D*Ts*T)*z T^2)% % simplifying the expression by using a0, b0, b1, b2 % % W(z)Y(z)/ U(z)(a0 * z^2)/(b0 * z^2 b1 * z b2)% % where % % a0Ts^2 % b0T^2 2*D*T*Ts Ts^2 % b1-2*T^2 -2*D*Ts*T % b2T^2 % % expressing the transferfunctionW(z)%interms of polynomial of z^(-1)% where z^(-1)represents a discretetimedelay of Ts % % W(z)Y(Z)/ U(z)a0 /(b0 b1 * z^(-1) b2 * z^(-2))% % expressing the input - output relationship: % % Y(z)*(b0 b1 * z^(-1) b2 * z^(-2))U(z)* a0 % % From the above, the discretetimeresponse to the signal U(z)is % % b0 * Y(z)a0 * U(z)- b1 * Y(z)* z^(-1)- b2 * Y(z)* z^(-2)% % As such, the discrete-time system, that implements the following % transferfunctionis: % % b0 * y(k)a0 * u(k)- b1 * y(k-1)- b2 * y(k-2)% % y(k)(a0 * u(k)- b1 * y(k-1)- b2 * y(k-2))/ b0 % properties m_T m_D m_out1;% y(k-1)m_out2;% y(k-2)end methodsfunctionobjCPt2BwEuler(T,D)obj.m_TT;obj.m_DD;obj.m_out10;obj.m_out20;endfunction[l_out]update(obj,u,delta_t)a0delta_t^2;b0obj.m_T^2 2*obj.m_D*obj.m_T*delta_t delta_t^2;b1-2*obj.m_T^2 -2*obj.m_D*delta_t*obj.m_T;b2obj.m_T^2;l_out(a0 * u - b1*obj.m_out1 -b2 * obj.m_out2)/b0;obj.m_out2obj.m_out1;obj.m_out1l_out;end end endclassdef CPt2CodeImplhandle % PT2 filter according to C implementation properties m_T m_D m_1stIntegrator m_2ndIntegrator end methodsfunctionobjCPt2CodeImpl(T,D,Integrator)obj.m_TT;obj.m_DD;obj.m_1stIntegratorIntegrator.newInstance;obj.m_2ndIntegratorIntegrator.newInstance;endfunction[out]update(obj,u,delta_t)% Conserve states x_1obj.m_1stIntegrator.value;x_2obj.m_2ndIntegrator.value;if(obj.m_Tdelta_t)obj.m_1stIntegrator.integrate(x_2, delta_t);obj.m_2ndIntegrator.integrate(-1.0/(obj.m_T*obj.m_T)*x_1 -2.0*obj.m_D/obj.m_T*x_2 1.0/(obj.m_T*obj.m_T)*u, delta_t);else%setoutput value equal to input obj.m_1stIntegrator.valueu;%setderivate y to current gradient of input obj.m_2ndIntegrator.value(u-x_1)/delta_t;end outx_1;end end endclassdef CPt2FwEulerhandle % % PT2 element % s-domain(continous)transfer function: % % W(s)Y(s)/ U(s)1/(T^2 * s^2 2* D * T * s 1)% % Where T istimeconstant % D is damping factor % % s-domain(continous)transferfunctionis converted to % z-domain(discrete)timetransferfunction% with the forward Euler(rectangular)approximation % % s(z-1)/ Ts;% % where Ts is the samplingtime% % This leads to % % W(z)Y(z)/ U(z)Ts^2/(T^2*z^2 (-2*T^2 2*D*Ts*T)*z T^2 -2*D*T*Ts Ts^2)% % simplifying the expression by using a2, b0, b1, b2 % % W(z)Y(z)/ U(z)a2 /(b0 * z^2 b1 * z b2)% % where % % a2Ts^2 % b0Ts^2 % b1-2*T^2 2*D*Ts*T % b2T^2 -2*D*T*Ts Ts^2 % % expressing the transferfunctionW(z)%interms of polynomial of z^(-1)% where z^(-1)represents a discretetimedelay of Ts % % W(z)Y(Z)/ U(z)a2 * z^(-2)/(b0 b1 * z^(-1) b2 * z^(-2))% % expressing the input - output relationship: % % Y(z)*(b0 b1 * z^(-1) b2 * z^(-2))U(z)* a2 * z^(-2)% % From the above, the discretetimeresponse to the signal U(z)is % % b0 * Y(z)a2 * z^(-2)* U(z)- b1 * Y(z)* z^(-1)- b2 * Y(z)* z^(-2)% % As such, the discrete-time system, that implements the following % transferfunctionis: % % b0 * y(k)a2 * u(k-2)- b1 * y(k-1)- b2 * y(k-2)% % y(k)(a2 * u(k-2)- b1 * y(k-1)- b2 * y(k-2))/ b0 % properties m_T m_D m_out1;% y(k-1)m_out2;% y(k-2)m_u1;% u(k-1)m_u2;% u(k-2)end methodsfunctionobjCPt2FwEuler(T,D)obj.m_TT;obj.m_DD;obj.m_out10;obj.m_out20;obj.m_u10;obj.m_u20;endfunction[l_out]update(obj,u,delta_t)a2delta_t^2;b0obj.m_T^2;b1-2*obj.m_T^2 2* obj.m_D * delta_t * obj.m_T;b2obj.m_T^2 -2*obj.m_D * obj.m_T * delta_t delta_t^2;l_out(a2 * obj.m_u2 - b1 * obj.m_out1 - b2 * obj.m_out2)/ b0;obj.m_out2obj.m_out1;obj.m_out1l_out;obj.m_u2obj.m_u1;obj.m_u1u;end end end%% PT2 Element variations comparisonclear;T1;%timeconstant D1.2;% damping constant Ts200E-3;%timestep / averagetimestep tmax10;% simulation length l_pt2CPt2CodeImpl(T ,D, CIntegratorCodeImpl());l_pt2bweulerCPt2BwEuler(T, D);l_pt2biliCPt2Bilinear(T,D);l_pt2fweulerCPt2FwEuler(T,D);l_pt2codeWithTrapezoidCPt2CodeImpl(T ,D, CIntegratorTrapezoid());%% samplingtimes%1. uniform sampling rate t_vec[0:Ts:tmax];%2. denser sampling near the beginning % t_veclinspace(0,1,tmax/Ts).^2 * tmax;%3. sparser sampling near the beginning % t_veclinspace(0,1,tmax/Ts).^0.5 * tmax;%4. Gaussian randomtimeintervals % rng(1)% randsrandn(1,ceil(tmax/Ts))* 65E-3 Ts;% t_veccumsum(rands);%%idx1;prevT0;outzeros(length(t_vec),5);fortt_vec out(idx,1)l_pt2.update(1.0,t - prevT);out(idx,2)l_pt2bweuler.update(1.0,t - prevT);out(idx,3)l_pt2bili.update(1.0,t - prevT);out(idx,4)l_pt2fweuler.update(1.0,t - prevT);out(idx,5)l_pt2codeWithTrapezoid.update(1.0,t - prevT);prevTt;idxidx1;end figure plot(t_vec,out);hold on syms s t tf1/ s/(T^2 * s^2 2* D* T *s 1);stepResponse(t)ilaplace(tf,s,t);plot(t_vec, stepResponse(t_vec));% % no needforexternal calculation: % analyticalSol[];%if(D1)% analyticalSol(t)(-(D*t/T 1).*exp(-D*t/T)1);% elseif(D1)% analyticalSol(t)(-(D*T^2*sinh(sqrt(D^2 -1)*t/T)/sqrt(D^2 -1) T^2*cosh(sqrt(D^2 -1)*t/T)).*exp(-D*t/T)/T^2 1);% elseif(D1)% analyticalSol(t)(-(D*T^2*sin(sqrt(-D^2 1)*t/T)/sqrt(-D^2 1) T^2*cos(sqrt(-D^2 1)*t/T)).*exp(-D*t/T)/T^2 1);% end % plot(t_vec, analyticalSol(t_vec));legend(code,discrete bw-euler,discrete bilinear,discrete fw euler,pt2 with trapezoid,analytical solution);hold on plot(t_vec,zeros(size(t_vec)),k.,HandleVisibility,off)% % t_vec_sys[0:1E-4:tmax];% systf([1],[T^22*D*T1]);% hold on;% step(sys,t_vec_sys,g);clear;syms T D Ts z s % Backward rectangular(euler)% s(z-1)/(Ts*z);% Bilinear transform: % s(2/Ts)*(z-1)/(z1);% Forward Euler s(z-1)/ Tstf1/(T^2 * s^2 2* D* T *s 1);collect(tf,z)