DOA定位算法源码程序

% initialization
clear
y0=0;y1=0;u0=0.1; ve=0.1;u2=0;u1=0.1;d=0;e=0.001;e0=0;e1=0;e2=0.001;u=1;M=200; N=3
d=randn(M,1);
for i=1:M  %开始循环
d1=0.022*d(i);
u1=u;
for k=1:N  %点循环
if e==-6 |e==-4 & ve==-6| ve==-4    %规则1
u=6; else, end  
if e==-6 |e==-4 & ve==-4| ve==0     %规则2   
u=6; else, end
if e==-6 |e==-4 & ve==4             %规则3
u=4;else, end 
if e==-6 |e==-4 & ve==4| ve==6      %规则4
u=0; else, end 
if e==-4 & ve==-6| ve==-4           %规则5
u=4;else, end 
if e==-4  & ve==-4| ve==0           %规则6  
u=4; else, end
if e==-4 & ve==4                    %规则7
u=0; else, end 
if e==-4  & ve==4| ve==6            %规则8
u=-2; else, end 
if e==0  & ve==-6| ve==-4           %规则9
u=4; else, end 
if e==0  & ve==-2                   %规则10
u=2; else, end 
if e==0  & ve==0                    %规则11
u=0; else, end 
if e==0 4 & ve==2                   %规则12
u=-2; else, end 
if e==0  & ve==4| ve==6             %规则13
u=-4; else, end 
if e==2  & ve==-6| ve==-4          %规则14
u=2; else, end 
if e==2 & ve==-2                    %规则15
u=0; else, end 
if e==2  & ve==0| ve==2             %规则16
u=-4; else, end 
if e==2  & ve==4| ve==6             %规则17
u=-4; else, end 
if e==4 |e==6 & ve==-6| ve==-4       %规则18
u=0; else, end 
if e==4 |e==6 & ve==-2              %规则19
u=-4; else, end 
if e==4 |e==6 & ve==0               %规则20
u=-6; else, end 
if e==4 |e==6 & ve==4| ve==6        %规则21
u=-6; else, end 
u1=u;
%y2=0.36*y1-0.132*y0+0.117*u1+0.076*u0+d1
y2=0.36*y1-0.132*y0+0.116*u1+0.076*u0+d1; %有噪声系统
y3(i)=y2;      %存储系统输出，标幺化系统最大输出为1
up(i)=1;       %系统输入为1
d2(i)=d1;      %存噪声信号
e=(u1/6-y2)*6; %模糊规则中u(k)的范围为{-6，6}，处理再还原到6
e2(i)=e;       %存储放大6倍的误差
ve=(e1-e0);
ve=round(ve);   %取整数，以便模糊运算
e=round(e);     %取整数，以便模糊运算
e0=e1;e1=e;
y0=y1;
y1=y2;
u0=u1; 
if  e<=0.001  break; %判断误差
else
end
end  %对应for k=1:N
end  %循环结束
M=i
%greapher
i=1:M;
subplot(3,1,1)
plot(i,up,i,y3,i,y3,'rx')
%title('step response of system'),
xlabel('k'),ylabel('up  and  y3')
 legend('up is system input','y3 is system output'); %图标注 
subplot(3,1,2)
e=0.5*(e2/6).^2   %求还原误差的均方值
plot(e)
xlabel('k'),ylabel('误差e')
subplot(3,1,3)
plot(d2)
xlabel('k'),ylabel('噪声d')'