Mercurial > hg > ltpda

comparison m-toolbox/classes/@ssm/kalman.m @ 0:f0afece42f48

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