基于卡尔曼滤波的PID控制

clear all;
close all;

ts=0.001;

a=25;b=133;
sys=tf（b[1a0]）;
dsys=c2d（systs‘z‘）;
[numden]=tfdata（dsys‘v‘）;

A1=[0 1;0 -a];
B1=[0;b];
C1=[1 0];
D1=[0];
[ABCD]=c2dm（A1B1C1D1ts‘z‘）;

Q=1;              
R=1;               

P=B*Q*B‘;          
x=zeros（21）;       

u_1=0;u_2=0;
y_1=0;y_2=0;
ei=0;
error_1=0;
for k=1:1:1000
time（k）=k*ts;

yd（k）=sin（0.01*k）;
kp=80.0;ki=10;kd=0.2;

w（k）=0.002*rands（1）;   
v（k）=0.002*rands（1）;   
y（k）=-den（2）*y_1-den（3）*y_2+num（2）*u_1+num（3）*u_2;
yv（k）=y（k）+v（k）;


Mn=P*C‘/（C*P*C‘+R）;
P=A*P*A‘+B*Q*B‘; 
P=（eye（2）-Mn*C）*P;
    
x=A*x+Mn*（yv（k）-C*A*x）;
ye（k）=C*x+D;     
    
M=1;
if M==1         
 	yout（k）=yv（k）;
elseif M==2     
   yout（k）=ye（k）;
end
error（k）=yd（k）-yout（k）;
ei=ei+error（k）*ts;

u（k）=kp*error（k）+ki*ei+kd*（error（k）-error_1）/ts;  
u（k）=u（k）+w（k）;

errcov（k）=C*P*C‘;     


x=A*x+B*u（k）;

u_2=u_1;u_1=u（k）;
y_2=y_1;y_1=yout（k）;
error_1=error（k）;
end
figure（1）;
plot（timeyd‘r‘timeyout‘k:‘‘linewidth‘2）;
xlabel（‘time（s）‘）;ylabel（‘ydyout‘）;
legend（‘Ideal position signal‘‘Position tracking‘）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      21965  2017-12-22 11:35  先进PIDkaerman.slx

     文件       1130  2017-12-22 11:21  先进PIDkaermanPID.m

     文件        299  2017-12-22 11:20  先进PIDkaermanplot.m

     文件       1121  2017-12-22 11:17  先进PIDUntitled1jy.m

     文件       1121  2017-12-22 11:18  先进PIDUntitledjy2.m

     文件       1124  2017-12-22 11:12  先进PIDUntitledxp1.m

     文件       1124  2017-12-22 11:13  先进PIDUntitledxp2.m

     文件       1128  2017-12-22 11:09  先进PIDUntitledzx1.m

     文件       1128  2017-12-22 11:09  先进PIDUntitledzx2.m

     文件     238792  2017-12-23 14:19  先进PID卡尔曼滤波的基本原理及应用_彭丁聪_看图王.pdf

     文件    1050706  2017-12-23 14:18  先进PID基于卡尔曼滤波器的PID设计.pptx

     目录          0  2017-12-28 15:24  先进PID

----------- ---------  ---------- -----  ----

              1319638                    12