0001 function [X1, X2, X3, P] = ltpda_diff_acc(varargin)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
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
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
0034 sigX1 = varargin{1};
0035 sigX12 = varargin{2};
0036 sigFsus = varargin{3};
0037 pl = varargin{4};
0038
0039
0040 pl = combine(pl, getDefaultPlist());
0041
0042
0043 p1 = find(pl, 'p1');
0044
0045
0046 fs = sigX12.data.fs;
0047 sigx12 = sigX12.data.y;
0048 sigx1 = sigX1.data.y;
0049 sigfsus = sigFsus.data.y;
0050
0051
0052 [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1);
0053
0054
0055 inhist = [sigX1.hist sigX12.hist sigFsus.hist];
0056 h = history(ALGONAME, VERSION, pl, inhist);
0057 h = set(h, 'invars', invars);
0058
0059
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
0082 function [x1, x2, x3, p] = kalman(fs, sigx1, sigx12, sigfsus, p1)
0083
0084
0085
0086
0087
0088
0089 t = 1/fs;
0090
0091
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
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
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
0161 K = P0*H'*inv(H*P0*H'+R);
0162
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
0168 P = (eye(3) - K*H)*P0;
0169 p(:,:,i) = P;
0170
0171
0172 x0 = PHI*x_est + Bd*u(i,:)';
0173 P0 = PHI*P*PHI' + Q;
0174
0175
0176 end
0177
0178
0179
0180 function pl = getDefaultPlist()
0181
0182 pl = plist();
0183 pl = append(pl, param('p1', 1));
0184
0185