0001 function [X1, X2, X3, P] = ltpda_diff_acc(varargin)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
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
0023 sigX1 = varargin{1};
0024 sigX12 = varargin{2};
0025 sigFsus = varargin{3};
0026 pl = varargin{4};
0027
0028
0029 pl = combine(pl, getDefaultPlist());
0030
0031
0032 p1 = find(pl, 'p1');
0033
0034
0035 fs = sigX12.data.fs;
0036 sigx12 = sigX12.data.x;
0037 sigx1 = sigX1.data.x;
0038 sigfsus = sigFsus.data.x;
0039
0040
0041 [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1);
0042
0043
0044 inhist = [sigX1.hist sigX12.hist sigFsus.hist];
0045 h = history(ALGONAME, VERSION, pl, inhist);
0046 h = set(h, 'invars', invars);
0047
0048
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
0071 function [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1)
0072
0073
0074
0075
0076
0077
0078 t = 1/fs;
0079
0080
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
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
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
0150 K = P0*H'*inv(H*P0*H'+R);
0151
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
0157 P = (eye(3) - K*H)*P0;
0158 p(:,:,i) = P;
0159
0160
0161 x0 = PHI*x_est + Bd*u(i,:)';
0162 P0 = PHI*P*PHI' + Q;
0163
0164
0165 end
0166
0167
0168
0169 function pl = getDefaultPlist()
0170
0171 pl = plist();
0172 pl = append(pl, param('p1', 1));
0173
0174