function [J,F]=ps43(w0,L)
% function [J,F]=ps43(w0,L)
%
% w0 -  resonance frequency
% L  -  observer gains
% J  -  minimal cost
% F  -  optimal filter

if nargin<1, w0=1; end          % default w0
if nargin<2, L=[w0+1;1]; end    % default observer gain

s=tf('s');                      % useful constant
F0=(s*L(1)+L(2))/(s^2+w0^2+s*L(1)+L(2)); % some filter
W=1/(s^2+w0^2);
L=(s^2+w0^2)/(s+w0+1)^2;

assignin('base','F0',F0)        % export variables
assignin('base','W',W)
assignin('base','L',L)

p=linmod('ps43des');            % extract open loop
p=minreal(ss(p.a,p.b,p.c,p.d)); % remove uncontrollable modes
K=h2syn(p,1,1);                 % optimize 
F=minreal(F0+K*L);              % complete filter

assignin('base','F',F)          % export variables
p=linmod('ps43test');           % extract error system
G=minreal(ss(p.a,p.b,p.c,p.d)); % remove uncontrollable modes
J=norm(G)^2;
if nargin==0,
    A=ssdata(G);
    r=max(real(eig(A)));
    fprintf('w0=%f:  J=%f,  check %f<0\n',w0,J,r)
    ww=linspace(0,3*w0,500);
    f=abs(squeeze(freqresp(F,1i*ww)));
    g=abs(squeeze(freqresp(G,1i*ww)));
    close(gcf)
    subplot(2,1,1);plot(ww,f);grid
    xlabel('w (rad/sec)')
    ylabel('|F(jw)| (rad/sec)')
    subplot(2,1,2);plot(ww,g);grid
    xlabel('w (rad/sec)')
    ylabel('|G(jw)| (rad/sec)')
end