image thumbnail

CRCBode() - Contoured Robust Controller Bode Plot Function

by

 

Generate controller Bode plots with contours showing robust performance and stability metric.

CRCBode(wi,mPi,aPi,mCi,aCi,Wui,Wsi,mLim,aLim,cLim,controlStyle,contourValues,contourStyles)
function CRCBode(wi,mPi,aPi,mCi,aCi,Wui,Wsi,mLim,aLim,cLim,controlStyle,contourValues,contourStyles)

    global mAxis aAxis

    % CRCBODE Create contoured robust controller bode (CRCBode) plots. 
    % Calculate and plot forbidden regions (shaded) in which controller magnitude 
    % and phase do not satisfy robust performance criteria.
    %
    % Robust Performance Criteria:
    % Gamma = |Ws*S|+|Wu*T|
    % Q = 20*log10(|Ws*S|+|Wu*T|)
    % Q = 10*log10((Wu*mP*mC)^2+2*Wu*Ws*mP*mC+Ws^2)/(1+2*mP*mC*cosd(aP+aC)+(mP*mC)^2)
    % Desire Q < 0, for all plants (in uncertainty set) at all frequencies
    %
    % Usage: JD_RCBode(wi,mPi,aPi,mCi,aCi,Wui,Wsi,mLim,aLim)
    %
    %	wi            : Frequency vector [Hz]
    %	mPi           : Plant magnitude vector [dB]
    %	aPi           : Plant phase vector [deg]
    %	mCi           : Controller magnitude vector [dB]
    %	aCi           : Controller phase vector [deg]
    %	Wui           : Uncertainty weighting vector [dB]
    %	Wsi           : Sensitivity weighting vector [dB]
    %   mLim          : Mangitude axis limit:  [min dB, max dB] or 'auto' 
    %   aLim          : Phase axis limit:  [min deg, max deg] or 'auto'
    %   cLim          : Q colorbar axis limits [min Q, max Q] [dB]
    %   controlStyle  : Cell array defining line style for controller (1x3)
    %                  {Color, LineStyle, LineWidth}
    %   contourValues : Vector of contours to highight (nC x 1) [dB]
    %   contourStyles : Cell array of line styles for highlighted contours (nC x 3)
    %                  {Color, LineStyle, LineWidth; etc.}
    %
    %--------------------------------------------------------------------------
    % Author        : JD Taylor (Carnegie Mellon University)
    % Rev. 1        : 2011.07.07
    % Rev. 2        : 2012.03.23
    %   - Added inputs specifying contour levels to highlight
    %   - Plot each contour segment independently to prevent connecting lines

    % Set number of test points (resolution) for 
    % Frequency (w), Magnitude (m), and Phase angle (a) 
    Nw = 500;
    Nm = 200;
    Na = 200;

    n = 20; % number of contour steps

    % Ensure plant magnitude and phase are (length(wi) x Np)
    % where Np is number of plants or linearizations
    % i.e. multiple plant freq responses enter as column vectors 
    % in mPi & aPi
    if size(mPi,2)==length(wi) mPi = mPi'; end
    if size(aPi,2)==length(wi) aPi = aPi'; end
    Np = size(mPi,2);

    % Create logarithmically spaced frequency test vector (Nw x 1)
    w = logspace(log10(min(wi)),log10(max(wi)),Nw)';

    % Interpolate input plant and weighting vectors at test frequencies (Nw x 1)
    mP = 10.^(interp1(wi,mPi,w)./20);
    aP = interp1(wi,aPi,w);
    Wu = 10.^(interp1(wi,Wui,w)./20);
    Ws = 10.^(interp1(wi,Wsi,w)./20);

    % Magnitude Forbidden Regions
    mmC = 10.^(linspace(min(mLim),max(mLim),Nm)./20); % Create linearly spaced magnitude test points [dB] (1 x Nm)
    maC = interp1(wi,aCi,w); % Interpolate controller phase angle at test frequencies (Nw x 1)

    % Phase Forbidden Regions
    amC = 10.^(interp1(wi,mCi,w)./20);  % Interpolate controller magnitude at test frequencies (Nw x 1)
    aaC = linspace(min(aLim),max(aLim),Na); % Create linearly spaced phase test points [deg] (1 x Na)

    for i=1:Np % Repeat for all input plants (take maximum performance quantity)

        % Calculate performance quantity (Q) at all magnitude test points and frequencies and store as matrix (Nw x Nm) 
        mQ(:,:,i) = 10*log10(((Wu.*mP(:,i)*mmC).^2 + 2*Wu.*Ws.*mP(:,i)*mmC + Ws.^2*ones(1,Nm))./...
            (ones(Nw,Nm) + 2*mP(:,i).*cosd(aP(:,i)+maC)*mmC + (mP(:,i)*mmC).^2));

        % Calculate performance quantity (Q) at all magnitude test points and frequencies and store as matrix (Nw x Na) 
        aQ(:,:,i) = 10*log10(((Wu.*mP(:,i).*amC).^2 + 2*Wu.*Ws.*mP(:,i).*amC + Ws.^2)*ones(1,Na)./...
            (ones(Nw,Na) + 2*mP(:,i).*amC*ones(1,Na).*cosd(aP(:,i)*ones(1,Na)+ones(Nw,1)*aaC) + (mP(:,i).*amC).^2*ones(1,Na)));

    end

    % Calculate maximum performance quantity over all plants
    mQ = max(mQ,[],3);
    aQ = max(aQ,[],3);

    colormap(flipud(bone))

    % Controller Bode Magnitude Plot
    mAxis = subplot(2,1,1);
    % box on
    hold on

    [X,Y] = meshgrid(w,20.*log10(mmC));
    contourf(X',Y',mQ,n,'LineStyle','none');

    % Plot Highlighted Contours
    nC = length(contourValues);

    for i=1:nC

        C = contourc(w,20.*log10(mmC),mQ',[contourValues(i) contourValues(i)]);

        % Plot sections of the contour independently (prevent connecting lines)
        [row,col] = find(C == contourValues(i));
        col(end+1) = size(C,2)+1;

        for j=1:length(col)-1

          h = line(C(1,col(j)+1:col(j+1)-1),C(2,col(j)+1:col(j+1)-1));
          set(h,'Color',contourStyles{i,1},'LineStyle',contourStyles{i,2},'LineWidth',contourStyles{i,3})

        end

    end

    line(w,20*log10(amC),'Color',controlStyle{1},'LineStyle',controlStyle{2},'LineWidth',controlStyle{3}) % Magnitude [dB] vs Frequency [Hz]

    set(mAxis,'XScale','log')
    ylabel('Magnitude [dB]');
    h = colorbar;
    set(get(h,'ylabel'),'String','\Gamma [dB]')

    % Controller Bode Phase Plot
    aAxis = subplot(2,1,2);
    % box on
    hold on

    [X,Y] = meshgrid(w,aaC);
    contourf(X',Y',aQ,n,'LineStyle','none');

    for i=1:nC

        C = contourc(w,aaC,aQ',[contourValues(i) contourValues(i)]);

        % Plot sections of the contour independently (prevent connecting lines)
        [row,col] = find(C == contourValues(i));
        col(end+1) = size(C,2)+1;

        for j=1:length(col)-1

          h = line(C(1,col(j)+1:col(j+1)-1),C(2,col(j)+1:col(j+1)-1));
          set(h,'Color',contourStyles{i,1},'LineStyle',contourStyles{i,2},'LineWidth',contourStyles{i,3})

        end

    end

    % Plot controller frequency response
    line(w,maC,'Color',controlStyle{1},'LineStyle',controlStyle{2},'LineWidth',controlStyle{3}) % Phase angle [deg] vs Frequency [Hz]

    set(aAxis,'XScale','log')
    xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
    h = colorbar;
    set(get(h,'ylabel'),'String','\Gamma [dB]')

    set(mAxis,'CLim',cLim)
    set(aAxis,'CLim',cLim)

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Calcuate performance quantity for controller freq resp 
    for i=1:Np
        mQC(:,i) = 10*log10(((Wu.*mP(:,i).*amC).^2 + 2*Wu.*Ws.*mP(:,i).*amC + Ws.^2)./...
            (ones(Nw,1) + 2*mP(:,i).*cosd(aP(:,i)+maC).*amC + (mP(:,i).*amC).^2));    
    end
    
    mQC = max(mQC,[],2);
    mQC = mQC';
    l = find(diff(mQC>0) == 1) + 1;
    h = find(diff(mQC>0) == -1);
    
    if length(l)<length(h) l = [1 l]; end
    if length(l)>length(h) h = [h length(mQC)]; end
    
%     l = [1 l];
%     h = [h length(mQC)];
    
    for i=1:length(l)
             
        % Plot intersections of controller frequency response with forbidden regions (red)
        axes(mAxis)
        line(w(l(i):h(i)),20*log10(amC(l(i):h(i))),'Color','r','LineStyle','-','LineWidth',controlStyle{3}); % Magnitude [dB] vs Frequency [deg]

        % Plot intersections of controller frequency response with forbidden regions (red)
        axes(aAxis)
        line(w(l(i):h(i)),maC(l(i):h(i)),'Color','r','LineStyle','-','LineWidth',controlStyle{3}); % Phase angle [deg] vs Frequency [Hz]

    end

end













Contact us