% 
% Written by:
% -- 
% John L. Weatherwax                2007-07-01
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

close all; clc; clear; 

q=10; 
r=1; 


% 
% The optimal filter design: 
% 
betas = linspace( 0.1, 1, 100 );

beta_tilde = -betas .* sqrt( 1 + ( q / r ) * ( ( 1./betas ).^2 ) ); 
p_infty    = -r * betas .* ( 1 + beta_tilde ./ betas ); 

fh_all = figure(); hold on; 
ph_optimal = plot( betas, p_infty, '-g', 'LineWidth', 3 ); 
xlabel('true system beta'); ylabel('estimated error covariance p\_infinity'); 
axis( [ 0.1, 1.0, 2.0, 4.0 ] ); 


%
% the beta=0.5 filter (assuming we "know" q/r)
% 
beta_think = 0.5;
k_think    = -beta_think + sqrt( beta_think^2 + q/r ); 

p_beta_one_half = p_inf_fn_of_true_beta(beta_think,k_think, betas,q,r); 

figure(fh_all);
ph_betaOH = plot( betas, p_beta_one_half, '--k', 'LineWidth', 2 ); 

legend( [ph_optimal,ph_betaOH], {'optimal filter', 'beta 0.5'});
drawnow;
saveas( gcf, '../../WriteUp/Graphics/Chapter7/example_7_1_6_plots.eps', 'psc2' );


%
% The S_1 filter for this problem: 
%
n_beta_f_samples = 50; % number of samples to use in griding the domain of \beta_f
beta_f_domain    = linspace( 0.1, 2.0, n_beta_f_samples );

n_k_samples      = 50; % number of samples to use in griding the domain of k 

n_q_samples      = 50; % number of samples to use in griding the domain of q
q_domain         = linspace( 1, 10, n_q_samples ); 

n_r_samples      = 50; % number of samples to use in griding the domain of r
r_domain         = linspace( 0, 2, n_r_samples );

n_beta_samples   = 50; % number of samples to use in griding the domain of \beta
beta_domain      = linspace( 0.1, 1.0, n_beta_samples );

% form tensors with three indices/dimensions corresponding to (\beta,q,r) of dimension
%     [ n_beta_samples, n_q_samples, n_r_samples ]: 
% 
beta_array = repmat( beta_domain(:), [1, n_q_samples, n_r_samples] );
q_array    = repmat( q_domain, [n_beta_samples, 1, n_r_samples] );
r_array    = repmat( reshape( r_domain(:), [1,1,n_r_samples] ), [n_beta_samples, n_q_samples, 1] ); % create a [1,1,n_r_samples] array and then tile this by [n_beta_samples,n_q_samples] to fill out the other dimensions

% now do the minimization/maximization: 
%
min_value = +Inf; best_beta_f = 0.; best_k = 0.; % storage for the min/max filter result

for bfi=1:length(beta_f_domain) 
  bf = beta_f_domain(bfi); 
  
  k_domain = linspace( -bf+0.0001, 4, n_k_samples ); % note that stability requires that k > -beta_f 
  %k_domain(end) = k_optimal(bf,q,r); % force us to look at the optimal kalman gain k 
						     
  for ki=1:n_k_samples
    k  = k_domain(ki); 
    
    % combine these tensors into the function we want to maximize: 
    % 
    num1  = k^2 * r_array + q_array; 
    den1  = 2 * ( bf + k );
    num2  = ( bf^2 - beta_array.^2 ) .* q_array; 
    den2  = 2 * beta_array * ( bf + k ) .* ( beta_array + bf + k );
    p_inf = ( num1 ./ den1 ) + ( num2 ./ den2 ); 
    
    [m,ind] = max(p_inf(:)); % maximize it 
    
    % make sure that our maximum is where we expect (beta_min,q_max,r_max))
    [ii,jj,kk] = ind2sub( [n_beta_samples, n_q_samples, n_r_samples], ind );
    if( ~( ii==1 && jj==n_q_samples && kk==n_r_samples ) ) 
      fprintf('wrong indices found!\n'); 
    end
    
    if( m < min_value )
      min_value = m; best_beta_f = bf; best_k = k; 
    end
    
  end 
end
fprintf('Design criterion S1 has beta_f= %10.6f, k= %10.6f with S1= %10.6f\n', best_beta_f, best_k, min_value );

figure(fh_all); 
pInf_S1 = p_inf_fn_of_true_beta(best_beta_f,best_k, betas,q,r); 
ph_S1 = plot( betas, pInf_S1, '--b' ); 

legend( [ph_optimal,ph_betaOH,ph_S1], {'optimal filter', 'beta 0.5','S1 filter (grid based)'});
drawnow;
saveas( gcf, '../../WriteUp/Graphics/Chapter7/example_7_1_6_plots.eps', 'psc2' );

% If we had optimally filtered assuming beta = best_beta_f then our optimal k would be 
best_k = k_optimal(best_beta_f, q, r); 
figure(fh_all); 
pInf_S1_alt = p_inf_fn_of_true_beta(best_beta_f,best_k, betas,q,r); 
ph_S1_alt = plot( betas, pInf_S1_alt, '.-b' ); 

legend( [ph_optimal,ph_betaOH,ph_S1,ph_S1_alt], {'optimal filter', 'beta 0.5','S1 filter (grid based)','S1 filter (analytic)'});
drawnow;
saveas( gcf, '../../WriteUp/Graphics/Chapter7/example_7_1_6_plots.eps', 'psc2' );

beta_min = beta_domain(1);
r_max    = r_domain(n_r_samples); 
q_max    = q_domain(n_q_samples); 

S1_exact = sqrt( beta_min^2 * r_max^2 + r_max * q_max ) - beta_min * r_max; 

fprintf('S1 calculated= %10.6f; S1 exact= %10.6f\n', min_value, S1_exact ); 


%
% The S_2 filter for this problem: 
%
n_beta_f_samples = 50; % number of samples to use in griding the domain of \beta_f
beta_f_domain    = linspace( 0.1, 2.0, n_beta_f_samples );

n_k_samples      = 50; % number of samples to use in griding the domain of k 

n_q_samples      = 50; % number of samples to use in griding the domain of q
q_domain         = linspace( 1, 10, n_q_samples ); 

n_r_samples      = 50; % number of samples to use in griding the domain of r
r_domain         = linspace( 0, 2, n_r_samples );

n_beta_samples   = 50; % number of samples to use in griding the domain of \beta
beta_domain      = linspace( 0.1, 1.0, n_beta_samples );

% form tensors with three indices/dimensions corresponding to (\beta,q,r) of dimension
%     [ n_beta_samples, n_q_samples, n_r_samples ]: 
% 
beta_array = repmat( beta_domain(:), [1, n_q_samples, n_r_samples] );
q_array    = repmat( q_domain, [n_beta_samples, 1, n_r_samples] );
r_array    = repmat( reshape( r_domain(:), [1,1,n_r_samples] ), [n_beta_samples, n_q_samples, 1] ); % create a [1,1,n_r_samples] array and then tile this by [n_beta_samples,n_q_samples] to fill out the other dimensions

% now do the minimization/maximization: 
%
min_value = +Inf; best_beta_f = 0.; best_k = 0.; % storage for the min/max filter result

for bfi=1:length(beta_f_domain) 
  bf = beta_f_domain(bfi); 
  
  k_domain = linspace( -bf+0.0001, 4, n_k_samples ); % note that stability requires that k > -beta_f 
  %k_domain(end) = k_optimal(bf,q,r); % force us to look at the optimal kalman gain k 
  
  for ki=1:n_k_samples
    k  = k_domain(ki); 
    
    % combine these tensors into the function we want to maximize: 
    % 
    num1  = k^2 * r_array + q_array; 
    den1  = 2 * ( bf + k );
    num2  = ( bf^2 - beta_array.^2 ) .* q_array; 
    den2  = 2 * beta_array * ( bf + k ) .* ( beta_array + bf + k );
    J_0   = -beta_array .* r_array + sqrt( ((beta_array.^2) .* (r_array.^2)) + q_array .* r_array ); 
    p_inf = ( num1 ./ den1 ) + ( num2 ./ den2 ) - J_0; 
    
    [m,ind] = max(p_inf(:)); % maximize it 
    
    if( m < min_value )
      min_value = m; best_beta_f = bf; best_k = k; 
    end
    
  end 
end
fprintf('Design criterion S2 has beta_f= %10.6f, k= %10.6f with S2= %10.6f\n', best_beta_f, best_k, min_value );

figure(fh_all); 
pInf_S2 = p_inf_fn_of_true_beta(best_beta_f,best_k, betas,q,r); 
ph_S2 = plot( betas, pInf_S2, '-.c', 'LineWidth', 2 ); 

legend( [ph_optimal,ph_betaOH,ph_S1,ph_S1_alt,ph_S2], {'optimal filter', 'beta 0.5','S1 filter (grid based)','S1 filter (analytic)','S2 filter'});
drawnow;
saveas( gcf, '../../WriteUp/Graphics/Chapter7/example_7_1_6_plots.eps', 'psc2' );



%
% The S_3 filter for this problem: 
%
n_beta_f_samples = 50; % number of samples to use in griding the domain of \beta_f
beta_f_domain    = linspace( 0.1, 2.0, n_beta_f_samples );

n_k_samples      = 50; % number of samples to use in griding the domain of k 

n_q_samples      = 50; % number of samples to use in griding the domain of q
q_domain         = linspace( 1, 10, n_q_samples ); 

n_r_samples      = 50; % number of samples to use in griding the domain of r
r_domain         = linspace( 0, 2, n_r_samples );

n_beta_samples   = 50; % number of samples to use in griding the domain of \beta
beta_domain      = linspace( 0.1, 1.0, n_beta_samples );

% form tensors with three indices/dimensions corresponding to (\beta,q,r) of dimension
%     [ n_beta_samples, n_q_samples, n_r_samples ]: 
% 
beta_array = repmat( beta_domain(:), [1, n_q_samples, n_r_samples] );
q_array    = repmat( q_domain, [n_beta_samples, 1, n_r_samples] );
r_array    = repmat( reshape( r_domain(:), [1,1,n_r_samples] ), [n_beta_samples, n_q_samples, 1] ); % create a [1,1,n_r_samples] array and then tile this by [n_beta_samples,n_q_samples] to fill out the other dimensions

% now do the minimization/maximization: 
%
min_value = +Inf; best_beta_f = 0.; best_k = 0.; % storage for the min/max filter result

for bfi=1:length(beta_f_domain) 
  bf = beta_f_domain(bfi); 
  
  k_domain = linspace( -bf+0.0001, 4, n_k_samples ); % note that stability requires that k > -beta_f 
  %k_domain(end) = k_optimal(bf,q,r); % force us to look at the optimal kalman gain k 
  
  for ki=1:n_k_samples
    k  = k_domain(ki); 
    
    % combine these tensors into the function we want to maximize: 
    % 
    num1  = k^2 * r_array + q_array; 
    den1  = 2 * ( bf + k );
    num2  = ( bf^2 - beta_array.^2 ) .* q_array; 
    den2  = 2 * beta_array * ( bf + k ) .* ( beta_array + bf + k );
    J_0   = -beta_array .* r_array + sqrt( ((beta_array.^2) .* (r_array.^2)) + q_array .* r_array ); 
    indsZ  = find(J_0(:)==0);
    indsNZ = find(J_0(:)~=0);
    J_0(indsZ)=min(abs(J_0(indsNZ)));
    p_inf = ( ( num1 ./ den1 ) + ( num2 ./ den2 ) - J_0 ) ./ J_0; 
    
    [m,ind] = max(p_inf(:)); % maximize it 
    
    if( m < min_value )
      min_value = m; best_beta_f = bf; best_k = k; 
    end
    
  end 
end
fprintf('Design criterion S3 has beta_f= %10.6f, k= %10.6f with S3= %10.6f\n', best_beta_f, best_k, min_value );

figure(fh_all); 
pInf_S3 = p_inf_fn_of_true_beta(best_beta_f,best_k, betas,q,r); 
ph_S3 = plot( betas, pInf_S3, '-.m', 'LineWidth', 2 ); 

legend( [ph_optimal,ph_S1,ph_S1_alt,ph_S2,ph_S3,ph_betaOH], {'optimal filter','S1 filter (grid based)','S1 filter (analytic)','S2 filter','S3 filter','beta 0.5'});
drawnow;
saveas( gcf, '../../WriteUp/Graphics/Chapter7/example_7_1_6_plots.eps', 'psc2' );