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

Generated on Fri 08-Jun-2007 16:09:11 by m2html © 2003