0
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
1 % KALMAN applies Kalman filtering to a discrete ssm with given i/o
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
2 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
3 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
4 % DESCRIPTION: KALMAN applies Kalman filtering to a discrete ssm with
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
5 % given i/o.
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
6 % CALL: [mat_out pl_out] = kalman(sys, plist_inputs)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
7 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
8 % INPUTS:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
9 % - sys, (array of) ssm object
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
10 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
11 % OUTPUTS:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
12 % _ mat_out contains specified returned aos
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
13 % _ pl_out contains 'lastX', the last state position
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
14 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
15 % <a href="matlab:utils.helper.displayMethodInfo('ssm', 'kalman')">Parameters Description</a>
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
16 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
17 % VERSION: $Id: kalman.m,v 1.51 2011/04/17 21:28:05 adrien Exp $
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
18 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
19 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
20
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
21 function varargout = kalman(varargin)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
22
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
23 %% starting initial checks
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
24
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
25 % use the caller is method flag
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
26 callerIsMethod = utils.helper.callerIsMethod;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
27
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
28 % Check if this is a call for parameters
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
29 if utils.helper.isinfocall(varargin{:})
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
30 varargout{1} = getInfo(varargin{3});
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
31 return
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
32 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
33
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
34 utils.helper.msg(utils.const.msg.MNAME, ['running ', mfilename]);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
35
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
36 % Collect input variable names
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
37 in_names = cell(size(varargin));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
38 for ii = 1:nargin,in_names{ii} = inputname(ii);end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
39
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
40 % Collect all SSMs and plists
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
41 [sys, ssm_invars, rest] = utils.helper.collect_objects(varargin(:), 'ssm', in_names);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
42 [pl, invars2, rest] = utils.helper.collect_objects(rest(:), 'plist');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
43 if ~isempty(rest)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
44 pl = combine(pl, plist(rest{:}));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
45 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
46 pl = combine(pl, getDefaultPlist());
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
47
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
48 %% retrieve system infos
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
49 if ~all(sys.isnumerical)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
50 error(['error because system ', sys.name, ' is not numerical']);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
51 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
52 timestep = sys.timestep;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
53 if timestep==0
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
54 error('timestep should not be 0 in simulate!!')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
55 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
56 if ~callerIsMethod
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
57 inhist = sys(:).hist;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
58 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
59 if pl.isparam('white noise variable names')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
60 error('The noise option used must be split between "covariance" and "cpsd". "noise variable names" does not exist anymore!')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
61 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
62
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
63 %% display time ?
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
64 displayTime = find(pl, 'displayTime');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
65
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
66 %% initial state
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
67 ssini = find(pl,'ssini');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
68 if isempty(ssini)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
69 ssini = cell(sys.Nss,1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
70 for i=1:sys.Nss
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
71 ssini{i} = zeros(sys.sssizes(i),1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
72 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
73 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
74 ssSizesIni = sys.statesizes;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
75 ssini = ssm.blockMatFusion(ssini,ssSizesIni,1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
76
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
77 %% modifying system's ordering
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
78 if find(pl, 'reorganize')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
79 sys = sys.reorganize(pl, 'set', 'for kalman', 'internal', 'internal');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
80 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
81 sys_est = sys(1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
82 sys_exp = sys(2);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
83
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
84 %% getting system's i/o sizes
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
85 Naos_in = sys_est.inputsizes(1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
86 Nnoise = sys_est.inputsizes(2);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
87 Nconstants = sys_est.inputsizes(3);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
88 NstatesOut = sys_est.outputsizes(1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
89 NoutputsOut = sys_est.outputsizes(2);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
90 Nknown = sys_exp.outputsizes(2);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
91
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
92 aos_in = find(pl, 'aos');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
93 known_out = find(pl, 'known outputs');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
94 constants_in = find(pl, 'constants');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
95 cov_in = find(pl, 'covariance');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
96 cpsd_in = find(pl, 'CPSD');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
97 noise_in = blkdiag(cov_in, cpsd_in/(timestep*2));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
98
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
99 if numel(aos_in)~=Naos_in
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
100 error(['There are ' num2str(numel(aos_in)) ' input aos and ' num2str(Naos_in) ' corresponding inputs indexed.' ])
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
101 elseif numel(known_out)~=Nknown
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
102 error(['There are ' num2str(numel(known_out)) ' known output aos and ' num2str(Nknown) ' corresponding inputs indexed.' ])
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
103 elseif numel(diag(noise_in))~=Nnoise
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
104 error(['There are ' num2str(numel(noise_in)) ' input noise variances and ' num2str(Naos_in) ' corresponding inputs indexed.' ])
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
105 elseif numel(constants_in)~=Nconstants
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
106 error(['There are ' num2str(numel(constants_in)) ' input constants and ' num2str(Nconstants) ' corresponding inputs indexed.' ])
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
107 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
108 [U1,S1,V1] = svd(noise_in.'); %#ok<NASGU>
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
109 noise_mat = U1*sqrt(S1)/sqrt(timestep*2);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
110
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
111 A = sys_est.amats{1,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
112 Cstates = sys_est.cmats{1,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
113 Coutputs = sys_est.cmats{2,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
114 Baos = sys_est.bmats{1,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
115 Daos = sys_est.dmats{2,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
116 Bnoise = sys_est.bmats{1,2}*noise_mat;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
117 % Dnoise = sys_est.dmats{1,2}*noise_mat;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
118 Bcst = sys_est.bmats{1,3};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
119 Dcst = sys_est.dmats{2,3};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
120
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
121 CoutputsK = sys_exp.cmats{2,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
122 DaosK = sys_exp.dmats{2,1};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
123 DnoiseK = sys_exp.dmats{2,2}*noise_mat;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
124 DcstK = sys_exp.dmats{2,3};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
125
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
126 %% getting correct number of samples
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
127 Nsamples = find(pl, 'Nsamples');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
128 f0 = 1/timestep;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
129 for i=1:Naos_in
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
130 Nsamples = min(Nsamples,length(aos_in(i).y));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
131 try
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
132 if ~(f0==aos_in(i).fs)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
133 str = ['WARNING : ssm frequency is ',num2str(f0),...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
134 ' but sampling frequency of ao named ',...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
135 aos_in(i).name, ' is ', num2str(aos_in(i).fs) ];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
136 utils.helper.msg(utils.const.msg.MNAME, str);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
137 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
138 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
139 % maybe tdata should be retrieved and verified to be equal, rather than this.
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
140 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
141 for i=1:Nknown
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
142 Nsamples = min(Nsamples,length(known_out(i).y));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
143 try
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
144 if ~(f0==known_out(i).fs)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
145 str = ['WARNING : ssm frequency is ',num2str(f0),...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
146 ' but sampling frequency of ao named ',...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
147 aos_in(i).name, ' is ', num2str(aos_in(i).fs) ];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
148 utils.helper.msg(utils.const.msg.MNAME, str);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
149 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
150 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
151 % maybe tdata should be retrieved and verified to be equal, rather than this.
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
152 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
153 if Nsamples == inf % case there is no input!
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
154 display('warning : no input providing simulation duration is available!!')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
155 Nsamples = 0;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
156 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
157
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
158 %% evaluating Kalman feedback K, innovation gain M, state covariance P, output covariance Z
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
159 % given Q and R (process and measurement noise covariances)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
160 Qn = Bnoise*noise_in*transpose(Bnoise);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
161 Qn = (Qn + 1e-10*norm(Qn)*eye(size(Qn)));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
162 Rn = DnoiseK*noise_in*transpose(DnoiseK);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
163 Rn = Rn + 1e-10*norm(Rn)*eye(size(Rn));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
164 % Nn = Bnoise*noise_in*transpose(Dnoise);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
165 P = eye(size(A))*1e20;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
166 for i=1:10000
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
167 P = A*P*A'+Qn;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
168 K = P*CoutputsK'*(CoutputsK*P*CoutputsK'+Rn)^-1;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
169 P = (eye(size(A)) - K*CoutputsK)*P;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
170 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
171 Z = Coutputs*P*Coutputs' + Rn;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
172
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
173 %% constant vector
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
174 constants_vectX = Bcst*constants_in;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
175 constants_vectY = Dcst*constants_in;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
176 constants_vectYKnown = DcstK*constants_in;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
177
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
178 %% ao vector
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
179 aos_vect = zeros(Naos_in, Nsamples);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
180 for j = 1:Naos_in
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
181 aos_vect(j,:) = aos_in(j).y(1:Nsamples).';
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
182 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
183 Y_in = zeros(Nknown, Nsamples);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
184 for j=1:Nknown
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
185 Y_in(j,:) = reshape( known_out(j).y(1:Nsamples), 1, [] ).';
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
186 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
187
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
188 %% rewriting fields to ssm/doSimulate
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
189
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
190 A_kalman = A - K*Coutputs*A;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
191 Baos_kalman = [ Baos - K*CoutputsK*Baos - K*DaosK K];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
192 aos_vect_kalman = [aos_vect; Y_in ];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
193 Bcst_kalman = constants_vectX - K*constants_vectYKnown - K*CoutputsK*constants_vectX;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
194 Coutputs_kalman = [Cstates ; Coutputs];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
195 Dcst_kalman = [zeros(size(Cstates,1),1) ; constants_vectY];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
196 Daos_kalman = [...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
197 zeros(size(Cstates,1), size(Daos,2)) zeros(size(Cstates,1), size(K,2)) ;...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
198 Daos zeros(size(Daos,1), size(K,2))];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
199 Cstates_kalman = zeros(0, size(A,2));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
200 Bnoise_kalman = zeros(size(A,1), 0);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
201 Dnoise_kalman = zeros(size(Coutputs_kalman,1), 0);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
202
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
203 %% call to doSimulate
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
204 doTerminate = false;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
205 terminationCond = false;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
206 forceComplete = false;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
207
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
208 [x, y, lastX] = ssm.doSimulate(ssini, Nsamples-1, ...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
209 A_kalman, Baos_kalman, Coutputs_kalman, Cstates_kalman, Daos_kalman, Bnoise_kalman, Dnoise_kalman, ...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
210 Bcst_kalman, Dcst_kalman, aos_vect_kalman, doTerminate, terminationCond, displayTime, timestep, forceComplete);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
211
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
212 y = [Coutputs_kalman*lastX y];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
213
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
214 %% saving in aos
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
215 fs = 1/timestep;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
216 isysStr = sys.name;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
217 tini = find(pl, 'tini');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
218 if isa(tini,'double')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
219 tini = time(tini);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
220 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
221
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
222 ao_out = ao.initObjectWithSize(1, NoutputsOut + NstatesOut);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
223 for ii=1:NstatesOut
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
224 ao_out(ii).setData(tsdata( y(ii,:), fs ));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
225 ao_out(ii).setName(['kalman estimate of ' sys_est.outputs(1).ports(ii).name]);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
226 ao_out(ii).setXunits('s');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
227 ao_out(ii).setYunits(sys_est.outputs(1).ports(ii).units);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
228 ao_out(ii).setDescription(...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
229 ['Kalman estimate for ' isysStr, ' : ', sys_est.outputs(1).ports(ii).name,...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
230 ' ' sys_est.outputs(1).ports(ii).description]);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
231 ao_out(ii).setT0(tini);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
232 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
233 for ii=1:NoutputsOut
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
234 ao_out(NstatesOut+ii).setData(tsdata( y(NstatesOut+ii,:), fs ));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
235 ao_out(NstatesOut+ii).setName(['kalman estimate of ' sys_est.outputs(2).ports(ii).name]);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
236 ao_out(NstatesOut+ii).setXunits('s');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
237 ao_out(NstatesOut+ii).setYunits(sys_est.outputs(2).ports(ii).units);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
238 ao_out(NstatesOut+ii).setDescription(...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
239 ['Kalman estimate for ' isysStr, ' : ', sys_est.outputs(2).ports(ii).name, ...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
240 ' ' sys_est.outputs(2).ports(ii).description]);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
241 ao_out(NstatesOut+ii).setT0(tini);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
242 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
243
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
244 %% construct output matrix object
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
245 out = matrix(ao_out);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
246 if callerIsMethod
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
247 % do nothing
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
248 else
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
249 myinfo = getInfo('None');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
250 out.addHistory(myinfo, pl , ssm_invars(1), inhist );
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
251 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
252
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
253 %% construct output analysis object
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
254 plist_out = plist('process covariance', Qn, 'readout covariance', Rn, ...
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
255 'state covariance', P, 'output covariance', Z, 'Kalman gain', K );
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
256
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
257 %% Set output depending on nargout
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
258 if nargout == 1;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
259 varargout = {out};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
260 elseif nargout == 2;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
261 varargout = {out plist_out};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
262 elseif nargout == 0;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
263 iplot(ao_out);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
264 else
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
265 error('Wrong number of outputs')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
266 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
267 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
268
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
269 %--------------------------------------------------------------------------
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
270 % Get Info Object
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
271 %--------------------------------------------------------------------------
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
272 function ii = getInfo(varargin)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
273
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
274 if nargin == 1 && strcmpi(varargin{1}, 'None')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
275 sets = {};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
276 pl = [];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
277 else
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
278 sets = {'Default'};
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
279 pl = getDefaultPlist;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
280 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
281 % Build info object
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
282 ii = minfo(mfilename, 'ssm', 'ltpda', utils.const.categories.op, '$Id: kalman.m,v 1.51 2011/04/17 21:28:05 adrien Exp $', sets, pl);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
283 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
284
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
285 %--------------------------------------------------------------------------
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
286 % Get Default Plist
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
287 %--------------------------------------------------------------------------
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
288 function pl = getDefaultPlist()
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
289 pl = ssm.getInfo('reorganize', 'for kalman').plists;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
290 pl.remove('set');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
291
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
292 p = param({'covariance', 'The covariance of this noise between input ports for the <i>time-discrete</i> noise model.'}, []);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
293 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
294
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
295 p = param({'CPSD', 'The one sided cross-psd of the white noise between input ports.'}, []);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
296 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
297
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
298 p = param({'aos', 'An array of input AOs (experimental stimuli).'}, ao.initObjectWithSize(1,0));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
299 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
300
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
301 p = param({'constants', 'Array of DC values for the different corresponding inputs.'}, paramValue.DOUBLE_VALUE(zeros(0,1)));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
302 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
303
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
304 p = param({'known outputs', 'Array of AOs for the different corresponding outputs (experiment measurements).'}, ao.initObjectWithSize(1,0));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
305 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
306
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
307 p = param({'Nsamples', 'The maximum number of samples to simulate (AO length(s) overide this).'}, paramValue.DOUBLE_VALUE(inf));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
308 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
309
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
310 p = param({'ssini', 'A cell-array of vectors that give the initial position for simulation.'}, {});
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
311 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
312
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
313 p = param({'tini', 'The initial filtering time (seconds).'}, paramValue.DOUBLE_VALUE(0) );
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
314 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
315
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
316 p = param({'displayTime', 'Switch off/on the display'}, paramValue.TRUE_FALSE);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
317 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
318
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
319 p = param({'reorganize', 'When set to 0, this means the ssm does not need be modified to match the requested i/o. Faster but dangerous!'}, paramValue.TRUE_FALSE);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
320 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
321
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
322 p = param({'force complete', 'Force the use of the complete simulation code.'}, paramValue.FALSE_TRUE);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
323 pl.append(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
324
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
325
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
326 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
327
|