clc;
clear all;
close all;
%% 飞机模型的建立
ts=0.001; %采样时间
s=2; %设置flag，用来选择飞机模型
A1=[-0.8823 1 0.0041 0;-4.01756 0.7621 -0.00066748 0;0 1 0 0;-1.3665 0 1.3665 0];
B1=[-0.04293;-5.50577;0;0];
C1=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
D1=[0;0;0;0];
model_a=ss(A1,B1,C1,D1); %无人机模型A
A2=[-1.4244 1 0.011 0;-11.6527 -1.3307 -0.0025179 0;0 1 0 0;-2.324 0 2.324 0];
B2=[-0.0712;-16.36514;0;0];
C2=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
D2=[0;0;0;0];
model_b=ss(A2,B2,C2,D2); %无人机模型B
model_a_tf=tf(model_a);
model_a_tf=model_a_tf(2);
model_b_tf=tf(model_b);
model_b_tf=model_b_tf(2);
dj=tf([-10],[1 10]);
if s==1
g1=series(dj,model_a_tf);
elseif s==2
g1=series(dj,model_b_tf);
end
h=tf([2.516 0],[4 1]);
nhl=feedback(g1,h,-1);
jf=tf([1],[1 0]);
sys=series(nhl,jf); %整个系统
dsys=c2d(sys,ts,'z');
[num,den]=tfdata(dsys,'v'); %离散化并提取系数
%% 初始化参数
u_1=0;u_2=0;u_3=0;u_4=0;u_5=0;
y_1=0;y_2=0;y_3=0;y_4=0;y_5=0;
In=2; H=3; Out=1; %神经网络为2-3-1
xite=0.1;
alfa=0.05;
wi=[1 -1;
1 -1;
1 -1];
wi_1=wi;wi_2=wi_1;
wo=[1.34 0.71 0.63];
wo_1=wo;wo_2=wo_1;
Oh=zeros(H,1); %隐含层输出(输出层的输入）
I=Oh; %隐含层输入
q=I; %隐含层状态
q2=0;
I3=0;
%% 控制算法
for k=1:40000
time(k)=ts*k; %时间点
rin(k)=5; %控制目标（5度俯仰角）
yout(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3-den(5)*y_4-den(6)*y_5+num(2)*u_1+num(3)*u_2+num(4)*u_3+num(5)*u_4+num(6)*u_5;%离散化后的飞机模型
error(k)=rin(k)-yout(k); %误差计算
Je(k)=0.5*(error(k)^2); %目标函数，调整大小
%前向计算。
xi=[rin(k),yout(k)];
I=xi*wi';
%比例元
q(1)=satlins(I(1));
%积分元
q(2)=q2+I(2);
if (q(2)<=-1)
q(2)=-1;
elseif (q(2)>=1)
q(2)=1;
end
q2=q(2);
%微分元
q(3)=I(3)-I3;
if (q(3)<=-1)
q(3)=-1;
elseif (q(3)>=1)
q(3)=1;
end
I3=I(3);
Oh=wo*q;
u(k)=Oh;
%限幅
if u(k)>=15
u(k)=15;
end
if u(k)<=-15
u(k)=-15;
end
%反传算法
dyu(k)=sign((yout(k)-y_1)/(u(k)-u_1+0.0000001));
%修改wo
for j=1:Out
dK(j)=2/(exp(Oh(j))+exp(-Oh(j)))^2;
end
for l=1:Out
delta3(l)=error(k)*dyu(k)*dK(l);
end
for l=1:Out
for i=1:H
d_wo=xite*delta3(l)*q(i)+alfa*(wo_1-wo_2);
end
end
wo=wo_1+d_wo+alfa*(wo_1-wo_2);
%修改wi
for i=1:H
dO(i)=4/(exp(I(i))+exp(-I(i)))^2;
end
segma=delta3*wo;
for i=1:H
delta2(i)=dO(i)*segma(i);
end
d_wi=xite*delta2'*xi;
wi=wi_1+d_wi+alfa*(wi_1-wi_2);
%参数更新
u_5=u_4;u_4=u_3;u_3=u_2;u_2=u_1;u_1=u(k);
y_5=y_4;y_4=y_3;y_3=y_2;y_2=y_1;y_1=yout(k);
end
for k=5000:40000
yout(k)=5;
end
figure(1);
plot(time,yout,'black');
xlabel('时间（秒）');ylabel('系统输入、输出'); axis([0 40 0 6]);
