afc_estimate.m

这是AFC(自适应频率控制)程序

% Project Name:	AFC
% FILE NAME :	AFC_estimate.m
% ABSTRUCT:	This file is for AFC estimate.


%Initialization
AFC_detect;

%continue only when tone found
if (AFC_DETECT_BIT==1)
    AFC_est_init;
    afc_b0n=AFC_B0_N;
    afc_Estimate_iir_a2=AFC_A_2;
    afc_est_a1=afc_a1+afc_a1*AFC_ADJUST_A1;
    afc_r_zero=AFC_R_ZERO;
    afc_mscale=AFC_MSCALE;
    afc_delta=AFC_DELTA;
    afc_alphap=0.82/9;
    afc_e_comp=AFC_E_COMP;
    afc_sum_x2=0;
    afc_sum_xy=0;

    input_index=detected_sample-302;
    afc_Estimate_yn_2=x(input_index+0*2);
    afc_Estimate_yn_1=x(input_index+1*2);
    input_index=detected_sample-302+1*2;

    %if n5=1, ie pole adaptaion
    if (n5==1)
        for n=1:1:124
            input_index=input_index+2;
            %IIR
            afc_Estimate_yn=afc_b0n*x(input_index) + afc_Estimate_yn_1*afc_est_a1 + afc_Estimate_yn_2*afc_Estimate_iir_a2;
        
            %Pole adaptation
            pole_gain=-afc_Estimate_yn_1*afc_est_a1 + afc_Estimate_yn_2*afc_r_zero + afc_r_zero*afc_Estimate_yn;   
            step_gain=afc_delta*afc_Estimate_yn_1*afc_mscale;
    
            if (afc_Estimate_yn_1*pole_gain > 0)
                if ((step_gain-pole_gain)*afc_Estimate_yn_1 < 0)
                    afc_delta=afc_delta*2;
                else 
                    afc_delta=afc_delta/2;
                end
                afc_est_a1=afc_est_a1 + afc_delta*afc_alphap;
            else
                if ((step_gain+pole_gain)*afc_Estimate_yn_1 < 0)
                    afc_delta=afc_delta*2;
                else
                    afc_delta=afc_delta/2;
                end
                afc_est_a1=afc_est_a1 - afc_delta*afc_alphap;
            end
            afc_Estimate_yn_2=afc_Estimate_yn_1;
            afc_Estimate_yn_1=afc_Estimate_yn;
        end
    end

    for j=1:1:length(scale_tbl)
        if (afc_e_comp <= afc_Ex_n_1)
            break;
        end
        afc_e_comp=afc_e_comp/2;
    end
    scale=scale_tbl(j);
    afc_b0n=scale*afc_b0n*16;

    for i=0:1:1
        input_index=detected_sample-302;
        afc_Estimate_yn_2=x(input_index+0*2+i)*scale*16;
        afc_Estimate_yn_1=x(input_index+1*2+i)*scale*16;
        input_index=detected_sample-302+1*2+i;

        %IIR filter the first 26 data
        for n=1:1:26
            input_index=input_index+2;
            afc_Estimate_yn=afc_b0n*x(input_index) + afc_Estimate_yn_1*afc_est_a1 + afc_Estimate_yn_2*afc_Estimate_iir_a2;
            afc_Estimate_yn_2=afc_Estimate_yn_1;
            afc_Estimate_yn_1=afc_Estimate_yn;
        end

    %simualte DSP implementation    
        afc_sum_xy=afc_sum_xy+(AFC_SUM_XY_COEFF*afc_Estimate_yn_1*afc_Estimate_yn_2);

        %LSE over 12 sets of data, each set has 8 data points
        for j=1:1:12
            afc_sx2=0;
            afc_sxy=0;
            for k=1:1:8
                input_index=input_index+2;
                afc_Estimate_yn=afc_b0n*x(input_index) + afc_Estimate_yn_1*afc_est_a1 + afc_Estimate_yn_2*afc_Estimate_iir_a2;
                afc_sx2=afc_sx2+afc_Estimate_yn_1^2/8;
                afc_sxy=afc_sxy+afc_Estimate_yn_1*afc_Estimate_yn;
                afc_Estimate_yn_2=afc_Estimate_yn_1;
                afc_Estimate_yn_1=afc_Estimate_yn;
            end
            afc_sxy=afc_sxy/8;
            afc_sum_x2=afc_sum_x2+1/24*afc_sx2;
            afc_sum_xy=afc_sum_xy+1/12*afc_sxy;
        end
        afc_sum_xy=afc_sum_xy-(AFC_SUM_XY_COEFF*afc_Estimate_yn_1*afc_Estimate_yn_2);
    %end simulate DSP implementation

    %sum over 96 samples
    %    for j=1:1:96
    %        input_index=input_index+2;
    %        afc_Estimate_yn=afc_b0n*x(input_index) + afc_Estimate_yn_1*afc_est_a1 + afc_Estimate_yn_2*afc_Estimate_iir_a2;
    %        afc_sum_x2=afc_sum_x2+afc_Estimate_yn_1^2;
    %        afc_sum_xy=afc_sum_xy+afc_Estimate_yn*afc_Estimate_yn_1;
    %        afc_Estimate_yn_2=afc_Estimate_yn_1;
    %        afc_Estimate_yn_1=afc_Estimate_yn;
    %    end

    end

    theta=0.965*abs(afc_sum_xy/2)/afc_sum_x2+0.174*(abs(afc_sum_xy/2)/afc_sum_x2)^2;
    if (afc_sum_xy/2 < 0)
        theta=-theta;
    end
    FreqErr=theta/2/pi*270833.333;
    EstFreq=270833.333/4-FreqErr;
    fprintf(1,'Estimated Frequency error=%d Hz\n',FreqErr);
    fprintf(1,'Estimated Frequency=%d Hz\n',EstFreq);
end