%改進的蟻群算法應(yīng)用于搜索信號有效帶寬
%信噪比SNR=-10dB
%信號有效帶寬250~300Hz

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%高斯噪聲環(huán)境下寬帶矢量信號
%固定信號源，水聽器位于原點
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all
clc;

%初始化參數(shù)
x_a=0;
y_a=0;           %水聽器坐標(biāo)
len=8192;        %采樣點數(shù)
fs=4096;         %采樣頻率
SNR=-10;         %信噪比
nfft=4096;
%信號帶寬
freq_up=301;     
freq_down=350;
%待檢測頻率范圍
up=201;
down=500;

%信號源方位
target_x=30;
target_y=40;
r_s=sqrt((target_x-x_a)^2+(target_y-y_a)^2); 
phi_s=atan2(target_y-y_a,target_x-x_a);

%產(chǎn)生噪聲
An=8;                  %噪聲幅度基值
W=randn(len,4);
N=[1,0,0,0;
   0,1/3,0,0;
   0,0,1/3,0;
   0,0,0,1/3];
N=sqrt(N);
noise=W*N;
p_n=noise(:,1);
En_n=std(An*p_n).^2;            %噪聲能量	
df=fs/2;           
dEn_n=En_n/df;                  %能量密度

%產(chǎn)生隨機信號
signal_p=randn(len,1);
%信號通過橢圓帶通濾波器
wn=[freq_up freq_down]*2/fs;
[b,a]=ellip(6,0.1,30,wn);
signal=filter(b,a,signal_p);

%帶寬信號
En_s=std(signal).^2;                    %信號能量
dEn_s=En_s/(freq_down-freq_up);         %信號能量密度
A_s=sqrt(2*10.^(SNR/10)*En_n/En_s);   %信號幅值
p_s=A_s.*signal;       %聲壓  
vx_s=p_s.*cos(phi_s);  %X方向振速
vy_s=p_s.*sin(phi_s);  %Y方向振速

%信號+噪聲
p=p_s+An*noise(:,1);
En=sum(p.^2)/length(p);
vx=vx_s+An*noise(:,2);
vy=vy_s+An*noise(:,3);
vz=An*noise(:,4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%建立信號頻率與蟻群算法中路徑長度的關(guān)系
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功率譜估計
window=hanning(nfft);               %加漢寧窗
[En_p,f]=psd(p,nfft,fs,window);
[En_vx,f]=psd(vx,nfft,fs,window);
[En_vy,f]=psd(vy,nfft,fs,window);
[pvx,f]=csd(p,vx,nfft,fs,window);
[pvy,f]=csd(p,vy,nfft,fs,window);
[pvz,f]=csd(p,vz,nfft,fs,window);

figure(1);
plot(f,10*log10(En_p),'k');
title('聲壓功率譜');
xlabel('frequence/Hz');
ylabel('Energy/dB');

En_p=En_p(up:down);
En_vx=En_vx(up:down);
En_vy=En_vy(up:down);
pvx=pvx(up:down);
pvy=pvy(up:down);
pvz=pvz(up:down);

temp_f=f(up:down);
l_length=length(temp_f);  
for i=1:l_length
    enegry(i,1)=real(En_p(i));
    enegry(i,2)=real(pvx(i));
    enegry(i,3)=real(pvy(i));
    enegry(i,4)=real(pvz(i));
    I(i)=sqrt(enegry(i,2).^2+enegry(i,3).^2+enegry(i,4).^2);
end
l=abs(10*log10(1000./I))+2;

figure(2);
plot(f(up:down),l,'k');
title('路徑');
xlabel('frequence/Hz');
ylabel('length');


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%蟻群算法
%參考文獻：《蟻群算法原理及其應(yīng)用》 段海濱
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%參數(shù)初始化
number=down-up+1;
q=1:number;
alpha=0.2;      %信息啟發(fā)式因子
beta=0.08;       %期望啟發(fā)式因子
tao(q)=0;        %路徑的初始信息量
rou=0.5 ;        %信息素揮發(fā)系數(shù)
N=2000;         %蟻群大小
Q=30;          %信息素強度
D(q)=l;       % 路徑的長度設(shè)定
yita=1./l';        %路徑的啟發(fā)函數(shù)
min_beta=1.5;

  %總搜索次數(shù)為time
time=20;           
n(q,time)=0;
beta_1(time)=0;
beta_1(1)=beta;
b=2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%路徑上信息素為零，蟻群第一次對路徑進行隨機選擇
for i_0=1:N
    p=rand;
    p_0=1/number;
    sum_p_0=0;
    for j_0=1:number
        sum_p_0=sum_p_0+p_0;
        if sum_p_0>p
            n(j_0,1)=n(j_0,1)+1;
            break
        end
    end
end
            
%第一次選擇后，路徑上信息素改變
detatao=n(: ,1).*Q./l';
tao=(1-rou).*tao'+detatao;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%蟻群從第二次開始連續(xù)進行time-1次循環(huán)選擇
for k=2:time
    detatao=0;
    for i=1:N
        %每只螞蟻在選擇路徑之前，先計算各條路徑的選擇概率
        sum_1=sum(tao.^alpha.*yita.^beta);
        p(q)=tao.^alpha.*yita.^beta./sum_1;
        %運用輪盤賭選擇算法
        p_1=rand;
        sum_p=0;
        for j=1:number
            sum_p=sum_p+p(j);
            if sum_p>p_1;
                n(j,k)=n(j,k)+1;
                break
            end
        end
    end
    
%根據(jù)每次循環(huán)輸出的最優(yōu)解自動調(diào)節(jié)alpha因子   
    for c_0=1:6
        n_0(c_0,k)=sum(n(50*(c_0-1)+1:50*c_0,k));
    end 
    A_0=n_0(1,k);
    for c_1=2:6
        if A_0<=n_0(c_1,k)
            A_0=n_0(c_1,k);
            c_2=c_1;
        end
    end
    n_2(1)=sum(n(50*(c_2-1)+1:50*c_2,1));
    n_2(k)=sum(n(50*(c_2-1)+1:50*c_2,k));
    if (n_2(k)<=n_2(k-1))&(b*beta<=min_beta)
        beta=b*beta;
    else
        beta=beta;
    end
beta_1(k)=beta;

%信息素更新
detatao=n(:,k).*Q./l';
tao=(1-rou)*tao+detatao;
end
%在循環(huán)搜索后統(tǒng)計被檢測信號的每個頻率平均螞蟻數(shù)目
%輸出檢測到的信號的有效頻帶
A(number)=0;
for d=1:number
    sum_a(d)=sum(n(d,:));
    average(d)=sum_a(d)/time;
    if average(d)>10
        A(d)=d;
    end 
end
for d_1=1:number
    if A(d_1)~=0
        freq_up_1=A(d_1)+200;
        break
    end
end
for d_2=number:-1:1
    if A(d_2)~=0
        freq_down_1=A(d_2)+200;
        break
    end
end
%噪聲通過橢圓帶通濾波器
wn0=[freq_up_1 freq_down_1]*2/fs;
[b0,a0]=ellip(6,0.1,30,wn0);
noise0=filter(b0,a0,p_n);
En_n0=std(An*noise0).^2;

En_s1=std(p_s).^2;
SNR0=10*log10(En_s1/En_n0);

for k_1=1:time
    for c_3=1:6
        n_3(c_3,k_1)=sum(n(50*(c_3-1)+1:50*c_3,k_1));
    end
end

for c_4=1:6
    n_4(c_4)=sum(n_3(c_4,:))/time;
end
B=n_4(1);
for c_5=2:6
    if B<=n_4(c_5)
        B=n_4(c_5);
        c_6=c_5;
    end
end

figure(3);
plot(n_3(c_6,:),'k');
title('有效路徑(帶寬)上的螞蟻數(shù)目');
xlabel('number');
ylabel('time');

figure(4);
plot(f(up:down),average,'k');
% title('搜索完成后各個頻率上的螞蟻分布');
xlabel('頻率(Hz)');
ylabel('螞蟻數(shù)目只');

n
n_3
n_2
beta_1
n_4
freq_up_1
freq_down_1
SNR0