levy.m

用matlab实现motor运动控制的控制器设计仿真

function [num,den]=levy(fre,mag,ang,n)
%chapter 6 exercise 4
%levy法拟合成传递函数
%input
% fre-角频率(rad/s),mag-幅值,ang-相位(角度单位),n-分母阶数
%output
% num-分子系数,den-分母系数
alpha=1e-4;%精度
P=mag.*cos(pi*ang/180);
Q=mag.*sin(pi*ang/180);
N=length(fre);
omga_1=ones(1,N);
L=1;
while (1)
    gama=zeros(1,2*n);
    S=zeros(1,2*n);
    T=zeros(1,2*n);
    U=zeros(1,2*n);
    for k=1:2*n
        if mod(k,2)==1
            gama(k)=sum(omga_1.*(fre.^(k-1))');
            S(k)=sum(omga_1.*(fre.^(k-1))'.*P');
        else 
            T(k)=sum(omga_1.*(fre.^(k-1))'.*Q');
            U(k)=sum(omga_1.*(fre.^(k))'.*(P.^2+Q.^2)');
        end
    end
    for i=1:n
        for j=1:n
            if mod(i,2)==1
                if mod(j+1,4)==0
                    A11(i,j)=-gama(i+j-1);
                    Tt(i,j)=-T(i+j);
                    A22(i,j)=-U(i+j);
                else
                    A11(i,j)=gama(i+j-1);
                    Tt(i,j)=T(i+j);
                    A22(i,j)=U(i+j);
                end
                if mod(j,4)==0
                    St(i,j)=-S(i+j);
                else
                    St(i,j)=S(i+j);
                end
            else
                if mod(j,4)==0
                    A11(i,j)=-gama(i+j-1);
                    Tt(i,j)=-T(i+j);
                    A22(i,j)=-U(i+j);
                else
                    A11(i,j)=gama(i+j-1);
                    Tt(i,j)=T(i+j);
                    A22(i,j)=U(i+j);
                end
                if mod(j+3,4)==0
                    St(i,j)=-S(i+j);
                else
                    St(i,j)=S(i+j);
                end
            end
        end
    end
    A12=Tt+St;
    A21=Tt-St;
    A=[A11,A12;A21,A22];
    B=[S(1:n)+T(1:n),U(1:n)]';
    X=[inv(A)*B]';
    X2=X(n+1:2*n);
    for i=1:N
        A1=1;A2=0;
        for k=1:n
            if mod(k,2)==1
                if mod(k+1,4)==1
                    A2=A2+X2(k)*fre(i)^k;
                else
                    A2=A2-X2(k)*fre(i)^k;
                end
            elseif mod(k,2)==0
                if mod(k,4)==0
                    A1=A1+X2(k)*fre(i)^k;
                else
                    A1=A1-X2(k)*fre(i)^k;
                end
            end
        end
        D(i)=A1^2+A2^2;
    end
    omga_2=1./D;
    z=sum(abs(1-omga_2./omga_1))/N;
    if z<=alpha
        break
    end
    omga_1=omga_2;
    L=L+1;
    if L>100*n 
        error('No convergence');
        break
    end
end 
num=ones(1,n);
den=ones(1,n+1);
for k=1:n
    num(k)=X(n+1-k);
    den(k)=X(2*n+1-k);
end