0001 function [X1, X2, X3] = ltpda_dkalman(varargin)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
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
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.m,v 1.3 2007/07/16 12:53:23 ingo Exp $';
0044
0045
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
0059 fs = sigX12.data.fs;
0060 sigx12 = sigX12.data.x;
0061 sigx1 = sigX1.data.x;
0062 sigfsus = sigFsus.data.x;
0063
0064
0065 u = [sigx1 sigfsus];
0066 size(sigx1);
0067 size(sigfsus);
0068 size(u);
0069
0070 for i=1:length(sigx1)
0071
0072
0073 K = P0*Cd'*inv(Cd*P0*Cd'+R);
0074
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
0080 P = (eye(3) - K*Cd)*P0;
0081
0082 x0 = Ad*x_est + Bd*u(i,:)';
0083 P0 = Ad*P*Ad' + Q;
0084 end
0085
0086
0087 inhist = [sigX1.hist sigX12.hist sigFsus.hist];
0088 h = history(ALGONAME, VERSION, inhist);
0089 h = set(h, 'invars', invars);
0090
0091
0092 ts = tsdata(sigX1.data.t, x1);
0093 ts = set(ts, 'name', 'x1');
0094 X1 = ao(ts, h);
0095 X1 = set(X1, 'name', 'X1');
0096
0097 ts = tsdata(sigX1.data.t, x2);
0098 ts = set(ts, 'name', 'x2');
0099 X2 = ao(ts, h);
0100 X2 = set(X2, 'name', 'X2');
0101
0102 ts = tsdata(sigX1.data.t, x3);
0103 ts = set(ts, 'name', 'x3');
0104 X3 = ao(ts, h);
0105 X3 = set(X3, 'name', 'X3');
0106
0107
0108
0109 function pl = getDefaultPlist()
0110
0111 pl = plist();
0112
0113
0114