filters/@ekf/mupdate.m

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