filters/@ekf/tupdate.m

   1 [obj, xpred, Ppred]=tupdate(obj, varargin)\r
   2 % Time update method for the EKF (Extended Kalman Filter)\r
   3 %\r
   4 % Syntax: (* = optional)\r
   5 %\r
   6 % [ekfobj, xpred, Ppred] = mupdate(ekfobj, u*, xhat*, Phat*, t*);\r
   7 %\r
   8 % In arguments:\r
   9 %\r
  10 % 1. ekfobj\r
  11 %       EKF filter object that will be used for the filtering\r
  12 % 2* u\r
  13 %       A matrix u(t) containg the deterministic data, for this particular step.\r
  14 % 2* []\r
  15 %       No u(t) will be used in the calculations.\r
  16 % 3* xhat\r
  17 %       Estimate of x\r
  18 % 3* []\r
  19 %       xhat will be set to ekfobj.xhat(:,end), ie the estimate of the last filter step.\r
  20 % 4* Phat\r
  21 %       Estimate of P\r
  22 % 4* []\r
  23 %       Phat will be set to ekfobj.Phat(:,:,end), ie the estimate of the last filter step.\r
  24 % 5* t\r
  25 %       The time of the actual filtering step. Ts will be set to this value\r
  26 % 5* []\r
  27 %       t will be set to ekfobj.t, and also Ts will be set to this value\r
  28 %\r
  29 % Out arguments:\r
  30 %\r
  31 % 1. ekfobj\r
  32 %       EKF object containg the result of the time update operation.\r
  33 % 2. xpred\r
  34 %       One-step prediction of x\r
  35 %       This can also be accessed from the object using the ekfobj.xpred property\r
  36 % 3. Ppred\r
  37 %       One-step prediction of P\r
  38 %       This can also be accessed from the object using the ekfobj.Ppred property\r
  39 %\r
  40 % For more help, type 'props(ekf)'\r
  41 \r
  42 % Toolbox for nonlinear filtering.\r
  43 % Copyright (C) 2005  Jakob Rosén <jakob.rosen@gmail.com>\r
  44 %\r
  45 % This program is free software; you can redistribute it and/or\r
  46 % modify it under the terms of the GNU General Public License\r
  47 % as published by the Free Software Foundation; either version 2\r
  48 % of the License, or (at your option) any later version.\r
  49 %\r
  50 % This program is distributed in the hope that it will be useful,\r
  51 % but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  52 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  53 % GNU General Public License for more details.\r
  54 %\r
  55 % You should have received a copy of the GNU General Public License\r
  56 % along with this program; if not, write to the Free Software\r
  57 % Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\r
  58 \r
  59 % Since ue defaults to [], we don't have to go through the long procedure with it\r
  60 if nargin>=2; ue=varargin{1}; else; ue=[]; end;\r
  61 \r
  62 % Declare the arguments\r
  63 x=[];\r
  64 P=[];\r
  65 t=[];\r
  66 \r
  67 % Fetch arguments, if they exist\r
  68 if nargin>=3; x=varargin{2}; end;\r
  69 if nargin>=4; P=varargin{3}; end;\r
  70 if nargin>=5; t=varargin{4}; end;\r
  71 \r
  72 % If an empty argument, or no argument at all, was supplied - use its default value!\r
  73 if isempty(x)\r
  74         x=obj.xhat(:,end);              % xhat default\r
  75 end\r
  76 \r
  77 if isempty(P)\r
  78         P=obj.Phat(:,:,end);            % Phat default\r
  79 end;\r
  80 \r
  81 if isempty(t)\r
  82         t=obj.t;                        % t default\r
  83 end\r
  84 \r
  85 % Time update block\r
  86 model=obj.model;\r
  87 \r
  88 Q=cov_w(model,t);\r
  89 F=fgradx_general(model, x, t, ue, 0);\r
  90 G=fgradw_general(model, x, t, ue, []);\r
  91 xpred=f_general(model, x, t, ue, 0);\r
  92 Ppred=F*P*F'+G*Q*G';\r
  93 \r
  94 Ts=t;\r
  95 t=t+get(model,'T');     % Update time\r
  96 \r
  97 % Are the history buffers enabled?\r
  98 if obj.historysize\r
  99         % Not supported yet.\r
 100         % A good solution is to write to the history at the time update. Investigate it...\r
 101         warning('This operation does not support history buffers');\r
 102 end\r
 103 \r
 104 % Store relevant variables in the object\r
 105 obj.xhat=x;\r
 106 obj.xpred=xpred;\r
 107 obj.y=y;\r
 108 obj.u=ue;\r
 109 obj.Phat=P;\r
 110 obj.Ppred=Ppred;\r
 111 obj.Ts=Ts;\r
 112 obj.t=t;\r