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_in, syst)

 INPUTS: ao_in_list - plist of :
                         - ao containing output data stream

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_in, syst)
0008 %
0009 % INPUTS: ao_in_list - plist of :
0010 %                         - ao containing output data stream
0011 
0012 %                         - plist describing an input noise with a mean
0013 %                             'MEAN' (vector) and a sigma 'SIGMA' (Matrix)
0014 % syst - system data in plist format
0015 %
0016 % OUTPUTS: ao_out _ output analysis object
0017 %
0018 % ***** THERE ARE NO DEFAULT PARAMETERS *****
0019 %
0020 % &&VERSION: $Id: ltpda_ss_kalman_filter.m,v 1.2 2008/02/25 12:43:44 adrien Exp $
0021 %
0022 % HISTORY: 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.2 2008/02/25 12:43:44 adrien Exp $';
0035 CATEGORY = 'STATESPACE';
0036 display(['starting ' ALGONAME]);
0037 
0038 if isequal( varargin{1}, 'Version')
0039     varargout = VERSION;
0040     return;
0041 elseif isequal(varargin{1}, 'Params')
0042     varargout = plist();
0043     return;
0044 elseif isequal(varargin{1}, 'Category')
0045     varargout = CATEGORY;
0046     return;
0047 end
0048 
0049 ao_Y = varargin{1};
0050 ao_in_list = varargin{2};
0051 syst = varargin{3};
0052 data_plist = varargin{4};
0053 
0054 %% retrieve system infos
0055 A = find(syst, 'AMAT');
0056 B = find(syst, 'BMATS');
0057 C = find(syst, 'CMAT');
0058 D = find(syst, 'DMATS');
0059 A = A{1};
0060 C = C{1};
0061 nss = size(A,1);
0062 nout = size(C,1);
0063 %check iniside A, Bs, c, Ds there is no symbolic variable
0064 Xini = find(syst, 'XINI');
0065 timestep = find(syst, 'TIMESTEP');
0066 NBINPUTS = find(syst, 'NBINPUTS');
0067 INPUTISUSED = find(syst, 'INPUTISUSED');
0068 INPUTNAMES = find(syst, 'INPUTNAMES');
0069 % INPUTSIZES = find(syst, 'INPUTSIZES');
0070 XISOUTPUT = find(syst, 'XISOUTPUT');
0071 
0072 
0073 %% get info from plist for noise inputs
0074 % input covariances
0075 Qs = find(data_plist, 'Qs');
0076 Rs = find(data_plist, 'Rs');
0077 Noise_Names = find(data_plist, 'Noise_Names');
0078 Q = zeros(nss);
0079 R = zeros(nout);
0080 for i=1:length(Noise_Names)
0081     for j=1:NBINPUTS
0082         if isequal(Noise_Names{i},INPUTNAMES{j} )
0083             Q = Q + B{j}*Qs{i}*B{j}'; %assumption in this covariance addition is that the inputs are not correlated
0084             R = R + D{j}*Rs{i}*D{j}';
0085         end
0086     end
0087 end
0088 
0089 %% read out AO (time-series data)
0090 ao_in = cell(NBINPUTS,1);
0091 for i=1:NBINPUTS
0092     if INPUTISUSED(i) 
0093         ao_in{i} = find(ao_in_list, INPUTNAMES{i});
0094         if isequal(class(ao_in{i}), 'ao')
0095             if not(isequal(ao_in{i}.data.x,{})) %making sure ao is not empty
0096                 t = ao_in{i}.data.x;
0097                 dt = diff(t);
0098                 if sum(not(isequal(dt,timestep)))>1
0099                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong timestep for simulation'];
0100                     error(msg);
0101                 end
0102                 if not( isequal( size(B{i},1), size(D{i},1), size(ao_in{i}.data.y,1) ) )
0103                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong input size'];
0104                     error(msg);
0105                 end
0106             end
0107         end
0108     end
0109 end
0110 %% getting correct number of samples
0111 nsamples = size(ao_Y.data.y,1);
0112 
0113 %% taking only output data (in case XISOUTPUT = 1)
0114 SizeData = size(ao_Y.data.y,2);
0115 if XISOUTPUT
0116     range = (nss+1):SizeData;
0117 else
0118     range = 1:SizeData;
0119 end
0120 
0121 
0122 
0123 
0124 %% Kalman filter vector allocation
0125 P0=eye(nss);
0126 X_est=zeros(size(A,1),nsamples+1);
0127 X_upd=zeros(size(A,1),nsamples+1);
0128 X_upd(:,1)=Xini;
0129 Y_tilde=zeros(nout,nsamples+1);
0130 
0131 %%%%%%%%%%%%%%%%%%%
0132 %%% Kalman filter %% chg
0133 %%%%%%%%%%%%%%%%%%%
0134 
0135 for i=2:nsamples %chg
0136     IncrementX = zeros(nss,1);
0137     IncrementY = zeros(nout,1);
0138     for j=1:length(ao_in)
0139         if not(isequal(ao_in{j},[]))
0140             IncrementX = IncrementX +  B{j}*ao_in{j}.data.y(i,:) ;
0141             IncrementY = IncrementY +  D{j}*ao_in{j}.data.y(i,:) ;
0142         end
0143     end
0144     X_est(:,i) = A*X_upd(:,i-1)+IncrementX; %Estimate
0145     Y_tilde(:,i) = ao_Y.data.y(i,range)'-C*X_est(:,i)-IncrementY; %Measurement residual chg dEst forgotten
0146     P = A*P0*A'+Q;
0147     S = C*P*C'+R;  %Covariance of Y-tilde
0148     K = P*C'*inv(S); %K. gain
0149     X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i); %Update
0150     P0 = (eye(size(K,1))-K*C)*P;
0151 end
0152 
0153 
0154 %% data object
0155 if exist('t','var')
0156     ts = tsdata(t,[X_est; Y_tilde]) ;
0157 else
0158     ts = tsdata(timestep*(1:(nsamples+1))',[X_est; Y_tilde]') ;
0159 end
0160 
0161 %% history object, old history from ao_in plus new
0162 % hi_old = ao_Y.hist;
0163 hi_new = history('Kalman','version', syst);
0164 % hi = [hi_old hi_new];
0165 % h = history(hi);
0166 
0167 %% construct output analysis object
0168 varargout  = ao(ts,hi_new);
0169 end

Generated on Tue 26-Feb-2008 10:52:52 by m2html © 2003