0001
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
0035
0036
0037
0038
0039
0040
0041
0042
0043
0044
0045
0046
0047
0048
0049
0050
0051
0052
0053
0054
0055
0056
0057
0058
0059
0060
0061
0062
0063 function varargout = kalman(varargin)
0064
0065
0066 utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
0067
0068
0069 if utils.helper.isinfocall(varargin{:})
0070 varargout{1} = getInfo(varargin{3});
0071 return
0072 end
0073
0074
0075 if ~nargin==2, error('wrong number of inputs!'), end
0076
0077 if ~isequal(class(varargin{1}),'ssm'), error(['argument is not a ssm but a ', class(varargin{1})]),end
0078 if ~isequal(class(varargin{2}),'plist'), error(['argument is not a plist but a ', class(varargin{2})]),end
0079
0080 sys = varargin{1};
0081 plist_inputs = varargin{2};
0082
0083
0084 i_sys=1;
0085
0086
0087 if ~sys(i_sys).isnumerical
0088 error(['error because system ',sys(i_sys).name,' is not numerical']);
0089 end
0090 M2 = ssm.cell_fusion(sys(i_sys).mmats, sys(i_sys).sssizes, sys(i_sys).sssizes);
0091 Minv = ssm.cell_recut(M2^-1, sys(i_sys).sssizes, sys(i_sys).sssizes);
0092 if ~isequal(M2, eye(size(M2)))
0093 utils.helper.msg(utils.const.msg.MNAME, 'warning, M is not the identity!!! It is:');
0094 end
0095 sssizes = sys(i_sys).sssizes;
0096 timestep = sys(i_sys).timestep;
0097 Ninputs = sys(i_sys).Ninputs;
0098 Noutputs = sys(i_sys).Noutputs;
0099 Nss = sys(i_sys).Nss;
0100 outputsizes = sys(i_sys).outputsizes;
0101 inputsizes = sys(i_sys).inputsizes;
0102
0103 ssini = find(plist_inputs,'ssini');
0104 if isempty(ssini)
0105 ssini = cell(sys(i_sys).Nss,1);
0106 for i=1:sys(i_sys).Nss
0107 ssini{i} = zeros(sys(i_sys).sssizes(i),1);
0108 end
0109 end
0110
0111
0112 noise_varnames = find(plist_inputs, 'noise variable names');
0113 Nnoise_in = length(noise_varnames);
0114 noise_in = find(plist_inputs, 'variance');
0115 noise_pos = zeros(Nnoise_in,2);
0116
0117 aos_varnames = find(plist_inputs, 'aos variable names');
0118 Naos_in = length(aos_varnames);
0119 aos_in = find(plist_inputs, 'aos');
0120 aos_pos = zeros(size(aos_varnames),2);
0121
0122 constants_varnames = find(plist_inputs, 'constant variable names');
0123 Nconstants_in = length(constants_varnames);
0124 constants_in = find(plist_inputs, 'constants');
0125 constants_pos = zeros(size(constants_varnames),2);
0126
0127 outputs_varnames = find(plist_inputs, 'output variable names');
0128 Noutputs_in = length(outputs_varnames);
0129 outputs_in = find(plist_inputs, 'outputs');
0130 outputs_pos = zeros(size(outputs_varnames),1);
0131
0132
0133
0134
0135
0136
0137
0138
0139 options = plist('inputs', aos_varnames , 'outputs', outputs_varnames, 'states', 'ALL' );
0140 [A B C D Ts aos_varnames2 ss_varnames2 outputs_varnames2] = double(copy(sys(i_sys), true), options);
0141
0142 options = plist('inputs', noise_varnames, 'outputs', outputs_varnames, 'states', 'ALL' );
0143 [A E C F Ts noise_varnames2 ss_varnames2 outputs_varnames2] = double(copy(sys(i_sys), true), options);
0144
0145 options = plist('inputs', constants_varnames , 'outputs', outputs_varnames, 'states', 'ALL' );
0146 [A G C H Ts constants_varnames2 ss_varnames2 outputs_varnames2] = double(copy(sys(i_sys), true), options);
0147
0148
0149
0150 Qn = E*noise_in*transpose(E);
0151 Qn = (Qn + 1e-10*norm(Qn)*eye(size(Qn)));
0152 Rn = F*noise_in*transpose(F);
0153 Rn = Rn + 1e-10*norm(Rn)*eye(size(Rn));
0154 Nn = E*noise_in*transpose(F);
0155 P = eye(size(A))*1e20;
0156 for i=1:1000
0157 P = A*P*A'+Qn;
0158 K = P*C'*(C*P*C'+Rn)^-1;
0159 P = (eye(size(A)) - K*C)*P;
0160 end
0161 Z = C*P*C' + Rn;
0162
0163
0164 As = ssm.cell_recut(A, sssizes, sssizes);
0165 Bs = ssm.cell_recut(B, sssizes, Naos_in);
0166 Gs = ssm.cell_recut(G, sssizes, Nconstants_in);
0167 Cs = ssm.cell_recut(C, Noutputs_in, sssizes);
0168 Ks = ssm.cell_recut(K, sssizes, Noutputs_in);
0169
0170
0171
0172 for i=1:Naos_in
0173
0174 [positions, sum, value] = ssm.cellstrfind(aos_varnames,aos_varnames2{i},'all');
0175
0176 if sum==1
0177 aos_pos(positions) = i;
0178 aos_data{positions} = aos_in(positions).data.getY ;
0179 elseif sum>1
0180 error(['more than one so variable is named ', value]);
0181 end
0182 end
0183 for i=1:Nconstants_in
0184
0185 [positions, sum, value] = ssm.cellstrfind(constants_varnames,constants_varnames2{i},'all');
0186
0187 if sum==1
0188 constants_pos(positions) = i;
0189 elseif sum>1
0190 error(['more than one constant variable is named ', value]);
0191 end
0192 end
0193 for i=1:Noutputs_in
0194
0195 [positions, sum, value] = ssm.cellstrfind(outputs_varnames,outputs_varnames2{i},'all');
0196
0197 if sum==1
0198 outputs_pos(positions) = i;
0199 elseif sum>1
0200 error(['more than one output variable is named ', value]);
0201 end
0202 end
0203
0204
0205 Nsamples = inf;
0206 f0 = 1/timestep;
0207 for i=1:Naos_in
0208 Nsamples = min(Nsamples,length(aos_in(i).data.y));
0209 if ~(f0==aos_in(i).data.fs)
0210 str = ['WARNING : ssm frequency is ',num2str(f0),...
0211 ' but sampling frequency of ao named ',...
0212 aos_in(i).name, ' is ', num2str(aos_in(i).data.fs) ];
0213 utils.helper.msg(utils.const.msg.MNAME, str);
0214 end
0215
0216 end
0217 if Nsamples == inf
0218 Nsamples = 0;
0219 end
0220
0221
0222 return_states = find(plist_inputs, 'return states');
0223 returnStates = false(1,Nss);
0224 for i=1:Nss
0225 returnStates(i) = ~isempty(find( ismember(return_states, i)));
0226 end
0227 return_outputs = find(plist_inputs, 'return outputs');
0228 returnOutputs = ~isempty(find( ismember(return_outputs, 1)));
0229
0230
0231 x = cell(Nss, 1);
0232 lastX = ssini;
0233 for i=1:Nss
0234 if returnStates(i)
0235 x{i} = zeros(sssizes(i), Nsamples);
0236 end
0237 end
0238 if returnOutputs
0239 y = zeros(Noutputs, Nsamples);
0240 end
0241 IncrementX = cell(Nss,1);
0242
0243
0244 for k = 1:Nsamples
0245 if rem(k,500)==0
0246 utils.helper.msg(utils.const.msg.MNAME, ['filtering time : ',num2str(k*timestep) ]);
0247 end
0248
0249
0250 for s = 1:Nss
0251 IncrementX{s} = zeros(sssizes(s),1);
0252 end
0253 IncrementY = zeros(Noutputs_in,1);
0254
0255
0256 for i=1:Naos_in
0257 for s=1:Nss
0258 IncrementX{s} = IncrementX{s} +Bs{s}(:,aos_pos(i))*aos_data{i}(k);
0259 end
0260 IncrementY = IncrementY +D(:,aos_pos(i))*aos_data{i}(k);
0261 end
0262 for i=1:Nconstants_in
0263 for s=1:Nss
0264 IncrementX{s} = IncrementX{s} + Gs{s}(:,constants_pos(i))*constants_in(i);
0265 end
0266 IncrementY = IncrementY + H(:,constants_pos(i))*constants_in(i);
0267 end
0268
0269
0270 lastX = ssm.cell_add(IncrementX, ssm.cell_mult(As, lastX));
0271
0272
0273 lastY = zeros(Noutputs_in,1);
0274 for s=1:Nss
0275 lastY = lastY + Cs{s}*lastX{s};
0276 end
0277 lastY = lastY + IncrementY;
0278
0279 thisY = zeros(Noutputs_in,1);
0280 for i=1:Noutputs_in
0281 thisY(outputs_pos(i)) = outputs_in(i).data.y(k);
0282 end
0283
0284
0285 for i=1:Noutputs_in
0286 for s=1:Nss
0287 lastX{s} = lastX{s} + Ks{s}*( thisY - lastY) ;
0288 end
0289 end
0290
0291 lastY = zeros(Noutputs_in,1);
0292 for s=1:Nss
0293 lastY = lastY + Cs{s}*lastX{s};
0294 end
0295 lastY = lastY + IncrementY;
0296
0297
0298
0299 for s=1:Nss
0300 if returnStates(i)
0301 x{s}(:,k) = lastX{s};
0302 end
0303 end
0304 if returnOutputs
0305 y(:,k) = lastY;
0306 end
0307 end
0308
0309 Naos = 0;
0310 for i=1:Nss
0311 if returnStates(i)
0312 Naos = Naos + sssizes(i);
0313 end
0314 end
0315 if returnOutputs
0316 Naos = Naos + Noutputs_in;
0317 end
0318 ao_out(Naos) = ao;
0319 if ~exist('t','var')
0320 t = (1:Nsamples)*timestep;
0321 end
0322 count = 1;
0323 for i=1:Nss
0324 for j=1:sssizes(i)
0325 if returnStates(i)
0326 ao_out(count) = ao(tsdata(t,x{i}(j,:)));
0327 ao_out(count).setName(['kal_X_',num2str(i),'_',num2str(j)]);
0328 ao_out(count).setDescription(...
0329 ['ssm#: ',num2str(i_sys),...
0330 ', state: ',sys(i_sys).ssnames{i},' ',sys(i_sys).ssvarnames{i}{j}]);
0331 ao_out(count) = ao_out(count).addHistory(ssm.getInfo(mfilename), plist_inputs , {''}, sys(i_sys).hist );
0332 count = count+1;
0333 end
0334 end
0335 end
0336 for i=1:Noutputs_in
0337 if returnOutputs
0338 ao_out(count) = ao(tsdata(t,y(i,:)));
0339 ao_out(count).setName(['kal_Y_',num2str(i)]);
0340 ao_out(count).setDescription(...
0341 ['ssm#: ',num2str(i_sys),', output: ',outputs_varnames2{i}] );
0342 ao_out(count) = ao_out(count).addHistory(ssm.getInfo(mfilename), plist_inputs , {''}, sys(i_sys).hist );
0343 count = count+1;
0344 end
0345 end
0346
0347 plist_data_out = plist('K', K, 'P', P, 'Z', Z);
0348 varargout = {ao_out, plist_data_out};
0349 end
0350
0351 function ii = getInfo(varargin)
0352 if nargin == 1 && strcmpi(varargin{1}, 'None')
0353 sets = {};
0354 pls = [];
0355 elseif nargin == 1 && ~isempty(varargin{1}) && ischar(varargin{1})
0356 sets{1} = varargin{1};
0357 pls = getDefaultPlist(sets{1});
0358 else
0359 sets = {'Default'};
0360 pls = [];
0361 for kk=1:numel(sets)
0362 pls = [pls getDefaultPlist(sets{kk})];
0363 end
0364 end
0365
0366 ii = minfo(mfilename, 'ssm', '', 'STATESPACE', '$Id: resp.m,v 1.17 2008/07/22 10:22:38 ingo Exp $', sets, pls);
0367 end
0368
0369 function plo = getDefaultPlist(set)
0370 switch set
0371 case 'Default'
0372 plo = plist(...
0373 'noise variable names', {'noise_readout', 'noise_special'}, ...
0374 'variance', [1 0.1;1 0.1],...
0375 'aos variable names', {'input_1' 'input_2'},...
0376 'aos', [ao, ao],...
0377 'constant variable names',{ 'noise_readout' 'noise_readout2'}, ...
0378 'constant', [1;6], ...
0379 'output variable names', {'output_1'}, ...
0380 'outputs', ao , ...
0381 'return states', [], ...
0382 'return outputs', [1] , ...
0383 'ssini', {} ...
0384 );
0385 otherwise
0386 error('### Unknown parameter set [%s].', set);
0387 end
0388 end