examples/mctest3.m

   1 function [xerr, P] = mctest2(type, N, M)\r
   2 % Just testscript\r
   3 \r
   4 % Toolbox for nonlinear filtering.\r
   5 % Copyright (C) 2005  Gustaf Hendeby, Jakob Rosén\r
   6 %\r
   7 % This program is free software; you can redistribute it and/or\r
   8 % modify it under the terms of the GNU General Public License\r
   9 % as published by the Free Software Foundation; either version 2\r
  10 % of the License, or (at your option) any later version.\r
  11 %\r
  12 % This program is distributed in the hope that it will be useful,\r
  13 % but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  14 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  15 % GNU General Public License for more details.\r
  16 %\r
  17 % You should have received a copy of the GNU General Public License\r
  18 % along with this program; if not, write to the Free Software\r
  19 % Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.\r
  20 \r
  21 T = 1;\r
  22 f = [1 0 T 0;\r
  23      0 1 0 T;\r
  24      0 0 1 0;\r
  25      0 0 0 1];\r
  26 \r
  27 gw = [0.5*T^2 0;\r
  28       0 0.5*T^2;\r
  29       T 0;\r
  30       0 T];\r
  31 \r
  32 %h={'x1','x2','x4'};\r
  33 h=[1 0 0 0;\r
  34    0 1 0 0;\r
  35    0 0 0 1];\r
  36 \r
  37 w_var=[1 1];\r
  38 e_var=[0.25 0.2 0.1];\r
  39 \r
  40 %model=danl(f, gw, [], h, [], [0 0 0 0]',...\r
  41 %           sqrt(w_var), sqrt(e_var), [], 1, {'x1','x2','x3','x4'});\r
  42 model=danl(f, gw, [], h, [], [0 0 0 0]',...\r
  43            w_var, e_var, [], 1, {'x1','x2','x3','x4'});\r
  44 \r
  45 %model=danl(f, gw, [], h, [], [0 0 0 0]',...\r
  46 %           sqrt(w_var), sqrt(e_var), [1 1 1 1], 1, {'x1','x2','x3','x4'});\r
  47 \r
  48 \r
  49 xerr = zeros(4, N, M);\r
  50 P = zeros(4, 4, N, M);\r
  51 \r
  52 tic\r
  53 for i = 1:M\r
  54     fprintf('MC run %g...\n', i);\r
  55     % Initialize new objects to get entirely new random data\r
  56     simobj=simulator(model);\r
  57     if strcmpi(type, 'EKF')\r
  58         filterobj = ekf(model);\r
  59     elseif strcmpi(type, 'SIR')\r
  60         filterobj = sir(model);\r
  61     end\r
  62     simobj = simulate(simobj,N);\r
  63     filterobj = filter(filterobj, simobj);\r
  64     \r
  65     xerr(:,:, i) = get(filterobj, 'xhat') - get(simobj, 'x');\r
  66 %    P(:,:, :, i) = reshape(cell2mat(get(filterobj, 'Phat')), 4, 4, N);\r
  67     P(:,:, :, i) = get(filterobj, 'Phat');\r
  68 \r
  69 end\r
  70 toc\r
  71 \r
  72 \r
  73 %disp('Last P');\r
  74 %disp(P(:,:,end,1));\r
  75 %disp('Stationary P:');\r
  76 [Lbar Ppbar Pfbar]=dlqe(f, gw, h, diag(w_var), diag(e_var));\r
  77 %disp(Pfbar);\r
  78 \r
  79 \r
  80 MSE = mean(xerr.^2, 3);\r
  81 Pdiag = zeros(size(MSE));\r
  82 for i=1:4\r
  83     Pdiag(i,:) = squeeze(mean(P(i,i, :, :), 4))';\r
  84 end\r
  85 \r
  86 figure(1);\r
  87 clf; hold on;\r
  88 for i = 1:4\r
  89     subplot(2, 2, i); hold on;\r
  90     plot(MSE(i,:), 'b-');\r
  91     plot(Pdiag(i,:), 'r:');\r
  92 end\r
  93 legend('MSE', 'P');\r
  94    \r
  95 figure(2);\r
  96 clf; hold on;\r
  97 plot(sum(MSE, 1), 'b-');\r
  98 plot(sum(Pdiag, 1), 'r:');\r
  99 legend('MSE', 'P');\r
 100 \r