0001 function ao_out = ltpda_ss_kalman_filter(ao_Y, ao_in_list, data_plist, syst)
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
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
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
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
0069
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}';
0078 R = R + D{j}*Rs{i}*D{j}';
0079 end
0080 end
0081 end
0082
0083
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,{}))
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
0105 nsamples = size(ao_Y{i}.data.y,2);
0106
0107
0108
0109
0110
0111
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
0120
0121
0122 for i=2:sim_n
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;
0130 Y_tilde(:,i) = ao_Y.data.y(:,i)-C*X_est(:,i)-IncrementY;
0131 P = A*P0*A'+Q;
0132 S = C*P*C'+R;
0133 K = P*C'*inv(S);
0134 X_upd(:,i) = X_est(:,i)+K*Y_tilde(:,i);
0135 P0 = (eye(size(K,1))-K*C)*P;
0136 end
0137
0138
0139
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
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
0153 ao_out = ao(ts,h) ;
0154 end