| 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
|
|
