Home > classes > @ssm > kalman.m

kalman

PURPOSE ^

kalman applies Kalman filtering to a discrete ssm with given i/o

SYNOPSIS ^

function varargout = kalman(varargin)

DESCRIPTION ^

kalman applies Kalman filtering to a discrete ssm with given i/o
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 DESCRIPTION: %kalman applies Kalman filtering to a discrete ssm with
 given i/o
 
 CALL: [ao_out, plist_data_out] = ...
 kalman(sys, plist_inputs_noise, plist_inputs_aos, plist_inputs_constants)

 INPUTS:
         - sys, (array of) ssm object
         - plist_inputs_noise contains 2 fields :
               - 'input variable names' with a cell vector of strings of
               the desired input variable names
               - 'covariance' which gives the covariance of this noise between
               the different corresponding inputs given for the *TIME
               CONTINUOUS* noise model
         - plist_inputs_aos contains 2 fields :
               - 'input variable names' with a cell vector of strings of
               the desired input variable names
               - 'mean' which gives the aos for the different
               corresponding inputs
         - plist_inputs_constants contains 2 fields :
               - 'input variable names' with a cell vector of strings of
               the desired input variable names
               - 'constants' which gives the DC value for the different
               corresponding inputs
         - 2 arrays telling which blocks should be stored and returned
               - 'return states' : for blocks of the state space (ex [8])
               - 'return outputs' : for blocks of the output (ex [1 5])
    ***     the three plists CANNOT be arrays anymore.    ***
 OUTPUTS:
          -ao_out
                (array of) output analysis object containing the output
                aos
          -plist_data_out contains 3 fields
                - 'K' the Kalman feedback
                - 'P' the state covariance
                - 'Z' the output covariance

 Main assumptions :  noise is stationnary
                     system is invariant
                     noise is white
                     process and measurement noises may be correlated correlated
                     Right now the algrithm requires the control toolbox

 M-FILE INFO: Get information about this methods by calling
              >> ssm.getInfo('simulate')

              Get information about a specified set-plist by calling:
              >> ssm.getInfo('simulate', 'Default')

 ***** THERE ARE NO DEFAULT PARAMETERS *****

 VERSION: $Id:$

 HISTORY:
  10-08-2008 a. Grynagier  - revival
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 %kalman applies Kalman filtering to a discrete ssm with given i/o
0002 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0003 %
0004 % DESCRIPTION: %kalman applies Kalman filtering to a discrete ssm with
0005 % given i/o
0006 %
0007 % CALL: [ao_out, plist_data_out] = ...
0008 % kalman(sys, plist_inputs_noise, plist_inputs_aos, plist_inputs_constants)
0009 %
0010 % INPUTS:
0011 %         - sys, (array of) ssm object
0012 %         - plist_inputs_noise contains 2 fields :
0013 %               - 'input variable names' with a cell vector of strings of
0014 %               the desired input variable names
0015 %               - 'covariance' which gives the covariance of this noise between
0016 %               the different corresponding inputs given for the *TIME
0017 %               CONTINUOUS* noise model
0018 %         - plist_inputs_aos contains 2 fields :
0019 %               - 'input variable names' with a cell vector of strings of
0020 %               the desired input variable names
0021 %               - 'mean' which gives the aos for the different
0022 %               corresponding inputs
0023 %         - plist_inputs_constants contains 2 fields :
0024 %               - 'input variable names' with a cell vector of strings of
0025 %               the desired input variable names
0026 %               - 'constants' which gives the DC value for the different
0027 %               corresponding inputs
0028 %         - 2 arrays telling which blocks should be stored and returned
0029 %               - 'return states' : for blocks of the state space (ex [8])
0030 %               - 'return outputs' : for blocks of the output (ex [1 5])
0031 %    ***     the three plists CANNOT be arrays anymore.    ***
0032 % OUTPUTS:
0033 %          -ao_out
0034 %                (array of) output analysis object containing the output
0035 %                aos
0036 %          -plist_data_out contains 3 fields
0037 %                - 'K' the Kalman feedback
0038 %                - 'P' the state covariance
0039 %                - 'Z' the output covariance
0040 %
0041 % Main assumptions :  noise is stationnary
0042 %                     system is invariant
0043 %                     noise is white
0044 %                     process and measurement noises may be correlated correlated
0045 %                     Right now the algrithm requires the control toolbox
0046 %
0047 % M-FILE INFO: Get information about this methods by calling
0048 %              >> ssm.getInfo('simulate')
0049 %
0050 %              Get information about a specified set-plist by calling:
0051 %              >> ssm.getInfo('simulate', 'Default')
0052 %
0053 % ***** THERE ARE NO DEFAULT PARAMETERS *****
0054 %
0055 % VERSION: $Id:$
0056 %
0057 % HISTORY:
0058 %  10-08-2008 a. Grynagier  - revival
0059 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0060 
0061 function varargout = kalman(varargin)
0062 
0063   %% starting initial checks
0064   utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
0065 
0066   % Check if this is a call for parameters
0067   if utils.helper.isinfocall(varargin{:})
0068     varargout{1} = getInfo(varargin{3});
0069     return
0070   end
0071 
0072   % checking number of inputs work data
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   %% begin function body
0081 
0082   i_sys=1;
0083   % for i_sys = 1:Nsys
0084   %% retrieve system infos
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   %% collecting and finding position of data **for inputs** option plists
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   %% getting A - B - C - D - E - F - H realization
0124   % A - C are the state matrix
0125   % B - D are the signal matrices
0126   % E - F are the noise matrices
0127   % E - F are the constant input matrices
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   %% evaluating Kalman feedback K, innovation gain M, state covariance P, output covariance Z
0138   % given Q and R (process and measurement noise covariances)
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   %% dividing or modifying size of matrices
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   %% getting data position in new matrices
0160   
0161   for i=1:Naos_in
0162       % getting position
0163       [positions, sum, value] = ssm.cellstrfind(aos_varnames,aos_varnames2{i},'all');
0164       % checking against redundant input data
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       % getting position
0174       [positions, sum, value] = ssm.cellstrfind(constants_varnames,constants_varnames2{i},'all');
0175       % checking against redundant input data
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       % getting position
0184       [positions, sum, value] = ssm.cellstrfind(outputs_varnames,outputs_varnames2{i},'all');
0185       % checking against redundant output data
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   %% getting correct number of samples
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     % maybe tdata should be retrieved and verified to be equal, rather than this.
0205   end
0206   if Nsamples == inf % case there is no input!
0207     Nsamples = 0;
0208   end
0209   
0210   %% evaluating data to return
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   %% simulation first step
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   %% simulation loop
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     %% resetting fields
0239     for s = 1:Nss
0240       IncrementX{s} = zeros(sssizes(s),1);
0241     end
0242     IncrementY = zeros(Noutputs_in,1);
0243     
0244     %% prediction step
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     % differential equation
0259     lastX = ssm.cell_add(IncrementX, ssm.cell_mult(As, lastX));
0260     
0261     % output equations
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     %% state correction (=update) step
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     % output correction
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     %% computing and storing outputs
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   %% saving in aos
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   %% construct output analysis object
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   % Build info object
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

Generated on Mon 25-Aug-2008 22:39:29 by m2html © 2003