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.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
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
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
0070 XISOUTPUT = find(syst, 'XISOUTPUT');
0071
0072
0073
0074
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}';
0084 R = R + D{j}*Rs{i}*D{j}';
0085 end
0086 end
0087 end
0088
0089
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,{}))
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
0111 nsamples = size(ao_Y.data.y,1);
0112
0113
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
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
0133
0134
0135 for i=2:nsamples
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;
0145 Y_tilde(:,i) = ao_Y.data.y(i,range)'-C*X_est(:,i)-IncrementY;
0146 P = A*P0*A'+Q;
0147 S = C*P*C'+R;
0148 K = P*C'*inv(S);
0149 X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i);
0150 P0 = (eye(size(K,1))-K*C)*P;
0151 end
0152
0153
0154
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
0162
0163 hi_new = history('Kalman','version', syst);
0164
0165
0166
0167
0168 varargout = ao(ts,hi_new);
0169 end