% % 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' );