Home > m > analyses > stuttgart > ltpda_diff_acc.m

ltpda_diff_acc

PURPOSE ^

LTPDA_DIFF_ACC

SYNOPSIS ^

function [X1, X2, X3, P] = ltpda_diff_acc(varargin)

DESCRIPTION ^

 LTPDA_DIFF_ACC

 Usage: [X1, X2, X3, P] = ltpda_diff_acc(sigX1, sigX12, sigFsus, pl)


 Bob 21-05-07

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 function [X1, X2, X3, P] = ltpda_diff_acc(varargin)
0002 
0003 % LTPDA_DIFF_ACC
0004 %
0005 % Usage: [X1, X2, X3, P] = ltpda_diff_acc(sigX1, sigX12, sigFsus, pl)
0006 %
0007 %
0008 % Bob 21-05-07
0009 %
0010 
0011 % Check if this is a call for parameters (necessary forthe gui)
0012 if nargin == 1
0013   if ischar(varargin{1})
0014     if strcmp(varargin{1}, 'Params')
0015       X1 = getDefaultPlist();
0016       return
0017     end
0018   end
0019 end
0020 
0021 
0022 % capture input variable names
0023 invars = {};
0024 for j=1:nargin
0025   if isa(varargin{j}, 'ao')
0026     invars = [invars cellstr(inputname(j))];
0027   end
0028 end
0029 
0030 ALGONAME = mfilename;
0031 VERSION  = '$Id: ltpda_diff_acc.html,v 1.8 2008/02/12 12:17:57 hewitson Exp $';
0032 
0033 % Get input signals
0034 sigX1   = varargin{1};
0035 sigX12  = varargin{2};
0036 sigFsus = varargin{3};
0037 pl      = varargin{4};
0038 
0039 % Combine plists
0040 pl = combine(pl, getDefaultPlist());
0041 
0042 % Get parameters from plist
0043 p1 = find(pl, 'p1');
0044 
0045 % Get data from analysis objects
0046 fs   = sigX12.data.fs;
0047 sigx12  = sigX12.data.y;
0048 sigx1   = sigX1.data.y;
0049 sigfsus = sigFsus.data.y;
0050 
0051 % Call Kalman thingy
0052 [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1);
0053 
0054 % History for output AOs
0055 inhist = [sigX1.hist sigX12.hist sigFsus.hist];
0056 h = history(ALGONAME, VERSION, pl, inhist);
0057 h = set(h, 'invars', invars);
0058 
0059 % Make output AOs
0060 ts = tsdata(sigX1.data.x, x1);
0061 ts = set(ts, 'name', 'x1');
0062 X1 = ao(ts, h);
0063 X1 = set(X1, 'name', 'X1');
0064 
0065 ts = tsdata(sigX1.data.x, x2);
0066 ts = set(ts, 'name', 'x2');
0067 X2 = ao(ts, h);
0068 X2 = set(X2, 'name', 'X2');
0069 
0070 ts = tsdata(sigX1.data.x, x3);
0071 ts = set(ts, 'name', 'x3');
0072 X3 = ao(ts, h);
0073 X3 = set(X3, 'name', 'X3');
0074 
0075 ts = cdata(p);
0076 ts = set(ts, 'name', 'P');
0077 P  = ao(ts, h);
0078 P  = set(P, 'name', 'P');
0079 
0080 %--------------------------------------------------------------------------
0081 % Apply Kalman filter
0082 function [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1)
0083 
0084 % x1 = randn(length(sigx1),1);
0085 % x2 = randn(length(sigx12),1);
0086 % x3 = randn(length(sigfsus),1);
0087 % p  = randn(3,3,length(sigfsus));
0088 
0089 t  = 1/fs;
0090 
0091 %define variables
0092 om2 = sqrt(2.24*(10^(-6)));
0093 deltaom = -7*(10^(-7));
0094 
0095 sigma1_quad = 0.675*(10^(-10));
0096 sigma2_quad = 0.417*(10^(-9));
0097 sigma3_quad = 0.5*(10^(-9));
0098 sigmay_quad = 0.675*(10^(-10));
0099 R = sigmay_quad;
0100 
0101 %matrices and vectors
0102 
0103 PHI = [cosh(om2*t)      (1/om2)*sinh(om2*t)  -(1/(om2^2))*(1-cosh(om2*t));
0104       om2*sinh(om2*t)   cosh(om2*t)         (1/om2)*sinh(om2*t);
0105       0                 0                    1                      ];
0106 
0107 Bd = [(-1/(om2^2))*(cosh(om2*t)-1)*(-deltaom)   (1/(om2^2))*(cosh(om2*t))-1;
0108      (1/om2)*sinh(om2*t)*(-deltaom)             (1/om2)*sinh(om2*t);
0109      0                                           0                        ];
0110 
0111 H = [1 0 0];
0112 
0113 x0 = [0  0  0]';
0114 
0115 P0 = [0.0001  0  0;
0116       0  0.0001  0;
0117       0  0  0.0001];
0118 
0119 %Variablen zur Berechnung von Q(k)
0120 alpha1 = (sigma1_quad/2) -(sigma2_quad/(2*(om2^2))) +(3*sigma3_quad/(2*(om2^4)));
0121 alpha2 = (sigma1_quad/4) +(sigma2_quad/(4*(om2^2))) +(sigma3_quad/(4*(om2^4)));
0122 beta = (1/4)*(sigma1_quad*om2 +sigma2_quad/om2 + sigma3_quad/(om2^3));
0123 epsi1 = sigma2_quad/2 -(sigma1_quad*(om2^2))/2 - sigma3_quad/(2*(om2^2));
0124 epsi2 = (sigma1_quad*(om2^2))/4 + sigma2_quad/4 + sigma3_quad/(4*(om2^2));
0125 
0126 Q11 = alpha1*t +(exp(-om2*t)-exp(om2*t))*(sigma3_quad/(om2^5)) +(exp(2*om2*t)-exp(-2*om2*t))*(alpha2/(2*om2));
0127 
0128 Q12 = -beta/om2 +sigma3_quad/(om2^4) +(beta/(2*om2))*(exp(2*om2*t)+exp(-2*om2*t)) -(sigma3_quad/(2*(om2^4)))*(exp(-om2*t)+exp(om2*t));
0129 
0130 Q13 = -(sigma3_quad/(om2^2))*t+(sigma3_quad/(2*(om2^3)))*(-exp(-om2*t)+exp(om2*t));
0131 
0132 Q21 = Q12;
0133 
0134 Q22 = epsi1*t+(epsi2/(2*om2))*(exp(2*om2*t)-exp(-2*om2*t));
0135 
0136 Q23 = -sigma3_quad/(om2^2) +(sigma3_quad/(2*(om2^2)))*(exp(-om2*t)+exp(om2*t));
0137 
0138 Q31 = Q13;
0139 
0140 Q32 = Q23;
0141 
0142 Q33 = sigma3_quad*t;
0143 
0144 Q = [Q11  Q12  Q13;
0145      Q21  Q22  Q23;
0146      Q31  Q32  Q33];
0147 
0148 x1 = zeros(size(sigx12));
0149 x2 = zeros(size(sigx12));
0150 x3 = zeros(size(sigx12));
0151 p  = zeros(3,3,length(sigx12));
0152 
0153 u = [sigx1 sigfsus];
0154 size(sigx1)
0155 size(sigfsus)
0156 size(u)
0157 
0158 for i=1:length(x1)
0159 
0160     % kalman gain
0161     K = P0*H'*inv(H*P0*H'+R);
0162     % new estimation
0163     x_est = x0 + K*(sigx12(i)-H*x0);
0164     x1(i) = x_est(1,1);
0165     x2(i) = x_est(2,1);
0166     x3(i) = x_est(3,1);
0167     % new covariance
0168     P = (eye(3) - K*H)*P0;
0169     p(:,:,i) = P;
0170 
0171     % updated state estimate
0172     x0 = PHI*x_est + Bd*u(i,:)';
0173     P0 = PHI*P*PHI' + Q;
0174 %     i = i+1;
0175 
0176 end
0177 
0178 %--------------------------------------------------------------------------
0179 % get default parameter list
0180 function pl = getDefaultPlist()
0181 
0182 pl = plist();
0183 pl = append(pl, param('p1', 1));
0184 
0185

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