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])
            - 1 field 'ssini' is a cell array of vectors that give the
               initial position for estimation. 0 is the default.

 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 %            - 1 field 'ssini' is a cell array of vectors that give the
0032 %               initial position for estimation. 0 is the default.
0033 %
0034 % OUTPUTS:
0035 %          -ao_out
0036 %                (array of) output analysis object containing the output
0037 %                aos
0038 %          -plist_data_out contains 3 fields
0039 %                - 'K' the Kalman feedback
0040 %                - 'P' the state covariance
0041 %                - 'Z' the output covariance
0042 %
0043 % Main assumptions :  noise is stationnary
0044 %                     system is invariant
0045 %                     noise is white
0046 %                     process and measurement noises may be correlated correlated
0047 %                     Right now the algrithm requires the control toolbox
0048 %
0049 % M-FILE INFO: Get information about this methods by calling
0050 %              >> ssm.getInfo('simulate')
0051 %
0052 %              Get information about a specified set-plist by calling:
0053 %              >> ssm.getInfo('simulate', 'Default')
0054 %
0055 % ***** THERE ARE NO DEFAULT PARAMETERS *****
0056 %
0057 % VERSION: $Id:$
0058 %
0059 % HISTORY:
0060 %  10-08-2008 a. Grynagier  - revival
0061 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0062 
0063 function varargout = kalman(varargin)
0064 
0065   %% starting initial checks
0066   utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
0067 
0068   % Check if this is a call for parameters
0069   if utils.helper.isinfocall(varargin{:})
0070     varargout{1} = getInfo(varargin{3});
0071     return
0072   end
0073 
0074   % checking number of inputs work data
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   %% begin function body
0083 
0084   i_sys=1;
0085   % for i_sys = 1:Nsys
0086   %% retrieve system infos
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   %% collecting and finding position of data **for inputs** option plists
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   %% getting A - B - C - D - E - F - H realization
0135   % A - C are the state matrix
0136   % B - D are the signal matrices
0137   % E - F are the noise matrices
0138   % E - F are the constant input matrices
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   %% evaluating Kalman feedback K, innovation gain M, state covariance P, output covariance Z
0149   % given Q and R (process and measurement noise covariances)
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   %% dividing or modifying size of matrices
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   %% getting data position in new matrices
0171   
0172   for i=1:Naos_in
0173       % getting position
0174       [positions, sum, value] = ssm.cellstrfind(aos_varnames,aos_varnames2{i},'all');
0175       % checking against redundant input data
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       % getting position
0185       [positions, sum, value] = ssm.cellstrfind(constants_varnames,constants_varnames2{i},'all');
0186       % checking against redundant input data
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       % getting position
0195       [positions, sum, value] = ssm.cellstrfind(outputs_varnames,outputs_varnames2{i},'all');
0196       % checking against redundant output data
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   %% getting correct number of samples
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     % maybe tdata should be retrieved and verified to be equal, rather than this.
0216   end
0217   if Nsamples == inf % case there is no input!
0218     Nsamples = 0;
0219   end
0220   
0221   %% evaluating data to return
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   %% simulation first step
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   %% simulation loop
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     %% resetting fields
0250     for s = 1:Nss
0251       IncrementX{s} = zeros(sssizes(s),1);
0252     end
0253     IncrementY = zeros(Noutputs_in,1);
0254     
0255     %% prediction step
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     % differential equation
0270     lastX = ssm.cell_add(IncrementX, ssm.cell_mult(As, lastX));
0271     
0272     % output equations
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     %% state correction (=update) step
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     % output correction
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     %% computing and storing outputs
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   %% saving in aos
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   %% construct output analysis object
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   % Build info object
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

Generated on Mon 08-Sep-2008 13:18:47 by m2html © 2003