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