0001 function varargout = ltpda_ss_kalman_filter(varargin)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030
0031
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
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
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
0080 XISOUTPUT = find(syst, 'XISOUTPUT');
0081
0082
0083
0084
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}';
0094 R = R + D{j}*Rs{i}*D{j}';
0095 end
0096 end
0097 end
0098
0099
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,{}))
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
0121 nsamples = size(ao_Y.data.y,1);
0122
0123
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
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
0139 for i=2:nsamples
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;
0149 Y_tilde(:,i) = ao_Y.data.y(i,range)'-C*X_est(:,i)-IncrementY;
0150 P = A*P0*A'+Q;
0151 S = C*P*C'+R;
0152 K = P*C'*inv(S);
0153 X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i);
0154 P0 = (eye(size(K,1))-K*C)*P;
0155 end
0156
0157
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
0165
0166 hi_new = history('Kalman','version', syst);
0167
0168
0169
0170
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