├── README.md ├── display_stats.m ├── example_parameters └── example_parameters_1.m ├── filter_parameters └── filter_parameters_1.m ├── filters ├── BDU.m ├── Filter.m ├── GCF.m ├── KF.m ├── LMI_RKF.m ├── ORF.m ├── RKF.m └── RSKF.m ├── generate_system_data.m ├── main.m ├── measure_target.m ├── plot_results.m ├── saved_system_data └── system_data_1.mat ├── sim_parameters.m └── update_target_system.m /README.md: -------------------------------------------------------------------------------- 1 | # SCL-RKF-code 2 | 3 | This repository contains the code used to obtain the results presented in the paper: 4 | > K. D. T Rocha and M. H. Terra, "Robust Kalman filter for systems subject to parametric uncertainties," Systems & Control Letters, vol. 157, Nov. 2021. [Link to paper](https://doi.org/10.1016/j.sysconle.2021.105034). 5 | -------------------------------------------------------------------------------- /display_stats.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Display Simulation Statistics %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Displays a series of estimation statistics for all filters. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | % ======================================================================= % 11 | % Compute simulation statistics 12 | % ======================================================================= % 13 | 14 | % Pre-allocating lists 15 | f_ids = cell(n_filters,1); % Filter identifications 16 | f_means = zeros(n_filters,1); % Mean of error variances 17 | f_stds = zeros(n_filters,1); % Standard deviation of error variances 18 | f_times = zeros(n_filters,1); % Average iteration execution times 19 | 20 | for f = 1:n_filters 21 | f_ids{f} = filters{f}.id; 22 | f_means(f) = filters{f}.mean_S2; 23 | f_stds(f) = filters{f}.std_S2; 24 | f_times(f) = filters{f}.T_it * 1e3; 25 | end 26 | 27 | % ======================================================================= % 28 | % Display results 29 | % ======================================================================= % 30 | 31 | fprintf("########## Simulation Statistics ##########\n\n"); 32 | 33 | VarNames = {'Filter','Mean_dB','Std_Dev_dB','T_iter_ms'}; 34 | 35 | sim_stats = table(f_ids,f_means,f_stds,f_times,'VariableNames',VarNames); 36 | 37 | disp(sim_stats); 38 | 39 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 40 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 41 | -------------------------------------------------------------------------------- /example_parameters/example_parameters_1.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Example Parameters 1 %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Adapted from: % 6 | % L. Xie, Y.C. Soh, C. de Souza. Robust Kalman Filtering for Uncertain % 7 | % Discrete-Time Systems. IEEE Trans. Automat. Control. 39 (6) (1994). % 8 | % % 9 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | %%% System and Sensing Model Data %%% 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | % ======================================================================= % 16 | % Target system 17 | % ======================================================================= % 18 | 19 | % Model: 20 | 21 | % x(k+1) = (F+dF)(k)x(k) + (G+dG)(k)u(k) + (H+dH)(k)w(k) 22 | 23 | % [dF(k) dG(k) dH(k)] = M1(k) D1(k) [EF(k) EG(k) EH(k)]; ||D1(k)|| <= 1 24 | 25 | % w(k) ~ N(0,Qk) - System noise 26 | 27 | % ----------------------------------------------------------------------- % 28 | % Initial state 29 | % ----------------------------------------------------------------------- % 30 | 31 | x0 = [2; 32 | 1]; 33 | 34 | % ----------------------------------------------------------------------- % 35 | % Input vector 36 | % ----------------------------------------------------------------------- % 37 | 38 | u_k = 0; 39 | 40 | % ----------------------------------------------------------------------- % 41 | % Nominal parameter matrices 42 | % ----------------------------------------------------------------------- % 43 | 44 | F = [0 -0.5; 45 | 1 1 ]; 46 | 47 | G = [0; 48 | 0]; 49 | 50 | H = [-6; 51 | 1]; 52 | 53 | Q = 1; 54 | 55 | % ----------------------------------------------------------------------- % 56 | % Uncertain parameter matrices 57 | % ----------------------------------------------------------------------- % 58 | 59 | M1 = [ 0; 60 | 10]; 61 | 62 | EF = [0.01 0.03]; 63 | 64 | EG = 0; 65 | 66 | EH = 0.01; 67 | 68 | % ======================================================================= % 69 | % Sensing model 70 | % ======================================================================= % 71 | 72 | % Model: 73 | 74 | % y(k) = (C+dC)(k)x(k) + (D+dD)(k)v(k) 75 | 76 | % [dC(k) dD(k)] = M2(k) D2(k) [EC(k) ED(k)]; ||D2(k)|| <= 1 77 | 78 | % v(k) ~ N(0,Rk) - Sensor noise 79 | 80 | % ----------------------------------------------------------------------- % 81 | % Nominal parameter matrices 82 | % ----------------------------------------------------------------------- % 83 | 84 | C = [-100 10]; 85 | 86 | D = 1; 87 | 88 | R = 1; 89 | 90 | % ----------------------------------------------------------------------- % 91 | % Uncertain parameter matrices 92 | % ----------------------------------------------------------------------- % 93 | 94 | M2 = 10; 95 | 96 | EC = EF; 97 | 98 | ED = EH; 99 | 100 | % ======================================================================= % 101 | % System model structure 102 | % ======================================================================= % 103 | 104 | sys_model = struct(); 105 | 106 | sys_model.F = F; 107 | sys_model.G = G; 108 | sys_model.H = H; 109 | sys_model.Q = Q; 110 | 111 | sys_model.C = C; 112 | sys_model.D = D; 113 | sys_model.R = R; 114 | 115 | sys_model.M1 = M1; 116 | sys_model.EF = EF; 117 | sys_model.EG = EG; 118 | sys_model.EH = EH; 119 | 120 | sys_model.M2 = M2; 121 | sys_model.EC = EC; 122 | sys_model.ED = ED; 123 | 124 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 126 | -------------------------------------------------------------------------------- /filter_parameters/filter_parameters_1.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Filter Parameters - Example 1 %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Adapted from: % 6 | % L. Xie, Y.C. Soh, C. de Souza. Robust Kalman Filtering for Uncertain % 7 | % Discrete-Time Systems. IEEE Trans. Automat. Control. 39 (6) (1994). % 8 | % % 9 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | % ======================================================================= % 12 | % Common parameters 13 | % ======================================================================= % 14 | 15 | % Initial state estimate 16 | xp0 = zeros(n,1); 17 | 18 | % Initial estimation error weighting matrix 19 | P0 = 1 * eye(n); 20 | 21 | % ======================================================================= % 22 | % Kalman Filter (KF) 23 | % ======================================================================= % 24 | 25 | % ----------------------------------------------------------------------- % 26 | % Initialization parameters 27 | % ----------------------------------------------------------------------- % 28 | 29 | KF_init_params = struct(); 30 | 31 | % Initial state estimate 32 | KF_init_params.xp0 = xp0; 33 | 34 | % Initial estimation error weighting matrix 35 | KF_init_params.P0 = P0; 36 | 37 | % ----------------------------------------------------------------------- % 38 | % Filter parameters 39 | % ----------------------------------------------------------------------- % 40 | 41 | KF_params = struct(); 42 | 43 | % ======================================================================= % 44 | % Robust Kalman Filter (RKF) 45 | % ======================================================================= % 46 | 47 | % ----------------------------------------------------------------------- % 48 | % Initialization parameters 49 | % ----------------------------------------------------------------------- % 50 | 51 | RKF_init_params = struct(); 52 | 53 | % Initial state estimate 54 | RKF_init_params.xp0 = xp0; 55 | 56 | % Initial estimation error weighting matrix 57 | RKF_init_params.P0 = P0; 58 | 59 | % ----------------------------------------------------------------------- % 60 | % Filter parameters 61 | % ----------------------------------------------------------------------- % 62 | 63 | RKF_params = struct(); 64 | 65 | % Penalty parameter 66 | RKF_params.mu = 1; 67 | 68 | % ksi parameter (lambda approximation) 69 | RKF_params.ksi = 0.1; 70 | 71 | % ======================================================================= % 72 | % Optimal Robust Filter (ORF) 73 | % ======================================================================= % 74 | 75 | % ----------------------------------------------------------------------- % 76 | % Initialization parameters 77 | % ----------------------------------------------------------------------- % 78 | 79 | ORF_init_params = struct(); 80 | 81 | % Initial estimation error weighting matrix 82 | ORF_init_params.P0 = P0; 83 | 84 | % ----------------------------------------------------------------------- % 85 | % Filter parameters 86 | % ----------------------------------------------------------------------- % 87 | 88 | ORF_params = struct(); 89 | 90 | % Penalty parameter 91 | ORF_params.mu = 1e13; 92 | 93 | % ksi parameter (lambda approximation) 94 | ORF_params.ksi = 0.1; 95 | 96 | % ======================================================================= % 97 | % Bounded Data Uncertainty Filter (BDU) 98 | % ======================================================================= % 99 | 100 | % ----------------------------------------------------------------------- % 101 | % Initialization parameters 102 | % ----------------------------------------------------------------------- % 103 | 104 | BDU_init_params = struct(); 105 | 106 | % Initial state estimate 107 | BDU_init_params.xp0 = xp0; 108 | 109 | % Initial estimation error weighting matrix 110 | BDU_init_params.P0 = P0; 111 | 112 | % ----------------------------------------------------------------------- % 113 | % Filter parameters 114 | % ----------------------------------------------------------------------- % 115 | 116 | BDU_params = struct(); 117 | 118 | % ksi parameter (lambda approximation) 119 | BDU_params.ksi = 0.5; 120 | 121 | % ======================================================================= % 122 | % Guaranteed Cost Filter (GCF) 123 | % ======================================================================= % 124 | 125 | % ----------------------------------------------------------------------- % 126 | % Initialization parameters 127 | % ----------------------------------------------------------------------- % 128 | 129 | GCF_init_params = struct(); 130 | 131 | % Initial state estimate 132 | GCF_init_params.xp0 = xp0; 133 | 134 | % Initial estimation error weighting matrix 135 | GCF_init_params.P0 = P0; 136 | 137 | % ----------------------------------------------------------------------- % 138 | % Filter parameters 139 | % ----------------------------------------------------------------------- % 140 | 141 | GCF_params = struct(); 142 | 143 | % epsilon parameters (approximations) 144 | GCF_params.eps_a = 0.01; 145 | GCF_params.eps_b = 0.01; 146 | 147 | % ======================================================================= % 148 | % LMI-Based Robust Kalman Filter (LMI_RKF) 149 | % ======================================================================= % 150 | 151 | % ----------------------------------------------------------------------- % 152 | % Initialization parameters 153 | % ----------------------------------------------------------------------- % 154 | 155 | LMI_RKF_init_params = struct(); 156 | 157 | % Initial state estimate 158 | LMI_RKF_init_params.xp0 = xp0; 159 | 160 | % Initial estimation error weighting matrix 161 | LMI_RKF_init_params.P0 = P0; 162 | 163 | % ----------------------------------------------------------------------- % 164 | % Filter parameters 165 | % ----------------------------------------------------------------------- % 166 | 167 | LMI_RKF_params = struct(); 168 | 169 | % Penalty parameter 170 | LMI_RKF_params.mu = 0.1; 171 | 172 | % ni parameter (approximations) 173 | LMI_RKF_params.ni = 1; 174 | 175 | % ======================================================================= % 176 | % Risk-Sensitive Kalman Filter (RSKF) 177 | % ======================================================================= % 178 | 179 | % ----------------------------------------------------------------------- % 180 | % Initialization parameters 181 | % ----------------------------------------------------------------------- % 182 | 183 | RSKF_init_params = struct(); 184 | 185 | % Initial state estimate 186 | RSKF_init_params.xp0 = xp0; 187 | 188 | % Initial least-favorable conditioned variance 189 | RSKF_init_params.V0 = 8 * eye(n); 190 | 191 | % ----------------------------------------------------------------------- % 192 | % Filter parameters 193 | % ----------------------------------------------------------------------- % 194 | 195 | RSKF_params = struct(); 196 | 197 | % tau parameter 198 | RSKF_params.tau = 0.1; 199 | 200 | % Tolerance parameter 201 | RSKF_params.c = 2.9; 202 | 203 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 204 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 205 | -------------------------------------------------------------------------------- /filters/BDU.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Bounded Data Uncertainty Filter (BDU) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the Robust Bounded Data Uncertainty Filter % 7 | % (BDU). % 8 | % - A.H. Sayed. A Framework for State-Space Estimation with Uncertain % 9 | % Models. IEEE Trans. Automat. Control. 46 (7) (2001). % 10 | % % 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | classdef BDU < Filter 16 | 17 | % =================================================================== % 18 | 19 | properties 20 | Pp; % Predicted estimation error weighting matrix 21 | end 22 | 23 | % =================================================================== % 24 | 25 | % =================================================================== % 26 | 27 | methods 28 | 29 | % --------------------------------------------------------------- % 30 | % Constructor 31 | % --------------------------------------------------------------- % 32 | 33 | function f = BDU(N,T,n) 34 | % Base class constructor 35 | f@Filter(N,T,n); 36 | 37 | % Filter identification 38 | f.id = "BDU (Sayed, 2001)"; 39 | end 40 | 41 | % --------------------------------------------------------------- % 42 | % Initialization (at each new experiment) 43 | % --------------------------------------------------------------- % 44 | 45 | function initialize(f,init_params) 46 | % Initialize predicted estimation 47 | f.xp = init_params.xp0; 48 | 49 | % Initialize predicted estimation error weighting matrix 50 | f.Pp = init_params.P0; 51 | end 52 | 53 | % --------------------------------------------------------------- % 54 | % Update State Estimate 55 | % --------------------------------------------------------------- % 56 | 57 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 58 | % Start timer 59 | t_start = tic; 60 | 61 | % Unpack system model matrices 62 | F = sys_model.F; 63 | H = sys_model.H; 64 | Q = sys_model.Q; 65 | C = sys_model.C; 66 | D = sys_model.D; 67 | R = sys_model.R; 68 | M1 = sys_model.M1; 69 | EF = sys_model.EF; 70 | EH = sys_model.EH; 71 | 72 | % Unpack filter parameters 73 | ksi = f_params.ksi; 74 | 75 | % Dimensions 76 | n = size(F,1); 77 | t1 = size(EF,1); 78 | 79 | % Lambda approximation 80 | lamb = (1+ksi) * norm(M1'*(C'/R*C)*M1); 81 | 82 | % Modified sensing model matrices 83 | Rhat = R - 1/lamb * C*(M1*M1')*C'; 84 | Re = Rhat + C*f.Pp*C'; 85 | 86 | % Filtered estimation error matrix 87 | Pf = f.Pp - f.Pp*C'/Re*C*f.Pp; 88 | 89 | % Modified system model matrices 90 | 91 | Qhat = inv(Q) + ... 92 | lamb * EH'/(eye(t1) + lamb*EF*Pf*EF')*EH; 93 | 94 | Phat = Pf - ... 95 | Pf*EF'/(1/lamb*eye(t1) + EF*Pf*EF')*EF*Pf; 96 | 97 | Hhat = H - lamb*F*Phat*EF'*EH; 98 | 99 | Fhat = (F - lamb*Hhat/Qhat*EH'*EF) * ... 100 | (eye(n) - lamb*Phat*(EF'*EF)); 101 | 102 | % New filtered state estimate 103 | xf_new = f.xp + Pf*C'/Rhat*(y_k - C*f.xp); 104 | f.xf(:,k,e) = xf_new; 105 | 106 | % New predicted estimation error matrix 107 | f.Pp = F*Phat*F' + Hhat/Qhat*Hhat'; 108 | 109 | % New predicted state estimate 110 | f.xp = Fhat*xf_new; 111 | 112 | % Stop timer and update total time 113 | t_iter = toc(t_start); 114 | f.T_ex = f.T_ex + t_iter; 115 | end 116 | 117 | % --------------------------------------------------------------- % 118 | 119 | end 120 | 121 | % =================================================================== % 122 | 123 | end 124 | 125 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 126 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 127 | -------------------------------------------------------------------------------- /filters/Filter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Filter Base Class %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Base class with properties and methods common to most filters. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | classdef Filter < handle 13 | 14 | % =================================================================== % 15 | 16 | properties 17 | id; % Filter identification 18 | 19 | xp; % Predicted estimation 20 | xf; % Filtered estimation 21 | 22 | S1; % Estimation error variance of each variable (ensemble average) 23 | S2; % Estimation error variance of whole state (ensemble average) 24 | 25 | mean_S2; % Mean of estimation error variance 26 | std_S2; % Standard deviation of estimation error variance 27 | 28 | T_ex; % Total execution time (one experiment) 29 | T_it; % Average iteration execution time 30 | end 31 | 32 | % =================================================================== % 33 | 34 | % =================================================================== % 35 | 36 | methods 37 | 38 | % --------------------------------------------------------------- % 39 | % Constructor 40 | % --------------------------------------------------------------- % 41 | 42 | function f = Filter(N,T,n) 43 | % Pre-allocation 44 | f.xf = zeros(n,N+1,T); 45 | f.S1 = zeros(n,N+1); 46 | f.S2 = zeros(1,N+1); 47 | 48 | % Initialize timer 49 | f.T_ex = 0; 50 | f.T_it = 0; 51 | end 52 | 53 | % --------------------------------------------------------------- % 54 | % Update Error Variance 55 | % --------------------------------------------------------------- % 56 | 57 | function update_error_variance(f,e,x_e) 58 | % Squared error of current experiment 59 | E = (x_e - f.xf(:,:,e)).^2; 60 | 61 | % Accumulate estimation error of each variable 62 | f.S1 = f.S1 + E; 63 | 64 | % Accumulate estimation error of whole state (in dB) 65 | f.S2 = f.S2 + 20*log10(sum(E)); 66 | end 67 | 68 | % --------------------------------------------------------------- % 69 | % Compute Filter Statistics 70 | % --------------------------------------------------------------- % 71 | 72 | function compute_filter_stats(f,T,N) 73 | % Estimation error variance of each variable (ensemble average) 74 | f.S1 = f.S1 / T; 75 | 76 | % Estimation error variance of whole state (ensemble average) 77 | % f.S2 = 20*log10(sum(f.S1)); 78 | % f.S2 = sum(f.S1); 79 | f.S2 = f.S2 / T; 80 | 81 | % Mean of estimation error variance 82 | f.mean_S2 = mean(f.S2); 83 | 84 | % Standard deviation of estimation error variance 85 | f.std_S2 = std(f.S2); 86 | 87 | % Average iteration execution time 88 | f.T_it = f.T_ex / (N*T); 89 | end 90 | 91 | % --------------------------------------------------------------- % 92 | 93 | end 94 | 95 | % =================================================================== % 96 | 97 | end 98 | 99 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | -------------------------------------------------------------------------------- /filters/GCF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Guaranteed Cost Filter (GCF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the Guaranteed Cost Filter (GCF). % 7 | % - Z. Dong, Z. You. Finite-Horizon Robust Kalman Filtering for % 8 | % Uncertain Discrete Time-Varying Systems with Uncertain-Covariance % 9 | % White Noises. IEEE Signal Process. Lett. 13 (8) (2006). % 10 | % % 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | classdef GCF < Filter 16 | 17 | % =================================================================== % 18 | 19 | properties 20 | Pf; % Filtered estimation error weighting matrix 21 | end 22 | 23 | % =================================================================== % 24 | 25 | % =================================================================== % 26 | 27 | methods 28 | 29 | % --------------------------------------------------------------- % 30 | % Constructor 31 | % --------------------------------------------------------------- % 32 | 33 | function f = GCF(N,T,n) 34 | % Base class constructor 35 | f@Filter(N,T,n); 36 | 37 | % Filter identification 38 | f.id = "GCF (Dong, 2006)"; 39 | end 40 | 41 | % --------------------------------------------------------------- % 42 | % Initialization (at each new experiment) 43 | % --------------------------------------------------------------- % 44 | 45 | function initialize(f,init_params) 46 | % Initialize predicted estimation 47 | f.xp = init_params.xp0; 48 | 49 | % Initialize filtered estimation error weighting matrix 50 | f.Pf = init_params.P0; 51 | end 52 | 53 | % --------------------------------------------------------------- % 54 | % Update State Estimate 55 | % --------------------------------------------------------------- % 56 | 57 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 58 | % Start timer 59 | t_start = tic; 60 | 61 | % Unpack system model matrices 62 | F = sys_model.F; 63 | H = sys_model.H; 64 | Q = sys_model.Q; 65 | C = sys_model.C; 66 | D = sys_model.D; 67 | R = sys_model.R; 68 | M1 = sys_model.M1; 69 | M2 = sys_model.M2; 70 | E1 = sys_model.EF; 71 | E2 = sys_model.EH; 72 | 73 | % Unpack filter parameters 74 | eps_a = f_params.eps_a; 75 | eps_b = f_params.eps_b; 76 | 77 | % Dimensions 78 | n = size(F,1); 79 | t1 = size(E1,1); 80 | t2 = size(E2,1); 81 | 82 | % Scaling parameters 83 | alfa = norm(E1*f.Pf*E1') + eps_a; 84 | beta = norm(E2*Q*E2') + eps_b; 85 | 86 | % Modified matrices 87 | 88 | Pbar = f.Pf + f.Pf*E1'/(alfa*eye(t1) - E1*f.Pf*E1')*E1*f.Pf; 89 | 90 | Qbar = Q + Q*E2'/(beta*eye(t2) - E2*Q*E2')*E2*Q; 91 | 92 | Rbar = R + R*E2'/(beta*eye(t2) - E2*R*E2')*E2*R; 93 | 94 | % Filter gain 95 | K = (F*Pbar*C' + alfa*(M1*M2')) / ... 96 | (C*Pbar*C' + D*Rbar*D' + (alfa + beta)*(M2*M2')); 97 | 98 | % New filtered state estimate 99 | f.xf(:,k,e) = F*f.xp + K*(y_k - C*f.xp); 100 | 101 | % New error weighting matrix 102 | f.Pf = F*f.Pf*F' + H*Qbar*H' - K*(C*Pbar*F' + alfa*(M2*M1')); 103 | 104 | % New predicted state estimate 105 | f.xp = (eye(n) + f.Pf*E1' / ... 106 | (alfa*eye(t1) - E1*f.Pf*E1')*E1)*f.xf(:,k,e); 107 | 108 | % Stop timer and update total time 109 | t_iter = toc(t_start); 110 | f.T_ex = f.T_ex + t_iter; 111 | end 112 | 113 | % --------------------------------------------------------------- % 114 | 115 | end 116 | 117 | % =================================================================== % 118 | 119 | end 120 | 121 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 122 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 123 | -------------------------------------------------------------------------------- /filters/KF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Kalman Filter (KF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the standard Kalman Filter. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | classdef KF < Filter 13 | 14 | % =================================================================== % 15 | 16 | properties 17 | Pp; % Predicted estimation error weighting matrix 18 | end 19 | 20 | % =================================================================== % 21 | 22 | % =================================================================== % 23 | 24 | methods 25 | 26 | % --------------------------------------------------------------- % 27 | % Constructor 28 | % --------------------------------------------------------------- % 29 | 30 | function f = KF(N,T,n) 31 | % Base class constructor 32 | f@Filter(N,T,n); 33 | 34 | % Filter identification 35 | f.id = "KF"; 36 | end 37 | 38 | % --------------------------------------------------------------- % 39 | % Initialization (at each new experiment) 40 | % --------------------------------------------------------------- % 41 | 42 | function initialize(f,init_params) 43 | % Initialize predicted estimation 44 | f.xp = init_params.xp0; 45 | 46 | % Initialize predicted estimation error weighting matrix 47 | f.Pp = init_params.P0; 48 | end 49 | 50 | % --------------------------------------------------------------- % 51 | % Update State Estimate 52 | % --------------------------------------------------------------- % 53 | 54 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 55 | % Start timer 56 | t_start = tic; 57 | 58 | % Unpack system model matrices 59 | F = sys_model.F; 60 | G = sys_model.G; 61 | H = sys_model.H; 62 | Q = sys_model.Q; 63 | C = sys_model.C; 64 | D = sys_model.D; 65 | R = sys_model.R; 66 | 67 | % Auxiliary matrices 68 | Qbar = H*Q*H'; 69 | Rbar = D*R*D'; 70 | 71 | % Kalman gain 72 | K = f.Pp*C'/(Rbar + C*f.Pp*C'); 73 | 74 | % New filtered state estimate 75 | xf_new = f.xp + K*(y_k - C*f.xp); 76 | f.xf(:,k,e) = xf_new; 77 | 78 | % Filtered estimation error matrix 79 | Pf = f.Pp - K*C*f.Pp; 80 | 81 | % New predicted estimation error matrix 82 | f.Pp = Qbar + F*Pf*F'; 83 | 84 | % New predicted state estimate 85 | f.xp = F*xf_new + G*u_k; 86 | 87 | % Stop timer and update total time 88 | t_iter = toc(t_start); 89 | f.T_ex = f.T_ex + t_iter; 90 | end 91 | 92 | % --------------------------------------------------------------- % 93 | 94 | end 95 | 96 | % =================================================================== % 97 | 98 | end 99 | 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 102 | -------------------------------------------------------------------------------- /filters/LMI_RKF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% LMI-Based Robust Kalman Filter (LMI-RKF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the LMI-Based Robust Kalman Filter. % 7 | % - M. Abolhasani, M. Rahmani. Robust Kalman Filtering for % 8 | % Discrete-Time Time-Varying Systems with Stochastic and Norm-Bounded % 9 | % Uncertainties, J. Dyn. Syst. Meas. Control. 140 (3) (2018). % 10 | % % 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | classdef LMI_RKF < Filter 16 | 17 | % =================================================================== % 18 | 19 | properties 20 | Pp; % Predicted estimation error weighting matrix 21 | Phi_opt; % Phi matrix (optimal) 22 | end 23 | 24 | % =================================================================== % 25 | 26 | % =================================================================== % 27 | 28 | methods 29 | 30 | % --------------------------------------------------------------- % 31 | % Constructor 32 | % --------------------------------------------------------------- % 33 | 34 | function f = LMI_RKF(N,T,n) 35 | % Base class constructor 36 | f@Filter(N,T,n); 37 | 38 | % Filter identification 39 | f.id = "LMI-RKF (Abolhasani, 2018)"; 40 | 41 | % Additional pre-allocations 42 | f.Phi_opt = zeros(n,n,N+1); 43 | end 44 | 45 | % --------------------------------------------------------------- % 46 | % Initialization (at each new experiment) 47 | % --------------------------------------------------------------- % 48 | 49 | function initialize(f,init_params) 50 | % Initialize predicted estimation 51 | f.xp = init_params.xp0; 52 | end 53 | 54 | % --------------------------------------------------------------- % 55 | % Pre-Compute LMIs 56 | % --------------------------------------------------------------- % 57 | 58 | function pre_compute_lmis(f,N,sys_model,init_params,f_params) 59 | 60 | fprintf("* Pre-computing LMI optimization for LMI-RKF...\n\n"); 61 | 62 | % Start timer 63 | t_start = tic; 64 | 65 | % Initialize predicted estimation error weighting matrix 66 | f.Pp = blkdiag(init_params.P0,init_params.P0); 67 | 68 | % Unpack system model matrices 69 | F = sys_model.F; 70 | H = sys_model.H; 71 | Q = sys_model.Q; 72 | C = sys_model.C; 73 | D = sys_model.D; 74 | R = sys_model.R; 75 | M1 = sys_model.M1; 76 | EF = sys_model.EF; 77 | EH = sys_model.EH; 78 | M2 = sys_model.M2; 79 | EC = sys_model.EC; 80 | ED = sys_model.ED; 81 | 82 | % Unpack filter parameters 83 | mu = f_params.mu; 84 | ni = f_params.ni; 85 | 86 | % Dimensions 87 | n = size(F,1); 88 | p = size(H,2); 89 | r = size(C,1); 90 | q = size(D,2); 91 | t1 = size(EF,1); 92 | 93 | % Time loop 94 | for k = 1:N+1 95 | % Lambda approximation 96 | lamb = (1+ni) * mu * norm(M2'*M2); 97 | 98 | % Auxiliary matrices 99 | T = 1/mu * eye(r) - 1/lamb * (M2*M2'); 100 | S = (C'/T*D + lamb*EC'*ED) / ... 101 | (inv(R) + D'/T*D + lamb*(ED'*ED)); 102 | 103 | % Augmented matrices 104 | M1til = [M1; M1]; 105 | EFtil = [EF zeros(t1,n)]; 106 | EHtil = [EH zeros(t1,q)]; 107 | Theta = blkdiag(Q,R); 108 | 109 | % Scaling parameters 110 | alfa = (1+ni) * norm(EFtil*f.Pp*EFtil'); 111 | beta = (1+ni) * norm(EHtil*Theta*EHtil'); 112 | 113 | % Modified matrices 114 | Pptil = inv(f.Pp) - 1/alfa * (EFtil'*EFtil); 115 | Thtil = inv(Theta) - 1/beta * (EHtil'*EHtil); 116 | 117 | % Initialize YALMIP 118 | yalmip('clear'); 119 | 120 | % LMI variables 121 | Phi = sdpvar(n); 122 | Pptil_1 = sdpvar(2*n); 123 | 124 | % Auxiliary matrices 125 | L = F*Phi*(C' - S*D')/T; 126 | Fhat = F - lamb*F*Phi*(EC' - S*ED')*EC; 127 | 128 | % Augmented matrices 129 | Ftil = [ F zeros(n) ; 130 | F - Fhat Fhat - L*C]; 131 | Htil = [H zeros(n,q); 132 | H -L*D ]; 133 | 134 | % Construct LMI 135 | L11 = Pptil_1 - (alfa + beta)*(M1til*M1til'); 136 | L21 = Pptil\Ftil'; 137 | L22 = inv(Pptil); 138 | L31 = Thtil\Htil'; 139 | L32 = zeros(p+q,2*n); 140 | L33 = inv(Thtil); 141 | 142 | LMI = [L11 L21' L31'; 143 | L21 L22 L32'; 144 | L31 L32 L33]; 145 | 146 | % Aggregate LMIs 147 | LMIs = [LMI >= 0, Phi >= 0]; 148 | 149 | % Define solver: SeDuMi 150 | opts = sdpsettings(); 151 | opts.solver = 'sedumi'; 152 | opts.verbose = 0; 153 | 154 | % Solve optimization problem subject to LMIs 155 | sol = optimize(LMIs, trace(Pptil_1), opts); 156 | 157 | % Analyze error flags 158 | if sol.problem == 0 159 | % Extract solution 160 | Phi_sol = value(Phi); 161 | Pp_new = value(Pptil_1); 162 | else 163 | fprintf('\nLMI error!\n'); 164 | sol.info 165 | yalmiperror(sol.problem) 166 | end 167 | 168 | % Updates 169 | f.Phi_opt(:,:,k) = Phi_sol; 170 | f.Pp = Pp_new; 171 | end 172 | 173 | % Stop timer and compute contribution to average iteration time 174 | t_exec = toc(t_start); 175 | f.T_it = t_exec / (N+1); 176 | end 177 | 178 | % --------------------------------------------------------------- % 179 | % Update State Estimate 180 | % --------------------------------------------------------------- % 181 | 182 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 183 | % Start timer 184 | t_start = tic; 185 | 186 | % Unpack system model matrices 187 | F = sys_model.F; 188 | C = sys_model.C; 189 | D = sys_model.D; 190 | R = sys_model.R; 191 | M2 = sys_model.M2; 192 | EC = sys_model.EC; 193 | ED = sys_model.ED; 194 | 195 | % Unpack filter parameters 196 | mu = f_params.mu; 197 | ni = f_params.ni; 198 | 199 | % Dimensions 200 | r = size(C,1); 201 | 202 | % Lambda approximation 203 | lamb = (1+ni) * mu * norm(M2'*M2); 204 | 205 | % Auxiliary matrices 206 | T = 1/mu * eye(r) - 1/lamb * (M2*M2'); 207 | S = (C'/T*D + lamb*EC'*ED) / ... 208 | (inv(R) + D'/T*D + lamb*(ED'*ED)); 209 | 210 | % New filtered state estimate 211 | xf_new = f.xp + f.Phi_opt(:,:,k) * ... 212 | (C' - S*D')/T*(y_k - C*f.xp) - ... 213 | lamb*f.Phi_opt(:,:,k)*(EC' - S*ED')*EC*f.xp; 214 | 215 | f.xf(:,k,e) = xf_new; 216 | 217 | % New predicted state estimate 218 | f.xp = F*xf_new; 219 | 220 | % Stop timer and update total time 221 | t_iter = toc(t_start) + f.T_it; 222 | f.T_ex = f.T_ex + t_iter; 223 | end 224 | 225 | % --------------------------------------------------------------- % 226 | 227 | end 228 | 229 | % =================================================================== % 230 | 231 | end 232 | 233 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 234 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 235 | -------------------------------------------------------------------------------- /filters/ORF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Optimal Robust Filter (ORF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the Optimal Robust Filter. % 7 | % - J.Y. Ishihara, M.H. Terra, J.P. Cerri. Optimal Robust Filtering for % 8 | % Systems Subject to Uncertainties. Automatica. 52 (2015). % 9 | % % 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | classdef ORF < Filter 15 | 16 | % =================================================================== % 17 | 18 | properties 19 | Pf; % Filtered estimation error weighting matrix 20 | end 21 | 22 | % =================================================================== % 23 | 24 | % =================================================================== % 25 | 26 | methods 27 | 28 | % --------------------------------------------------------------- % 29 | % Constructor 30 | % --------------------------------------------------------------- % 31 | 32 | function f = ORF(N,T,n) 33 | % Base class constructor 34 | f@Filter(N,T,n); 35 | 36 | % Filter identification 37 | f.id = "ORF (Ishihara, 2015)"; 38 | end 39 | 40 | % --------------------------------------------------------------- % 41 | % Initialization (at each new experiment) 42 | % --------------------------------------------------------------- % 43 | 44 | function initialize(f,init_params) 45 | % Initialize filtered estimation error weighting matrix 46 | f.Pf = init_params.P0; 47 | end 48 | 49 | % --------------------------------------------------------------- % 50 | % Update State Estimate 51 | % --------------------------------------------------------------- % 52 | 53 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 54 | % Start timer 55 | t_start = tic; 56 | 57 | % Unpack system model matrices 58 | F = sys_model.F; 59 | G = sys_model.G; 60 | H = sys_model.H; 61 | Q = sys_model.Q; 62 | C = sys_model.C; 63 | D = sys_model.D; 64 | R = sys_model.R; 65 | M1 = sys_model.M1; 66 | EF = sys_model.EF; 67 | EG = sys_model.EG; 68 | EH = sys_model.EH; 69 | M2 = sys_model.M2; 70 | EC = sys_model.EC; 71 | ED = sys_model.ED; 72 | 73 | if (k == 1) 74 | % Initial filtered estimation error matrix 75 | f.Pf = inv(inv(f.Pf) + C'/R*C); 76 | 77 | % Initial filtered state estimate 78 | f.xf(:,1,e) = f.Pf*C'/R*y_k; 79 | else 80 | % Unpack filter parameters 81 | mu = f_params.mu; 82 | ksi = f_params.ksi; 83 | 84 | % Dimensions 85 | n = size(F,1); 86 | r = size(C,1); 87 | p = size(H,2); 88 | q = size(D,2); 89 | t1 = size(EF,1); 90 | t2 = size(EC,1); 91 | 92 | % Lambda approximation 93 | % lamb = (1+ksi) * mu * norm(blkdiag((M1'*M1), (M2'*M2))); 94 | 95 | % Auxiliary matrices (using parameters) 96 | % Phi1 = 1/mu * eye(n) - 1/lamb * (M1*M1'); 97 | % Phi2 = 1/mu * eye(r) - 1/lamb * (M2*M2'); 98 | % Lam = 1/lamb * eye(t1+t2); 99 | 100 | % Auxiliary matrices (without parameters) 101 | Phi1 = 1/mu * eye(n); 102 | Phi2 = 1/mu * eye(r); 103 | Lam = 1/mu * eye(t1+t2); 104 | 105 | Pbar = blkdiag(f.Pf, Q, R); 106 | Phi = blkdiag(Phi1, Phi2); 107 | 108 | Ii = [eye(n+p+q) zeros(n+p+q,n)]; 109 | 110 | A = [ F H zeros(n,q) -eye(n); 111 | zeros(r,n) zeros(r,p) D C ]; 112 | 113 | EA = [ EF EH zeros(t1,q) zeros(t1,n); 114 | zeros(t2,n) zeros(t2,p) ED EC ]; 115 | 116 | x = [f.xf(:,k-1,e) ; 117 | zeros(p+q,1)]; 118 | 119 | b = [-G * u_k; 120 | y_k ]; 121 | 122 | Eb = [ -EG * u_k ; 123 | zeros(t2,1)]; 124 | 125 | % Framework matrices 126 | 127 | Pcal = blkdiag(Pbar, Phi, Lam); 128 | 129 | Acal = [Ii; A; EA]; 130 | 131 | bcal = [x; b; Eb]; 132 | 133 | M = [Pcal Acal ; 134 | Acal' zeros(2*n+p+q)]; 135 | 136 | v = [ bcal zeros(size(bcal,1),n); 137 | zeros(n+p+q,1) zeros(n+p+q,n) ; 138 | zeros(n,1) -eye(n) ]; 139 | 140 | u = [zeros(size(bcal,1),n); 141 | zeros(n+p+q,n) ; 142 | eye(n) ]; 143 | 144 | % Solution 145 | X = u' * (M \ v); 146 | 147 | % New filtered state estimate 148 | f.xf(:,k,e) = X(:,1); 149 | 150 | % New filtered estimation error matrix 151 | f.Pf = X(:,2:n+1); 152 | end 153 | 154 | % Stop timer and update total time 155 | t_iter = toc(t_start); 156 | f.T_ex = f.T_ex + t_iter; 157 | end 158 | 159 | % --------------------------------------------------------------- % 160 | 161 | end 162 | 163 | % =================================================================== % 164 | 165 | end 166 | 167 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 168 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 169 | -------------------------------------------------------------------------------- /filters/RKF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Robust Kalman Filter (RKF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the Robust Kalman Filter. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | classdef RKF < Filter 13 | 14 | % =================================================================== % 15 | 16 | properties 17 | Pp; % Predicted estimation error weighting matrix 18 | end 19 | 20 | % =================================================================== % 21 | 22 | % =================================================================== % 23 | 24 | methods 25 | 26 | % --------------------------------------------------------------- % 27 | % Constructor 28 | % --------------------------------------------------------------- % 29 | 30 | function f = RKF(N,T,n) 31 | % Base class constructor 32 | f@Filter(N,T,n); 33 | 34 | % Filter identification 35 | f.id = "RKF"; 36 | end 37 | 38 | % --------------------------------------------------------------- % 39 | % Initialization (at each new experiment) 40 | % --------------------------------------------------------------- % 41 | 42 | function initialize(f,init_params) 43 | % Initialize predicted estimation 44 | f.xp = init_params.xp0; 45 | 46 | % Initialize predicted estimation error weighting matrix 47 | f.Pp = init_params.P0; 48 | end 49 | 50 | % --------------------------------------------------------------- % 51 | % Update State Estimate 52 | % --------------------------------------------------------------- % 53 | 54 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 55 | % Start timer 56 | t_start = tic; 57 | 58 | % Unpack system model matrices 59 | F = sys_model.F; 60 | G = sys_model.G; 61 | H = sys_model.H; 62 | Q = sys_model.Q; 63 | C = sys_model.C; 64 | D = sys_model.D; 65 | R = sys_model.R; 66 | M1 = sys_model.M1; 67 | EF = sys_model.EF; 68 | EG = sys_model.EG; 69 | EH = sys_model.EH; 70 | M2 = sys_model.M2; 71 | EC = sys_model.EC; 72 | ED = sys_model.ED; 73 | 74 | % Unpack filter parameters 75 | mu = f_params.mu; 76 | ksi = f_params.ksi; 77 | 78 | % Dimensions 79 | n = size(F,1); 80 | r = size(C,1); 81 | t1 = size(EF,1); 82 | t2 = size(EC,1); 83 | 84 | % Check if there are uncertainties in the sensing model 85 | if (all(M2 == 0)) 86 | % Lambda approximation 87 | lamb = (1+ksi) * mu * norm(M1'*M1); 88 | 89 | % Modified sensing model matrices 90 | Phi2 = 1/mu * eye(r); 91 | Rhat = Phi2 + D*R*D'; 92 | Chat = C; 93 | else 94 | % Lambda approximation 95 | lamb = (1+ksi) * mu * norm(blkdiag((M1'*M1), (M2'*M2))); 96 | 97 | % Modified sensing model matrices 98 | Phi2 = 1/mu * eye(r) - 1/lamb * (M2*M2'); 99 | Rbar = 1/lamb * eye(t2) + ED*R*ED'; 100 | Rhat = Phi2 + D/(inv(R) + lamb*(ED'*ED))*D'; 101 | Chat = C - D*R*ED'/Rbar*EC; 102 | end 103 | 104 | % Modified system model matrices 105 | Phi1 = 1/mu * eye(n) - 1/lamb * (M1*M1'); 106 | Qbar = 1/lamb * eye(t1) + EH*Q*EH'; 107 | Qhat = Phi1 + H/(inv(Q) + lamb*(EH'*EH))*H'; 108 | Fhat = F - H*Q*EH'/Qbar*EF; 109 | Ghat = G - H*Q*EH'/Qbar*EG; 110 | 111 | % Filtered estimation error matrix (inverse) 112 | if (all(M2 == 0)) 113 | Pf_i = inv(f.Pp) + EF'/Qbar*EF + Chat'/Rhat*Chat; 114 | else 115 | Pf_i = inv(f.Pp) + EF'/Qbar*EF + ... 116 | Chat'/Rhat*Chat + EC'/Rbar*EC; 117 | end 118 | 119 | % New filtered state estimate 120 | xf_new = Pf_i \ ... 121 | (f.Pp\f.xp + Chat'/Rhat*y_k - EF'/Qbar*EG*u_k); 122 | 123 | f.xf(:,k,e) = xf_new; 124 | 125 | % New predicted estimation error matrix 126 | f.Pp = Fhat/Pf_i*Fhat' + Qhat; 127 | 128 | % New predicted state estimate 129 | f.xp = Fhat*xf_new + Ghat*u_k; 130 | 131 | % Stop timer and update total time 132 | t_iter = toc(t_start); 133 | f.T_ex = f.T_ex + t_iter; 134 | end 135 | 136 | % --------------------------------------------------------------- % 137 | 138 | end 139 | 140 | % =================================================================== % 141 | 142 | end 143 | 144 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 145 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 146 | -------------------------------------------------------------------------------- /filters/RSKF.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Risk-Sensitive Kalman Filter (RSKF) %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Class that implements the Risk-Sensitive Kalman Filter (RSKF). % 7 | % - M. Zorzi. Robust Kalman Filtering Under Model Perturbations, IEEE % 8 | % Trans. Automat. Control. 62 (6) (2017). % 9 | % % 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | classdef RSKF < Filter 15 | 16 | % =================================================================== % 17 | 18 | properties 19 | Pf; % Nominal conditional variance 20 | Vf; % Least-favorable conditional variance 21 | end 22 | 23 | % =================================================================== % 24 | 25 | % =================================================================== % 26 | 27 | methods 28 | 29 | % --------------------------------------------------------------- % 30 | % Constructor 31 | % --------------------------------------------------------------- % 32 | 33 | function f = RSKF(N,T,n) 34 | % Base class constructor 35 | f@Filter(N,T,n); 36 | 37 | % Filter identification 38 | f.id = "RSKF (Zorzi, 2017)"; 39 | end 40 | 41 | % --------------------------------------------------------------- % 42 | % Initialization (at each new experiment) 43 | % --------------------------------------------------------------- % 44 | 45 | function initialize(f,init_params) 46 | % Initialize predicted estimation 47 | f.xp = init_params.xp0; 48 | 49 | % Initialize least-favorable conditional variance 50 | f.Vf = init_params.V0; 51 | end 52 | 53 | % --------------------------------------------------------------- % 54 | % Update State Estimate 55 | % --------------------------------------------------------------- % 56 | 57 | function update_estimate(f,e,k,u_k,y_k,sys_model,f_params) 58 | % Start timer 59 | t_start = tic; 60 | 61 | % Unpack system model matrices 62 | F = sys_model.F; 63 | H = sys_model.H; 64 | Q = sys_model.Q; 65 | C = sys_model.C; 66 | D = sys_model.D; 67 | R = sys_model.R; 68 | 69 | % Unpack filter parameters 70 | tau = f_params.tau; 71 | c = f_params.c; 72 | 73 | % Dimensions 74 | n = size(F,1); 75 | p = size(H,2); 76 | r = size(C,1); 77 | q = size(D,2); 78 | 79 | % Adapted noise matrices 80 | H = [H zeros(n,q)]; 81 | D = [zeros(r,p) D]; 82 | 83 | % Kalman gain matrix 84 | K = (F*f.Vf*C' + H*D')/(C*f.Vf*C' + D*D'); 85 | 86 | % Update filtered state estimate 87 | f.xf(:,k,e) = F*f.xp + K*(y_k - C*f.xp); 88 | 89 | % Update predicted state estimate 90 | f.xp = f.xf(:,k,e); 91 | 92 | % Update nominal conditioned variance 93 | f.Pf = F*f.Vf*F' - K*(C*f.Vf*C' + D*D')*K' + H*H'; 94 | 95 | % Cholesky decomposition of Pf 96 | Lp = chol(f.Pf, 'lower'); 97 | 98 | % Gamma function 99 | if (tau == 0) 100 | gamma_tau = @(th) -log(1/det(eye(n) - th*f.Pf)) + ... 101 | trace(inv(eye(n) - th*f.Pf) - eye(n)) - c; 102 | elseif (tau == 1) 103 | gamma_tau = @(th) (trace(expm(th*(Lp'*Lp)) * ... 104 | (th*(Lp'*Lp - eye(n))) + eye(n)) - c); 105 | else 106 | gamma_tau = @(th) (trace(-1/(tau*(1-tau)) * ... 107 | (eye(n) - th*(1-tau)*(Lp'*Lp))^(tau/(tau-1)) + ... 108 | 1/(1-tau)*(eye(n) - th*(1-tau)*(Lp'*Lp))^(1/(tau-1)) + ... 109 | 1/tau * eye(n)) - c); 110 | end 111 | 112 | % Compute theta 113 | options = optimoptions('fsolve','Display','none'); 114 | th = fsolve(gamma_tau,0.01,options); 115 | 116 | % Update least-favorable conditioned variance 117 | % f.Vf = Lp*(eye(n) - th*(1-tau)*(Lp'*Lp))^(1/(tau-1))*Lp'; 118 | f.Vf = Lp*expm(th*(Lp'*Lp))*Lp'; 119 | 120 | % Stop timer and update total time 121 | t_iter = toc(t_start); 122 | f.T_ex = f.T_ex + t_iter; 123 | end 124 | 125 | % --------------------------------------------------------------- % 126 | 127 | end 128 | 129 | % =================================================================== % 130 | 131 | end 132 | 133 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 134 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 135 | -------------------------------------------------------------------------------- /generate_system_data.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Generate System Data %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Generates the actual state evolution of the target system and % 7 | % all sensor measurements. % 8 | % % 9 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 10 | 11 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 12 | %%% Setup %%% 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | 15 | % ======================================================================= % 16 | % Clear workspace 17 | % ======================================================================= % 18 | 19 | clc 20 | clear 21 | close all 22 | 23 | % ======================================================================= % 24 | % Simulation parameters 25 | % ======================================================================= % 26 | 27 | fprintf("--> Starting...\n"); 28 | 29 | example = 1; 30 | 31 | N = 1000; 32 | T = 1000; 33 | 34 | % ======================================================================= % 35 | % Get example initial parameters 36 | % ======================================================================= % 37 | 38 | fprintf("--> Defining simulation parameters for Example %d...\n", example); 39 | 40 | k = 1; % In case the system is time-varying, consider the initial step 41 | 42 | addpath example_parameters; 43 | 44 | filename = sprintf('example_parameters_%d', example); 45 | eval(filename); 46 | 47 | % ======================================================================= % 48 | % Problem dimensions 49 | % ======================================================================= % 50 | 51 | n = size(F,1); % System state 52 | m = size(G,2); % System input 53 | p = size(H,2); % System noise 54 | r = size(C,1); % Sensor measurements 55 | q = size(D,2); % Measurement noise 56 | 57 | s1 = size(M1,2); 58 | t1 = size(EF,1); 59 | s2 = size(M2,2); 60 | t2 = size(EC,1); 61 | 62 | % ======================================================================= % 63 | % Pre-allocation 64 | % ======================================================================= % 65 | 66 | % System state (all experiments) 67 | x = zeros(n,N+2,T); 68 | 69 | % System input (all experiments) 70 | u = zeros(m,N+1,T); 71 | 72 | % System noise (all experiments) 73 | w = zeros(p,N+1,T); 74 | 75 | % Sensor measurements (all experiments) 76 | y = zeros(r,N+1,T); 77 | 78 | % Measurement noise (all experiments) 79 | v = zeros(q,N+1,T); 80 | 81 | % Contraction matrices (all experiments) 82 | Delta1 = zeros(s1,t1,N+1,T); 83 | Delta2 = zeros(s2,t2,N+1,T); 84 | 85 | % ======================================================================= % 86 | % Data generation 87 | % ======================================================================= % 88 | 89 | fprintf("--> Generating system data...\n"); 90 | 91 | % Experiments loop 92 | for e = 1:T 93 | 94 | fprintf('===== Experiment %d =====\n', e); 95 | 96 | % Initialize system state for current experiment 97 | x(:,1,e) = x0; 98 | 99 | % Time loop 100 | for k = 1:N+1 101 | % Get system parameters 102 | eval(filename); 103 | 104 | % Current system state 105 | x_k = x(:,k,e); 106 | 107 | % Update system input vector 108 | u(:,k,e) = u_k; 109 | 110 | % Update target system state 111 | [x(:,k+1,e), w(:,k,e), Delta1(:,:,k,e)] = ... 112 | update_target_system(x_k,u_k,sys_model); 113 | 114 | % Obtain a measurement of the target system 115 | [y(:,k,e), v(:,k,e), Delta2(:,:,k,e)] = ... 116 | measure_target(x_k,sys_model); 117 | end 118 | end 119 | 120 | % ======================================================================= % 121 | % Save data 122 | % ======================================================================= % 123 | 124 | filename = sprintf("saved_system_data/system_data_%d", example); 125 | 126 | save(filename, 'x','u','w','y','v'); 127 | % save(filename, 'x','u','w','y','v','Delta1','Delta2'); 128 | 129 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 130 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 131 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Main %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Executes the simulations and shows the results % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 11 | %%% Setup %%% 12 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 13 | 14 | % ======================================================================= % 15 | % Clear workspace 16 | % ======================================================================= % 17 | 18 | clc 19 | clear 20 | close all 21 | 22 | % warning("off"); 23 | fprintf("--> Starting...\n\n"); 24 | 25 | % ======================================================================= % 26 | % Set simulation parameters 27 | % ======================================================================= % 28 | 29 | fprintf("--> Setting simulation parameters...\n\n"); 30 | 31 | sim_parameters; 32 | 33 | fprintf("* Example: %d\n", example); 34 | fprintf("* Time Horizon: %d\n", N); 35 | fprintf("* No. Experiments: %d\n", T); 36 | fprintf("* Filters: "); 37 | 38 | for f = 1:length(sim_filters) 39 | fprintf("%s ", sim_filters(f)); 40 | end 41 | 42 | fprintf("\n\n"); 43 | 44 | % ======================================================================= % 45 | % Load pre-generated system data 46 | % ======================================================================= % 47 | 48 | addpath saved_system_data; 49 | 50 | fprintf("--> Loading system data...\n\n"); 51 | 52 | try 53 | filename = sprintf("system_data_%d", example); 54 | load(filename); 55 | catch 56 | error("There is no data for example %d. Try a different one.",... 57 | example); 58 | end 59 | 60 | % Test if N and T are valid values 61 | 62 | if N > size(x,2)-2 63 | error("Maximum available time horizon is N = %d.", size(x,2)-2); 64 | end 65 | 66 | if T > size(x,3) 67 | error("Maximum available number of experiments is T = %d.", size(x,3)); 68 | end 69 | 70 | % ----------------------------------------------------------------------- % 71 | % Problem dimensions 72 | % ----------------------------------------------------------------------- % 73 | 74 | n = size(x,1); % System state 75 | 76 | % ======================================================================= % 77 | % Select example parameters file 78 | % ======================================================================= % 79 | 80 | addpath example_parameters; 81 | 82 | % Get system parameters 83 | try 84 | example_file = sprintf("example_parameters_%d", example); 85 | eval(example_file); 86 | catch 87 | error("There are no parameters defined for example %d.", example); 88 | end 89 | 90 | % ======================================================================= % 91 | % Set filter parameters 92 | % ======================================================================= % 93 | 94 | addpath filter_parameters; 95 | 96 | fprintf("--> Setting filter parameters...\n\n"); 97 | 98 | try 99 | filename = sprintf("filter_parameters_%d", example); 100 | eval(filename); 101 | catch 102 | error("There are no filter parameters defined for example %d.",... 103 | example); 104 | end 105 | 106 | % ======================================================================= % 107 | % Instantiate filters 108 | % ======================================================================= % 109 | 110 | addpath filters; 111 | 112 | % Number of filters 113 | n_filters = length(sim_filters); 114 | 115 | % Cell containing each filter instance 116 | filters = cell(1,n_filters); 117 | 118 | % Cell containing the initialization parameters of each filter 119 | f_init_params = cell(1,n_filters); 120 | 121 | % Cell containing the parameters of each filter 122 | f_params = cell(1,n_filters); 123 | 124 | % Populate cells according to selected filters 125 | for f = 1:n_filters 126 | try 127 | f_inst_str = sprintf("%s(N,T,n);", sim_filters(f)); 128 | filters{f} = eval(f_inst_str); 129 | 130 | f_init_str = sprintf("%s_init_params", sim_filters(f)); 131 | f_init_params{f} = eval(f_init_str); 132 | 133 | f_params_str = sprintf("%s_params", sim_filters(f)); 134 | f_params{f} = eval(f_params_str); 135 | 136 | % Pre-compute LMIs if necessary 137 | if strcmp(sim_filters(f), "LMI_RKF") 138 | filters{f}.pre_compute_lmis(N,sys_model,... 139 | f_init_params{f},f_params{f}); 140 | end 141 | catch 142 | error("Filter %s is not implemented!", sim_filters(f)); 143 | end 144 | end 145 | 146 | fprintf("--> Filters ready!\n\n"); 147 | 148 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 149 | %%% Simulation Loop %%% 150 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 151 | 152 | fprintf("--> Starting simulation...\n\n"); 153 | 154 | % ======================================================================= % 155 | % Start timer 156 | % ======================================================================= % 157 | 158 | tic 159 | 160 | % ======================================================================= % 161 | % Experiments loop 162 | % ======================================================================= % 163 | 164 | for e = 1:T 165 | 166 | fprintf("========== Experiment %d ==========\n", e); 167 | 168 | % Initialize filters 169 | 170 | for f = 1:n_filters 171 | filters{f}.initialize(f_init_params{f}); 172 | end 173 | 174 | % =================================================================== % 175 | % Time loop 176 | % =================================================================== % 177 | 178 | for k = 1:N+1 179 | 180 | % Get system parameters 181 | eval(example_file); 182 | 183 | % Current input 184 | u_k = u(:,k,e); 185 | 186 | % Current measurement 187 | y_k = y(:,k,e); 188 | 189 | % Update filter estimates 190 | 191 | for f = 1:n_filters 192 | filters{f}.update_estimate(e,k,u_k,y_k,sys_model,f_params{f}); 193 | end 194 | 195 | end % Time loop 196 | 197 | % ------------------------------------------------------------------- % 198 | % Update estimation errors 199 | % ------------------------------------------------------------------- % 200 | 201 | % System state sequence for current experiment 202 | x_e = x(:,1:N+1,e); 203 | 204 | % Update estimation errors of each filter 205 | for f = 1:n_filters 206 | filters{f}.update_error_variance(e,x_e); 207 | end 208 | 209 | end % Experiments loop 210 | 211 | fprintf("\n"); 212 | fprintf("--> Simulation finished!\n\n"); 213 | 214 | % ======================================================================= % 215 | % Stop timer 216 | % ======================================================================= % 217 | 218 | toc 219 | 220 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 221 | %%% Results %%% 222 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 223 | 224 | % ======================================================================= % 225 | % Compute simulation statistics 226 | % ======================================================================= % 227 | 228 | fprintf("\n"); 229 | fprintf("--> Computing statistics...\n\n"); 230 | 231 | % Compute final statistics of each filter 232 | for f = 1:n_filters 233 | filters{f}.compute_filter_stats(T,N); 234 | end 235 | 236 | % Display statistics 237 | display_stats; 238 | 239 | % ======================================================================= % 240 | % Plot results 241 | % ======================================================================= % 242 | 243 | fprintf("--> Plotting results...\n"); 244 | 245 | plot_results; 246 | 247 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 248 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 249 | -------------------------------------------------------------------------------- /measure_target.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Measure Target System %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Generate a sensor measurement of the target system. % 7 | % % 8 | % * Inputs: % 9 | % - x_curr: current system state, x(k) % 10 | % - sys_model: system model structure % 11 | % - Delta2: contraction matrix (optional) % 12 | % % 13 | % * Outputs: % 14 | % - y_k: measurement, y(k) % 15 | % - v_k: measurement noise signal, v(k) % 16 | % - Delta2_k: contraction matrix % 17 | % % 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | 20 | function [y_k,v_k,Delta2_k] = measure_target(x_curr,sys_model,Delta2) 21 | 22 | % ======================================================================= % 23 | % Generate uncertain parameter matrices 24 | % ======================================================================= % 25 | 26 | % Generate contraction (norm <= 1), if not given as argument 27 | if (nargin < 3) 28 | Delta2_k = unifrnd(-1,1); 29 | else 30 | Delta2_k = Delta2; 31 | end 32 | 33 | % Unpack sensing model matrices 34 | 35 | C = sys_model.C; 36 | D = sys_model.D; 37 | R = sys_model.R; 38 | 39 | M2 = sys_model.M2; 40 | EC = sys_model.EC; 41 | ED = sys_model.ED; 42 | 43 | % Parameter uncertainty matrices 44 | 45 | delta_C = M2 * Delta2_k * EC; 46 | delta_D = M2 * Delta2_k * ED; 47 | 48 | % Uncertain parameter matrices 49 | 50 | C_r = C + delta_C; 51 | D_r = D + delta_D; 52 | 53 | % ======================================================================= % 54 | % Generate noise signal 55 | % ======================================================================= % 56 | 57 | v_k = mvnrnd(zeros(size(R,1),1), R)'; 58 | 59 | % ======================================================================= % 60 | % Generate measurement of the target system 61 | % ======================================================================= % 62 | 63 | y_k = C_r * x_curr + D_r * v_k; 64 | 65 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 66 | 67 | end 68 | -------------------------------------------------------------------------------- /plot_results.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Plot Results %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Plots the simulation results. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | % ======================================================================= % 11 | % State estimation 12 | % ======================================================================= % 13 | 14 | figure(); 15 | 16 | % Experiment 17 | e = 1; 18 | 19 | % Final step 20 | Nf = N; 21 | 22 | for i = 1:n 23 | subplot(n,1,i); 24 | 25 | plot(0:Nf, x(i,1:Nf+1,e), ... 26 | 'k-', 'LineWidth', 1.5, ... 27 | 'DisplayName', 'Target System'); 28 | 29 | hold on; 30 | grid on; 31 | 32 | for f = 1:n_filters 33 | plot(0:Nf, filters{f}.xf(i,1:Nf+1,e), ... 34 | 'LineStyle', '--', 'LineWidth', 1.5, ... 35 | 'DisplayName', filters{f}.id); 36 | end 37 | 38 | ylabel_str = sprintf('x_%d', i); 39 | ylabel(ylabel_str); 40 | xlabel('Time Step k'); 41 | legend(); 42 | end 43 | 44 | % ======================================================================= % 45 | % Estimation error variance of each variable 46 | % ======================================================================= % 47 | 48 | figure(); 49 | 50 | title('Error Variance'); 51 | 52 | for i = 1:n 53 | subplot(n,1,i); 54 | 55 | for f = 1:n_filters 56 | plot(0:N, filters{f}.S1(i,1:N+1), ... 57 | 'LineStyle', '-', 'LineWidth', 1.5, ... 58 | 'DisplayName', filters{f}.id); 59 | grid on; 60 | hold on; 61 | end 62 | 63 | ylabel_str = sprintf('x_%d', i); 64 | ylabel(ylabel_str); 65 | xlabel('Time Step k'); 66 | legend(); 67 | end 68 | 69 | % ======================================================================= % 70 | % Estimation error variance (dB) 71 | % ======================================================================= % 72 | 73 | figure(); 74 | 75 | for f = 1:n_filters 76 | semilogx(1:N+1, filters{f}.S2(1:N+1), ... 77 | 'LineStyle', '-', 'LineWidth', 1.5, ... 78 | 'DisplayName', filters{f}.id); 79 | grid on; 80 | hold on; 81 | end 82 | 83 | title('Error Variance'); 84 | xlabel('Time Step k'); 85 | ylabel('Error Variance (dB)'); 86 | legend(); 87 | 88 | 89 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 90 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 91 | -------------------------------------------------------------------------------- /saved_system_data/system_data_1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kaiodt/SCL-RKF-code/7d8f845b41e286310a194142d0e701686450d90d/saved_system_data/system_data_1.mat -------------------------------------------------------------------------------- /sim_parameters.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Simulation Parameters %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Set desired simulation parameters. % 7 | % % 8 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 9 | 10 | % Simulation example 11 | example = 1; 12 | 13 | % Time horizon 14 | N = 100; 15 | 16 | % Number of experiments 17 | T = 10; 18 | 19 | % Choose which filters to simulate 20 | % Available filters: 21 | % KF - Kalman Filter 22 | % RKF - Robust Kalman Filter 23 | % ORF - Optimal Robust Filter (Ishihara, 2015) 24 | % BDU - Bounded Data Uncertainty Filter (Sayed, 2001) 25 | % GCF - Guaranteed Cost Filter (Dong, 2006) 26 | % LMI_RKF - LMI-Based Robust Kalman Filter (Abolhasani, 2018) 27 | % RSKF - Risk-Sensitive Kalman Filter (Zorzi, 2017) 28 | 29 | sim_filters = ["KF","RKF","ORF","BDU","GCF","LMI_RKF","RSKF"]; 30 | 31 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 32 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 33 | -------------------------------------------------------------------------------- /update_target_system.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%% Update Target System State %%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | % % 5 | % * Description: % 6 | % - Updates the state of the target system using the uncertain model. % 7 | % % 8 | % * Inputs: % 9 | % - x_curr: current state, x(k) % 10 | % - u_curr: current input, u(k) % 11 | % - sys_model: system model structure % 12 | % - Delta1: contraction matrix (optional) % 13 | % % 14 | % * Outputs: % 15 | % - x_new: new state, x(k+1) % 16 | % - w_k: noise signal, w(k) % 17 | % - Delta1_k: contraction matrix % 18 | % % 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | 21 | function [x_new,w_k,Delta1_k] = ... 22 | update_target_system(x_curr,u_curr,sys_model,Delta1) 23 | 24 | % ======================================================================= % 25 | % Generate uncertain parameter matrices 26 | % ======================================================================= % 27 | 28 | % Generate contraction (norm <= 1), if not given as argument 29 | if (nargin < 4) 30 | Delta1_k = unifrnd(-1,1); 31 | else 32 | Delta1_k = Delta1; 33 | end 34 | 35 | % Unpack system model matrices 36 | 37 | F = sys_model.F; 38 | G = sys_model.G; 39 | H = sys_model.H; 40 | Q = sys_model.Q; 41 | 42 | M1 = sys_model.M1; 43 | EF = sys_model.EF; 44 | EG = sys_model.EG; 45 | EH = sys_model.EH; 46 | 47 | % Parameter uncertainty matrices 48 | 49 | delta_F = M1 * Delta1_k * EF; 50 | delta_G = M1 * Delta1_k * EG; 51 | delta_H = M1 * Delta1_k * EH; 52 | 53 | % Uncertain parameter matrices 54 | 55 | F_r = F + delta_F; 56 | G_r = G + delta_G; 57 | H_r = H + delta_H; 58 | 59 | % ======================================================================= % 60 | % Generate noise signal 61 | % ======================================================================= % 62 | 63 | w_k = mvnrnd(zeros(size(Q,1),1), Q)'; 64 | 65 | % ======================================================================= % 66 | % Simulate the uncertain target system 67 | % ======================================================================= % 68 | 69 | % Update the state 70 | x_new = F_r * x_curr + G_r * u_curr + H_r * w_k; 71 | 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 73 | 74 | end 75 | --------------------------------------------------------------------------------