Home > m > timetools > statespacefunctions > ltpda_ss_kalman_filter.m

ltpda_ss_kalman_filter

PURPOSE ^

Kalman filter

SYNOPSIS ^

function varargout = ltpda_ss_kalman_filter(varargin)

DESCRIPTION ^

Kalman filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 DESCRIPTION: Kalman filter

 CALL: ao_out = simulate_discrete(ao_out, ao_in, data_plists, syst)

 INPUTS: ao_out - (array of) plist of ao containing output data stream
 ao_in - (array of) plist of ao containing input data stream
 data_plists - (array of) plist describing noise inputs process and
 observation covariances
 syst - (array of) system data in plist format

 OUTPUTS: ao_out (array of) output analysis object

 ***** THERE ARE NO DEFAULT PARAMETERS *****

 &&VERSION: $Id: ltpda_ss_kalman_filter.m,v 1.4 2008/03/11 16:52:56 adrien Exp $

 HISTORY: 26-02-2008 A Grynagier
 19-02-2008 A Grynagier
 17-02-2008 A Grynagier
 Creation 30-01-2008 M Weyrich

 TO DO: options to be defined
 implement input type of three kinds:
 - AO
 - Constant 
 - gaussian white noise
 use LTPDA functions to generate white noise
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 function varargout = ltpda_ss_kalman_filter(varargin)
0002 %Kalman filter
0003 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0004 %
0005 % DESCRIPTION: Kalman filter
0006 %
0007 % CALL: ao_out = simulate_discrete(ao_out, ao_in, data_plists, syst)
0008 %
0009 % INPUTS: ao_out - (array of) plist of ao containing output data stream
0010 % ao_in - (array of) plist of ao containing input data stream
0011 % data_plists - (array of) plist describing noise inputs process and
0012 % observation covariances
0013 % syst - (array of) system data in plist format
0014 %
0015 % OUTPUTS: ao_out (array of) output analysis object
0016 %
0017 % ***** THERE ARE NO DEFAULT PARAMETERS *****
0018 %
0019 % &&VERSION: $Id: ltpda_ss_kalman_filter.m,v 1.4 2008/03/11 16:52:56 adrien Exp $
0020 %
0021 % HISTORY: 26-02-2008 A Grynagier
0022 % 19-02-2008 A Grynagier
0023 % 17-02-2008 A Grynagier
0024 % Creation 30-01-2008 M Weyrich
0025 %
0026 % TO DO: options to be defined
0027 % implement input type of three kinds:
0028 % - AO
0029 % - Constant
0030 % - gaussian white noise
0031 % use LTPDA functions to generate white noise
0032 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0033 ALGONAME = mfilename;
0034 VERSION = '$Id: ltpda_ss_kalman_filter.m,v 1.4 2008/03/11 16:52:56 adrien Exp $';
0035 CATEGORY = 'STATESPACE';
0036 display(['starting ' ALGONAME]);
0037 
0038 if not(isempty(varargin))
0039     if isequal( varargin{1}, 'Version')
0040         varargout = VERSION;
0041         return;
0042     elseif isequal(varargin{1}, 'Params')
0043         varargout = plist();
0044         return;
0045     elseif isequal(varargin{1}, 'Category')
0046         varargout = CATEGORY;
0047         return;
0048     end
0049 end
0050 
0051 ao_out = ao();
0052 aos_Y = varargin{1};
0053 aos_in_list = varargin{2};
0054 systs = varargin{4};
0055 data_plists = varargin{3};
0056 for i_ao_Y = 1:length(aos_Y)
0057     for i_ao_in = 1:length(aos_in_list)
0058         for i_syst = 1:length(systs)
0059             for i_data_plist = 1:length(data_plists)
0060                 ao_Y = aos_Y(i_ao_Y);
0061                 ao_in_list = aos_in_list(i_ao_in);
0062                 syst = systs(i_syst);
0063                 data_plist = data_plists(i_data_plist);
0064 %% retrieve system infos
0065                 A = find(syst, 'AMAT');
0066                 B = find(syst, 'BMATS');
0067                 C = find(syst, 'CMAT');
0068                 D = find(syst, 'DMATS');
0069                 A = A{1};
0070                 C = C{1};
0071                 nss = size(A,1);
0072                 nout = size(C,1);
0073                 %check iniside A, Bs, c, Ds there is no symbolic variable
0074                 Xini = find(syst, 'XINI');
0075                 timestep = find(syst, 'TIMESTEP');
0076                 NBINPUTS = find(syst, 'NBINPUTS');
0077                 INPUTISUSED = find(syst, 'INPUTISUSED');
0078                 INPUTNAMES = find(syst, 'INPUTNAMES');
0079                 % INPUTSIZES = find(syst, 'INPUTSIZES');
0080                 XISOUTPUT = find(syst, 'XISOUTPUT');
0081 
0082 
0083 %% get info from plist for noise inputs
0084                 % input covariances
0085                 Qs = find(data_plist, 'Qs');
0086                 Rs = find(data_plist, 'Rs');
0087                 Noise_Names = find(data_plist, 'Noise_Names');
0088                 Q = zeros(nss);
0089                 R = zeros(nout);
0090                 for i=1:length(Noise_Names)
0091                     for j=1:NBINPUTS
0092                         if isequal(Noise_Names{i},INPUTNAMES{j} )
0093                             Q = Q + B{j}*Qs{i}*B{j}'; %assumption in this covariance addition is that the inputs are not correlated
0094                             R = R + D{j}*Rs{i}*D{j}';
0095                         end
0096                     end
0097                 end
0098 
0099 %% read out AO (time-series data)
0100                 ao_in = cell(NBINPUTS,1);
0101                 for i=1:NBINPUTS
0102                     if INPUTISUSED(i) 
0103                         ao_in{i} = find(ao_in_list, INPUTNAMES{i});
0104                         if isequal(class(ao_in{i}), 'ao')
0105                             if not(isequal(ao_in{i}.data.x,{})) %making sure ao is not empty
0106                                 t = ao_in{i}.data.x;
0107                                 dt = diff(t);
0108                                 if sum(not(isequal(dt,timestep)))>1
0109                                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong timestep for simulation'];
0110                                     error(msg);
0111                                 end
0112                                 if not( isequal( size(B{i},1), size(D{i},1), size(ao_in{i}.data.y,1) ) )
0113                                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong input size'];
0114                                     error(msg);
0115                                 end
0116                             end
0117                         end
0118                     end
0119                 end
0120 %% getting correct number of samples
0121                 nsamples = size(ao_Y.data.y,1);
0122 
0123 %% taking only output data (in case XISOUTPUT = 1)
0124                 SizeData = size(ao_Y.data.y,2);
0125                 if XISOUTPUT
0126                     range = (nss+1):SizeData;
0127                 else
0128                     range = 1:SizeData;
0129                 end
0130 
0131 %% Kalman filter vector allocation
0132                 P0=eye(nss);
0133                 X_est=zeros(size(A,1),nsamples+1);
0134                 X_upd=zeros(size(A,1),nsamples+1);
0135                 X_upd(:,1)=Xini;
0136                 Y_tilde=zeros(nout,nsamples+1);
0137                 
0138 %% Kalman filter
0139                 for i=2:nsamples %chg
0140                     IncrementX = zeros(nss,1);
0141                     IncrementY = zeros(nout,1);
0142                     for j=1:length(ao_in)
0143                         if not(isequal(ao_in{j},[]))
0144                             IncrementX = IncrementX +  B{j}*ao_in{j}.data.y(i,:) ;
0145                             IncrementY = IncrementY +  D{j}*ao_in{j}.data.y(i,:) ;
0146                         end
0147                     end
0148                     X_est(:,i) = A*X_upd(:,i-1)+IncrementX; %Estimate
0149                     Y_tilde(:,i) = ao_Y.data.y(i,range)'-C*X_est(:,i)-IncrementY; %Measurement residual chg dEst forgotten
0150                     P = A*P0*A'+Q;
0151                     S = C*P*C'+R;  %Covariance of Y-tilde
0152                     K = P*C'*inv(S); %K. gain
0153                     X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i); %Update
0154                     P0 = (eye(size(K,1))-K*C)*P;
0155                 end
0156 
0157 %% data object
0158                 if exist('t','var')
0159                     ts = tsdata(t,[X_est; Y_tilde]) ;
0160                 else
0161                     ts = tsdata(timestep*(1:(nsamples+1))',[X_est; Y_tilde]') ;
0162                 end
0163 
0164 %% history object, old history from ao_in plus new
0165                 % hi_old = ao_Y.hist;
0166                 hi_new = history('Kalman','version', syst);
0167                 % hi = [hi_old hi_new];
0168                 % h = history(hi);
0169 
0170 %% construct output analysis object
0171                 ao_out(i_ao_Y, i_ao_in, i_data_plist, i_syst)= ao(ts,hi_new);
0172             end
0173         end
0174     end
0175 end
0176 varargout = {ao_out};
0177 end

Generated on Mon 31-Mar-2008 13:54:54 by m2html © 2003