Home > m > analyses > stuttgart > ltpda_dkalman.m

ltpda_dkalman

PURPOSE ^

LTPDA_DKALMAN

SYNOPSIS ^

function [X1, X2, X3] = ltpda_dkalman(varargin)

DESCRIPTION ^

 LTPDA_DKALMAN

 Usage: [X1, X2, X3] = ltpda_dkalman(Ad,Bd,Cd,Dd,Q,R,P0,x0,in1,in2,meas)

     Ad: discrete system matrix [3x3]
     Bd: discrete input matrix [3x2]
     Cd: discrete measurement matrix [1x3]
     Dd: 0

     Q: discrete covariance matrix of process noise [3x3]
     R: discrete covariance matrix of measurement noise [1x1]
     x0: prior state estimate
     P0: error covariance for x0

     in1: first input ao
     in2: second input ao
     meas: measurement data (ao)

 veronika 03-07-07

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 function [X1, X2, X3] = ltpda_dkalman(varargin)
0002 
0003 % LTPDA_DKALMAN
0004 %
0005 % Usage: [X1, X2, X3] = ltpda_dkalman(Ad,Bd,Cd,Dd,Q,R,P0,x0,in1,in2,meas)
0006 %
0007 %     Ad: discrete system matrix [3x3]
0008 %     Bd: discrete input matrix [3x2]
0009 %     Cd: discrete measurement matrix [1x3]
0010 %     Dd: 0
0011 %
0012 %     Q: discrete covariance matrix of process noise [3x3]
0013 %     R: discrete covariance matrix of measurement noise [1x1]
0014 %     x0: prior state estimate
0015 %     P0: error covariance for x0
0016 %
0017 %     in1: first input ao
0018 %     in2: second input ao
0019 %     meas: measurement data (ao)
0020 %
0021 % veronika 03-07-07
0022 %
0023 
0024 % Check if this is a call for parameters (necessary forthe gui)
0025 if nargin == 1
0026   if ischar(varargin{1})
0027     if strcmp(varargin{1}, 'Params')
0028       X1 = getDefaultPlist();
0029       return
0030     end
0031   end
0032 end
0033 
0034 % capture input variable names
0035 invars = {};
0036 for j=1:nargin
0037   if isa(varargin{j}, 'ao')
0038     invars = [invars cellstr(inputname(j))];
0039   end
0040 end
0041 
0042 ALGONAME = mfilename;
0043 VERSION  = '$Id: ltpda_dkalman.html,v 1.6 2008/02/12 12:17:57 hewitson Exp $';
0044 
0045 % Get input signals
0046 Ad   = varargin{1};
0047 Bd   = varargin{2};
0048 Cd   = varargin{3};
0049 Dd   = varargin{4};
0050 Q       = varargin{5};
0051 R       = varargin{6};
0052 P0      = varargin{7};
0053 x0      = varargin{8};
0054 sigX1   = varargin{9};
0055 sigX12  = varargin{10};
0056 sigFsus = varargin{11};
0057 
0058 % Get data from analysis objects
0059 fs      = sigX12.data.fs;
0060 sigx12  = sigX12.data.y;
0061 sigx1   = sigX1.data.y;
0062 sigfsus = sigFsus.data.y;
0063 
0064 
0065 u = [sigx1 sigfsus];
0066 size(sigx1);
0067 size(sigfsus);
0068 size(u);
0069 
0070 for i=1:length(sigx1)
0071 
0072     % kalman gain
0073     K = P0*Cd'*inv(Cd*P0*Cd'+R);
0074     % new estimation
0075     x_est = x0 + K*(sigx12(i)-Cd*x0);
0076     x1(i) = x_est(1,1);
0077     x2(i) = x_est(2,1);
0078     x3(i) = x_est(3,1);
0079     % new covariance
0080     P = (eye(3) - K*Cd)*P0;
0081     % updated state estimate
0082     x0 = Ad*x_est + Bd*u(i,:)';
0083     P0 = Ad*P*Ad' + Q;
0084 end
0085 
0086 % History for output AOs
0087 inhist = [sigX1.hist sigX12.hist sigFsus.hist];
0088 h = history(ALGONAME, VERSION, inhist);
0089 h = set(h, 'invars', invars);
0090 
0091 % Make output AOs
0092 ts = tsdata(sigX1.data.x, x1);
0093 ts = set(ts, 'name', 'x1');
0094 X1 = ao(ts, h);
0095 X1 = set(X1, 'name', 'X1');
0096 
0097 ts = tsdata(sigX1.data.x, x2);
0098 ts = set(ts, 'name', 'x2');
0099 X2 = ao(ts, h);
0100 X2 = set(X2, 'name', 'X2');
0101 
0102 ts = tsdata(sigX1.data.x, x3);
0103 ts = set(ts, 'name', 'x3');
0104 X3 = ao(ts, h);
0105 X3 = set(X3, 'name', 'X3');
0106 
0107 %--------------------------------------------------------------------------
0108 % get default parameter list
0109 function pl = getDefaultPlist()
0110 
0111 pl = plist();
0112 
0113 
0114

Generated on Tue 12-Feb-2008 13:12:45 by m2html © 2003