Home > m > timetools > statespacefunctions > ltpda_kalman_filter.m

ltpda_kalman_filter

PURPOSE ^

Kalman filter

SYNOPSIS ^

function ao_out = ltpda_ss_kalman_filter(ao_Y, ao_in_list, data_plist, syst)

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 ao_out = ltpda_ss_kalman_filter(ao_Y, ao_in_list, data_plist, syst)
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_kalman_filter.html,v 1.1 2008/03/01 12:29:32 hewitson 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 
0034 ALGONAME = mfilename;
0035 VERSION = '$Id: ltpda_kalman_filter.html,v 1.1 2008/03/01 12:29:32 hewitson Exp $';
0036 CATEGORY = 'STATESPACE';
0037 display(['starting ' ALGONAME]);
0038 
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 syst = varargin{2};
0050 ao_in_list = varargin{1};
0051 
0052 %% retrieve system infos
0053 A = find(syst, 'AMAT');
0054 B = find(syst, 'BMATS');
0055 C = find(syst, 'CMAT');
0056 D = find(syst, 'DMATS');
0057 A = A{1};
0058 C = C{1};
0059 nss = size(A,1);
0060 %check iniside A, Bs, c, Ds there is no symbolic variable
0061 xini = find(syst, 'XINI');
0062 timestep = find(syst, 'TIMESTEP');
0063 NBINPUTS = find(syst, 'NBINPUTS');
0064 INPUTISUSED = find(syst, 'INPUTISUSED');
0065 INPUTNAMES = find(syst, 'INPUTNAMES');
0066 INPUTSIZES = find(syst, 'INPUTSIZES');
0067 
0068 %% get info from plist for noise inputs
0069 % input covariances
0070 Qs = find(data_plist, 'Qs');
0071 Rs = find(data_plist, 'Rs');
0072 Noise_Names = find(data_plist, 'Noise_Names');
0073 Q = zeros(nss);
0074 for i=1:length(Noise_Names)
0075     for j=1:NBINPUTS
0076         if isequal(Noise_Names{i},INPUTNAMES{j} )
0077             Q = Q + B{j}*Qs{i}*B{j}'; %assumption in this covariance addition is that the inputs are not correlated
0078             R = R + D{j}*Rs{i}*D{j}';
0079         end
0080     end
0081 end
0082 
0083 %% read out AO (time-series data)
0084 ao_in = cell(NBINPUTS,1);
0085 for i=1:NBINPUTS
0086     if INPUTISUSED(i) 
0087         ao_in{i} = find(ao_in_list, INPUTNAMES{i});
0088         if isequal(class(ao_in{i}), 'ao')
0089             if not(isequal(ao_in{i}.data.x,{})) %making sure ao is not empty
0090                 t = ao_in{i}.data.x;
0091                 dt = diff(t);
0092                 if sum(not(isequal(dt,timestep)))>1
0093                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong timestep for simulation'];
0094                     error(msg);
0095                 end
0096                 if not( isequal( size(B{i},1), size(D{i},1), size(ao_in{i}.data.y,1) ) )
0097                     msg = ['error in input number ', i , ' called ', INPUTNAMES{i}, 'because wrong input size'];
0098                     error(msg);
0099                 end
0100             end
0101         end
0102     end
0103 end
0104 %% getting correct number of samples
0105 nsamples = size(ao_Y{i}.data.y,2);
0106 
0107 
0108 
0109 
0110 
0111 %% Kalman filter vector allocation
0112 P0=eye(nss);
0113 X_est=zeros(size(A,1),nsamples+1);
0114 X_upd=zeros(size(A,1),nsamples+1);
0115 X_upd(:,1)=X(:,1);
0116 Y_tilde=zeros(size(Y,1),nsamples+1);
0117 
0118 %%%%%%%%%%%%%%%%%%%
0119 %%% Kalman filter %% chg
0120 %%%%%%%%%%%%%%%%%%%
0121 
0122 for i=2:sim_n %chg
0123     IncrementX = zeros(nss,1);
0124     IncrementY = zeros(size(C,1),1);
0125     for j=1:length(ao_in)
0126         IncrementX = IncrementX +  B{j}*ao_in{j}.data.y(:,i) ;
0127         IncrementY = IncrementY +  D{j}*ao_in{j}.data.y(:,i) ;
0128     end
0129     X_est(:,i) = A*X_upd(:,i-1)+IncrementX; %Estimate
0130     Y_tilde(:,i) = ao_Y.data.y(:,i)-C*X_est(:,i)-IncrementY; %Measurement residual chg dEst forgotten
0131     P = A*P0*A'+Q;
0132     S = C*P*C'+R;  %Covariance of Y-tilde
0133     K = P*C'*inv(S); %K. gain
0134     X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i); %Update
0135     P0 = (eye(size(K,1))-K*C)*P;
0136 end
0137 
0138 
0139 %% data object
0140 if exist('t','var')
0141     ts = tsdata(t,[X_est; Ytilde]) ;
0142 else
0143     ts = tsdata(1:nsamples,[X_est; Ytilde]) ;
0144 end
0145 
0146 %% history object, old history from ao_in plus new
0147 hi_old = ao_Y.hist ;
0148 hi_new = history('Kalman','version', syst);
0149 hi = [hi_old hi_new];
0150 h = history(hi);
0151 
0152 %% construct output analysis object
0153 ao_out  = ao(ts,h) ;
0154 end

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