基于前馈补偿的PID控制算法

%PID Feedforward Controler
clear all;
close all;

ts=0.001;
sys=tf（133[1250]）;
dsys=c2d（systs‘z‘）;
[numden]=tfdata（dsys‘v‘）;

u_1=0;u_2=0;
y_1=0;y_2=0;

error_1=0;ei=0;
for k=1:1:1000
time（k）=k*ts;
   
A=0.5;F=3.0;
rin（k）=A*sin（F*2*pi*k*ts）; 
drin（k）=A*F*2*pi*cos（F*2*pi*k*ts）;
ddrin（k）=-A*F*2*pi*F*2*pi*sin（F*2*pi*k*ts）;

%Linear model
yout（k）=-den（2）*y_1-den（3）*y_2+num（2）*u_1+num（3）*u_2;

error（k）=rin（k）-yout（k）;

ei=ei+error（k）*ts;

up（k）=80*error（k）+20*ei+2.0*（error（k）-error_1）/ts;

uf