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