基于kalman和模糊pid的matlab实现方法

%Fuzzy PID Control
close all;
clear all;

a=readfis(‘fuzzpid’); %Load fuzzpid.fis

ts=0.001;
sys=tf(5.235e005,[1,87.35,1.047e004,0]);%系统模型
%sys=tf(16470,[1,1667,3393]);%系统模型
dsys=c2d(sys,ts,‘tustin’); %用双线性逼近法对‘sys’离散化
[num,den]=tfdata(dsys,‘v’); %获取离散模型dsys的分子、分母系数向量

u_1=0.0;u_2=0.0;u_3=0.0;
y_1=0;y_2=0;y_3=0;

x=[0,0,0]’; %PID3个参数增量组成的数组

e_1=0;
ec_1=0;

% kp0=0.6;
% kd0=1;
% ki0=0.1;

kp0=0.6;
kd0=1;
ki0=0.01;

for k=2:1:5000 %仿真时间
time(k)=kts;
%r(k)=sign(sin(2pikts));
r(k)=sin(2pik*ts);
% r(k)=1;

%Using fuzzy inference to tunning PID
k_pid=evalfis([e_1,ec_1],a); %执行模糊推理计算
kp(k)=kp0+k_pid(1);
ki(k)=ki0+k_pid(2);
kd(k)=kd0+k_pid(3);
% kp(k)=kp0;
% ki(k)=ki0;
% kd(k)=kd0;
u(k)=kp(k)*x(1)+kd(k)*x(2)+ki(k)*x(3);

y(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3+num(1)*u(k)+num(2)*u_1+num(3)*u_2+num(4)*u_3; %将传递函数变换为差分方程

Y=y+0.05*awgn(y,0.4)-0.05;
m(1) = 0.0;
p = 0.8;
Q =0.95;%过程噪声协方差
R =10;%观测噪声协方差

m(k) =m(k - 1);%预估计k时刻状态变量的值
p = p + Q;%对应于预估值的协方差，由于状态变量是标量，状态转移矩阵A为数值1
kg = p / (p + R);%kalman gain
m(k) = m(k) + kg * (y(k) - m(k));
p = (1 - kg) * p;

e(k)=r(k)-y(k);
%%%%%%%%%%%%%%Return of PID parameters%%%%%%%%%%%%%%%
u_3=u_2;
u_2=u_1;
u_1=u(k);

y_3=y_2;
y_2=y_1;
y_1=y(k);

x(1)=e(k); % Calculating P
x(2)=e(k)-e_1; % Calculating D
x(3)=x(3)+e(k)*ts; % Calculating I

ec_1=x(2);
e_2=e_1;
e_1=e(k);
end

% for k=1001:1:2000 %仿真时间
% time(k)=k*ts;
% r(k)=2;
%
% %Using fuzzy inference to tunning PID
% k_pid=evalfis([e_1,ec_1],a); %执行模糊推理计算
% kp(k)=kp0+k_pid(1);
% ki(k)=ki0+k_pid(2);
% kd(k)=kd0+k_pid(3);
% % kp(k)=kp0;
% % ki(k)=ki0;
% % kd(k)=kd0;
% u(k)=kp(k)*x(1)+kd(k)*x(2)+ki(k)*x(3);
%
%
% y(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3+num(1)*u(k)+num(2)*u_1+num(3)*u_2+num(4)u_3; %将传递函数变换为差分方程
%
% Y=y+0.05awgn(y,0.4)-0.05;
% m(1) = 0.0;
% p = 0.8;
% Q =0.95;%过程噪声协方差
% R =10;%观测噪声协方差
%
% m(k) =m(k - 1);%预估计k时刻状态变量的值
% p = p + Q;%对应于预估值的协方差，由于状态变量是标量，状态转移矩阵A为数值1
% kg = p / (p + R);%kalman gain
% m(k) = m(k) + kg * (Y(k) - m(k));
% p = (1 - kg) * p;
%
% e(k)=r(k)-m(k);
% %%%%%%%%%%%%%%Return of PID parameters%%%%%%%%%%%%%%%
% u_3=u_2;
% u_2=u_1;
% u_1=u(k);
%
% y_3=y_2;
% y_2=y_1;
% y_1=y(k);
%
% x(1)=e(k); % Calculating P
% x(2)=e(k)-e_1; % Calculating D
% x(3)=x(3)+e(k)*ts; % Calculating I
%
% ec_1=x(2);
% e_2=e_1;
% e_1=e(k);
% end

figure(1);
plot(time,r,‘b’,time,m,‘k’);
xlabel(‘time(s)’);ylabel(‘rin and yout’);
%legend(‘input value’,’ result’);
legend(‘input value’,‘kalman fuzzy PID’);

figure(2);
plot(time,r,‘b’,time,Y,‘k’);
xlabel(‘time(s)’);ylabel(‘rin and yout’);
%legend(‘input value’,’ result’);
legend(‘input value’,‘no kalman fuzzy PID’);

%
% figure(3);
% plot(time,e,‘r’);
% xlabel(‘time(s)’);ylabel(‘error’);
% % figure(3);
% % plot(time,u,‘r’);
% % xlabel(‘time(s)’);ylabel(‘u’);

figure(4);
plot(time,kp,‘r’,time,ki,‘g’,time,kd,‘b’);
xlabel(‘time(s)’);ylabel(‘kp、ki and kd’);
legend(‘kp value’,‘ki value’,‘kd value’);

% figure(5);
% plot(time,ki,‘r’);
% xlabel(‘time(s)’);ylabel(‘ki’);
% figure(6);
% plot(time,kd,‘r’);
% xlabel(‘time(s)’);ylabel(‘kd’);

%Fuzzy Tunning PID Control
clear all;
close all;

a=newfis(‘fuzzpid’);

a=addvar(a,‘input’,‘e’,[-3,3]); %Parameter e
a=addmf(a,‘input’,1,‘NB’,‘zmf’,[-3,-1]);
a=addmf(a,‘input’,1,‘NM’,‘trimf’,[-3,-2,0]);
a=addmf(a,‘input’,1,‘NS’,‘trimf’,[-3,-1,1]);
a=addmf(a,‘input’,1,‘Z’,‘trimf’,[-2,0,2]);
a=addmf(a,‘input’,1,‘PS’,‘trimf’,[-1,1,3]);
a=addmf(a,‘input’,1,‘PM’,‘trimf’,[0,2,3]);
a=addmf(a,‘input’,1,‘PB’,‘smf’,[1,3]);

a=addvar(a,‘input’,‘ec’,[-3,3]); %Parameter ec
a=addmf(a,‘input’,2,‘NB’,‘zmf’,[-3,-1]);
a=addmf(a,‘input’,2,‘NM’,‘trimf’,[-3,-2,0]);
a=addmf(a,‘input’,2,‘NS’,‘trimf’,[-3,-1,1]);
a=addmf(a,‘input’,2,‘Z’,‘trimf’,[-2,0,2]);
a=addmf(a,‘input’,2,‘PS’,‘trimf’,[-1,1,3]);
a=addmf(a,‘input’,2,‘PM’,‘trimf’,[0,2,3]);
a=addmf(a,‘input’,2,‘PB’,‘smf’,[1,3]);

a=addvar(a,‘output’,‘kp’,[-3,3]); %Parameter kp
a=addmf(a,‘output’,1,‘NB’,‘zmf’,[-3,-1]);
a=addmf(a,‘output’,1,‘NM’,‘trimf’,[-3,-2,0]);
a=addmf(a,‘output’,1,‘NS’,‘trimf’,[-3,-1,1]);
a=addmf(a,‘output’,1,‘Z’,‘trimf’,[-2,0,2]);
a=addmf(a,‘output’,1,‘PS’,‘trimf’,[-1,1,3]);
a=addmf(a,‘output’,1,‘PM’,‘trimf’,[0,2,3]);
a=addmf(a,‘output’,1,‘PB’,‘smf’,[1,3]);
% a=addvar(a,‘output’,‘kp’,[-0.3,0.3]); %Parameter kp
% a=addmf(a,‘output’,1,‘NB’,‘zmf’,[-0.3,-0.1]);
% a=addmf(a,‘output’,1,‘NM’,‘trimf’,[-0.3,-0.2,0]);
% a=addmf(a,‘output’,1,‘NS’,‘trimf’,[-0.3,-0.1,0.1]);
% a=addmf(a,‘output’,1,‘Z’,‘trimf’,[-0.2,0,0.2]);
% a=addmf(a,‘output’,1,‘PS’,‘trimf’,[-0.1,0.1,0.3]);
% a=addmf(a,‘output’,1,‘PM’,‘trimf’,[0,0.2,0.3]);
% a=addmf(a,‘output’,1,‘PB’,‘smf’,[0.1,0.3]);

a=addvar(a,‘output’,‘ki’,[-0.6,0.6]); %Parameter ki
a=addmf(a,‘output’,2,‘NB’,‘zmf’,[-0.6,-0.2]);
a=addmf(a,‘output’,2,‘NM’,‘trimf’,[-0.6,-0.4,0]);
a=addmf(a,‘output’,2,‘NS’,‘trimf’,[-0.6,-0.2,0.2]);
a=addmf(a,‘output’,2,‘Z’,‘trimf’,[-0.4,0,0.4]);
a=addmf(a,‘output’,2,‘PS’,‘trimf’,[-0.2,0.2,0.6]);
a=addmf(a,‘output’,2,‘PM’,‘trimf’,[0,0.4,0.6]);
a=addmf(a,‘output’,2,‘PB’,‘smf’,[0.2,0.6]);
% a=addvar(a,‘output’,‘ki’,[-0.06,0.06]); %Parameter ki
% a=addmf(a,‘output’,2,‘NB’,‘zmf’,[-0.06,-0.02]);
% a=addmf(a,‘output’,2,‘NM’,‘trimf’,[-0.06,-0.04,0]);
% a=addmf(a,‘output’,2,‘NS’,‘trimf’,[-0.06,-0.02,0.02]);
% a=addmf(a,‘output’,2,‘Z’,‘trimf’,[-0.04,0,0.04]);
% a=addmf(a,‘output’,2,‘PS’,‘trimf’,[-0.02,0.02,0.06]);
% a=addmf(a,‘output’,2,‘PM’,‘trimf’,[0,0.04,0.06]);
% a=addmf(a,‘output’,2,‘PB’,‘smf’,[0.02,0.06]);

a=addvar(a,‘output’,‘kd’,[-3,3]); %Parameter kd
a=addmf(a,‘output’,3,‘NB’,‘zmf’,[-3,-1]);
a=addmf(a,‘output’,3,‘NM’,‘trimf’,[-3,-2,0]);
a=addmf(a,‘output’,3,‘NS’,‘trimf’,[-3,-1,1]);
a=addmf(a,‘output’,3,‘Z’,‘trimf’,[-2,0,2]);
a=addmf(a,‘output’,3,‘PS’,‘trimf’,[-1,1,3]);
a=addmf(a,‘output’,3,‘PM’,‘trimf’,[0,2,3]);
a=addmf(a,‘output’,3,‘PB’,‘smf’,[1,3]);
% a=addvar(a,‘output’,‘kd’,[-3,3]); %Parameter kd
% a=addmf(a,‘output’,3,‘NB’,‘zmf’,[-3,-1]);
% a=addmf(a,‘output’,3,‘NM’,‘trimf’,[-3,-2,0]);
% a=addmf(a,‘output’,3,‘NS’,‘trimf’,[-3,-1,1]);
% a=addmf(a,‘output’,3,‘Z’,‘trimf’,[-2,0,2]);
% a=addmf(a,‘output’,3,‘PS’,‘trimf’,[-1,1,3]);
% a=addmf(a,‘output’,3,‘PM’,‘trimf’,[0,2,3]);
% a=addmf(a,‘output’,3,‘PB’,‘smf’,[1,3]);

rulelist=[1 1 7 1 5 1 1;
1 2 7 1 3 1 1;
1 3 6 2 1 1 1;
1 4 6 2 1 1 1;
1 5 5 3 1 1 1;
1 6 4 4 2 1 1;
1 7 4 4 5 1 1;

      2 1 7 1 5 1 1;
      2 2 7 1 3 1 1;
      2 3 6 2 1 1 1;
      2 4 5 3 2 1 1;
      2 5 5 3 2 1 1;
      2 6 4 4 3 1 1;
      2 7 3 4 4 1 1;
      
      3 1 6 1 4 1 1;
      3 2 6 2 3 1 1;
      3 3 6 3 2 1 1;
      3 4 5 3 2 1 1;
      3 5 4 4 3 1 1;
      3 6 3 5 3 1 1;
      3 7 3 5 4 1 1;
      
      4 1 6 2 4 1 1;
      4 2 6 2 3 1 1;
      4 3 5 3 3 1 1;
      4 4 4 4 3 1 1;
      4 5 3 5 3 1 1;
      4 6 2 6 3 1 1;
      4 7 2 6 4 1 1;
      
      5 1 5 2 4 1 1;
      5 2 5 3 4 1 1;
      5 3 4 4 4 1 1;
      5 4 3 5 4 1 1;
      5 5 3 5 4 1 1;
      5 6 2 6 4 1 1;
      5 7 2 7 4 1 1;
      
      6 1 5 4 7 1 1;
      6 2 4 4 5 1 1;
      6 3 3 5 5 1 1;
      6 4 2 5 5 1 1;
      6 5 2 6 5 1 1;
      6 6 2 7 5 1 1; 
      6 7 1 7 7 1 1;

      7 1 4 4 7 1 1; 
      7 2 4 4 6 1 1;
      7 3 2 5 6 1 1;
      7 4 2 6 6 1 1;
      7 5 2 6 5 1 1;
      7 6 1 7 5 1 1;
      7 7 1 7 7 1 1];

a=addrule(a,rulelist); %对模糊系统添加规则
a=setfis(a,‘DefuzzMethod’,‘centroid’); %Defuzzy 设置模糊系统的属性,清晰化方法采用重心法
writefis(a,‘fuzzpid’); %将模糊推理系统保存到磁盘

a=readfis(‘fuzzpid’); %从磁盘载入模糊推理系统

figure(1);
plotmf(a,‘input’,1);
figure(2);
plotmf(a,‘input’,2);
figure(3);
plotmf(a,‘output’,1);
figure(4);
plotmf(a,‘output’,2);
figure(5);
plotmf(a,‘output’,3);
% figure(6);
% plotfis(a);

% fuzzy fuzzpid;
% showrule(a);
% ruleview fuzzpid;