function A=identify_kautz_volterra(u,yd,P,M)
% IDENTIFY_VOLTERRA identifies a Volterra model
% A=IDENTIFY_VOLTERRA(U,YD,P,M) uses U as the input and YD as the desired
% output to identify a Volterra model of nonlinear order P and memory
% order M. If P and M are omitted, the default values are P=5 and M=3.
%
% The return parameter A is a struct with P, M and theta.

if nargin<3
    P=5;
    M=3;
end
if (P+1)/2~=round((P+1)/2)
    error('P must be odd');
end
A.P=P;
A.M=M;
A.poles=[];
M1=A.M;
for P1=1:2:A.P
    Dbest=-9999;
    for Amp=0:0.15:0.99
        steps=max(0.01,floor(Amp*20));
        for phase=0:(2*pi/steps):2*pi-1e-4
            H=geth(u,P1,M1,[A.poles; Amp*exp(j*phase)]);
            A.theta=pinv(H)*yd;
%            A.theta=H\yd;
            y=H*A.theta;
            D=10*log10(var(yd)/var(yd-y));
%    disp([num2str(Amp) '  ' num2str(phase*180/pi) '   ' num2str(D)]);
            if D>Dbest
                Dbest=D;
                pole=Amp*exp(j*phase);
                theta=A.theta;
            end
        end
    end
    disp([num2str(abs(pole)) '  ' num2str(angle(pole)*180/pi) '   ' num2str(Dbest)]);
    A.poles=[A.poles;pole];
    A.theta=theta;
end
A.name='kautz_volterra';

function H=geth(x,P,M,poles)

if length(poles)~=(P+1)/2
    error('Number of poles and nonlinear order should match.');
end
s=0;
for p=0:(P-1)/2
    s=s+nchoosek(M+p,p)*nchoosek(M+p+1,p+1);
end
H=zeros(length(x),s);

v=g(x,poles(1),M+1);
n=0;
for i1=0:M   %1
    n=n+1;
    H(:,n)=v(:,i1+1);
end
if P>=3
    v=g(x,poles(2),M+1);
    for i1=0:M   %3
        for i2=i1:M
            for j1=0:M
                n=n+1;
                H(:,n)=v(:,i1+1).*v(:,i2+1).*conj(v(:,j1+1));
            end
        end
    end
end
if P>=5
    v=g(x,poles(3),M+1);
    for i1=0:M   %5
        for i2=i1:M
            for i3=i2:M
                for j1=0:M
                    for j2=j1:M
                        n=n+1;
                        H(:,n)=v(:,i1+1).*v(:,i2+1).*v(:,i3+1).*conj(v(:,j1+1)).*conj(v(:,j2+1));
                    end
                end
            end
        end
    end
end
if P>=7
    v=g(x,poles(4),M+1);
    for i1=0:M   %7
        for i2=i1:M
            for i3=i2:M
                for i4=i3:M
                    for j1=0:M
                        for j2=j1:M
                            for j3=j2:M
                                n=n+1;
                                H(:,n)=v(:,i1+1).*v(:,i2+1).*v(:,i3+1).*v(:,i4+1).*conj(v(:,j1+1)).*conj(v(:,j2+1)).*conj(v(:,j3+1));
                            end
                        end
                    end
                end
            end
        end
    end
end
if P>=9
    v=g(x,poles(5),M+1);
    for i1=0:M   %9
        for i2=i1:M
            for i3=i2:M
                for i4=i3:M
                    for i5=i4:M
                        for j1=0:M
                            for j2=j1:M
                                for j3=j2:M
                                    for j4=j3:M
                                        n=n+1;
                                        H(:,n)=v(:,i1+1).*v(:,i2+1).*v(:,i3+1).*v(:,i4+1).*v(:,i5+1).*conj(v(:,j1+1)).*conj(v(:,j2+1)).*conj(v(:,j3+1)).*conj(v(:,j4+1));
                                    end
                                end
                            end
                        end
                    end
                end
            end
        end
    end
end
if n~=s, error('Something wrong'); end;

function v=g(x,pole,L)
% x is input data, N*1. pole is the pole.

v=zeros(length(x),L);
v(:,1)=x;
a=[1 -pole];
b=[-conj(pole) 1];
for i=2:L
    v(:,i)=filter(b,a,v(:,i-1));
end
for i=1:L
    v(:,i)=filter(1,a,v(:,i));
end