mctest3.m

particle filter 粒子滤波器 matlab工具箱

function [xerr, P] = mctest2(type, N, M)
% Just testscript

% Toolbox for nonlinear filtering.
% Copyright (C) 2005  Gustaf Hendeby, Jakob Ros閚
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.

T = 1;
f = [1 0 T 0;
     0 1 0 T;
     0 0 1 0;
     0 0 0 1];

gw = [0.5*T^2 0;
      0 0.5*T^2;
      T 0;
      0 T];

%h={'x1','x2','x4'};
h=[1 0 0 0;
   0 1 0 0;
   0 0 0 1];

w_var=[1 1];
e_var=[0.25 0.2 0.1];

%model=danl(f, gw, [], h, [], [0 0 0 0]',...
%           sqrt(w_var), sqrt(e_var), [], 1, {'x1','x2','x3','x4'});
model=danl(f, gw, [], h, [], [0 0 0 0]',...
           w_var, e_var, [], 1, {'x1','x2','x3','x4'});

%model=danl(f, gw, [], h, [], [0 0 0 0]',...
%           sqrt(w_var), sqrt(e_var), [1 1 1 1], 1, {'x1','x2','x3','x4'});


xerr = zeros(4, N, M);
P = zeros(4, 4, N, M);

tic
for i = 1:M
    fprintf('MC run %g...\n', i);
    % Initialize new objects to get entirely new random data
    simobj=simulator(model);
    if strcmpi(type, 'EKF')
        filterobj = ekf(model);
    elseif strcmpi(type, 'SIR')
        filterobj = sir(model);
    end
    simobj = simulate(simobj,N);
    filterobj = filter(filterobj, simobj);
    
    xerr(:,:, i) = get(filterobj, 'xhat') - get(simobj, 'x');
%    P(:,:, :, i) = reshape(cell2mat(get(filterobj, 'Phat')), 4, 4, N);
    P(:,:, :, i) = get(filterobj, 'Phat');

end
toc


%disp('Last P');
%disp(P(:,:,end,1));
%disp('Stationary P:');
[Lbar Ppbar Pfbar]=dlqe(f, gw, h, diag(w_var), diag(e_var));
%disp(Pfbar);


MSE = mean(xerr.^2, 3);
Pdiag = zeros(size(MSE));
for i=1:4
    Pdiag(i,:) = squeeze(mean(P(i,i, :, :), 4))';
end

figure(1);
clf; hold on;
for i = 1:4
    subplot(2, 2, i); hold on;
    plot(MSE(i,:), 'b-');
    plot(Pdiag(i,:), 'r:');
end
legend('MSE', 'P');
   
figure(2);
clf; hold on;
plot(sum(MSE, 1), 'b-');
plot(sum(Pdiag, 1), 'r:');
legend('MSE', 'P');