models/@danl/calc_x0.m

   1 function v=calc_x0(obj,N);\r
   2 % Calculates the stochastic initital state estimate, x0\r
   3 %\r
   4 % Syntax: (* = optional)\r
   5 %\r
   6 % x0 = calc_x0(model, N);\r
   7 %\r
   8 % In arguments:\r
   9 %\r
  10 % 1. model\r
  11 %       Model object\r
  12 % 2. N\r
  13 %       Amount of x0 estimates. Each estimate is represented by a column in the resulting\r
  14 %       matrix, ie x0(i,k) represents the initial state i of estimate k.\r
  15 %       Useful for particle filter where you need initial estimates for each particle.\r
  16 %       For algorithms that doesn't utilize particles, such as the EKF algorithm, 'N'\r
  17 %       is usually set to 1.\r
  18 %\r
  19 % Out arguments:\r
  20 %\r
  21 % 1. x0\r
  22 %       The stochastic initial state estimate\r
  23 \r
  24 % Toolbox for nonlinear filtering.\r
  25 % Copyright (C) 2005  Jakob Rosén <jakob.rosen@gmail.com>\r
  26 %\r
  27 % This program is free software; you can redistribute it and/or\r
  28 % modify it under the terms of the GNU General Public License\r
  29 % as published by the Free Software Foundation; either version 2\r
  30 % of the License, or (at your option) any later version.\r
  31 %\r
  32 % This program is distributed in the hope that it will be useful,\r
  33 % but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  34 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  35 % GNU General Public License for more details.\r
  36 %\r
  37 % You should have received a copy of the GNU General Public License\r
  38 % along with this program; if not, write to the Free Software\r
  39 % Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\r
  40 \r
  41 % Get the NON-STOCHASTIC state estimate from the model object\r
  42 x0=obj.x0;\r
  43 \r
  44 % Draw random data from the p0 object\r
  45 if isempty(obj.p0)\r
  46         % No p0 exists. Don't add any noise.\r
  47         % This situation is not possible in the current system, but is included for\r
  48         % consistency.\r
  49         p0=0;\r
  50 else\r
  51         % We have a p0 object. Draw from it!\r
  52         p0=random(obj.p0,0,1,N);\r
  53 end;\r
  54 \r
  55 % Now add the random data to the x0 scalar taken from the model object.\r
  56 if length(x0)>1\r
  57         % x0 is a vector. We need to use repmat to make the dimensions match.\r
  58         v=repmat(x0,1,N)+p0;\r
  59 else\r
  60         % x0 is a scalar. There are no problems with the dimensions, just add!\r
  61         v=x0+p0;\r
  62 end\r