rmse.m

particle filter 粒子滤波器 matlab工具箱

function [simobj, cfilters]=rmse(varargin);
% RMSE method for the simulator class.
% Takes a simulator object and a filter, or a cell array of filters. Simulates data,
% filters it and calculates the RMSE. This is done a user-specified number of times, and
% the average RMSE is visualized.
%
% Syntax: (* = optional)
%
% rmse(simobj, filtarray, steps, runs*, u*, t*);
% rmse(simobj, filtarray, steps, runs*, u*, Ts*);
%
% In arguments:
%
% 1. simobj
%	simulator object that will be used for the filtering
% 2. filtarray
%	A filter object, or cell array of filter objects for use with the simulated data.
%	If a filter object is empty, the model assigned to the simulator object will be
%	assigned to the empty filter object.
% 3. steps
%	A positive integer containing the number of desired simulation steps.
%	These will be carried out, filtered and visualized. Note that no previous simulation or
%	filtered data (from the objects) will be used.
% 4* runs
%	The number of runs to perform before the average is calculated.
% 4* []
%	One run is performed.
% 5* u
%	A matrix u(t) containg deterministic data.
% 5* []
%	No u(t) will be used in the calculations.
% 6* Ts
%	A vector containing the desired sample points, where Ts(k) represents the
%	time of step k.
% 6* t
%	The time of the first simulation step. The discrete times of the succeeding steps will be
%	calculated using the T property of the assigned model.
%	The result is a generated sample point vector, Ts, that will be stored in the
%	simulator object.
% 6* []
%	't' will be set to simobj.t and the sample points, 'Ts', will be generated on
%	the basis of this.

% Toolbox for nonlinear filtering.
% Copyright (C) 2005  Jakob Ros閚 <[e-mail]>
%
% 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.

simobj=varargin{1};
if isempty(simobj)
	error('The simulator object has no model!');
end
cfilters=varargin{2};
steps=varargin{3};
T=get(get(simobj,'model'),'T');		% Get the discrete time of the model

if isa(cfilters,'cell')
	% A cell array was supplied. Calculate its length!
	filters=length(cfilters);
else
	% A single object was supplied. Make it into a cell array for compability!
	cfilters={cfilters};
	filters=1;
end

% Declare the arguments
runs=[];

% Fetch arguments, if they exist
if nargin>=4; runs=varargin{4}; end

% If an empty argument, or no argument at all, was supplied - use its default value!
if isempty(runs)
	runs=1;
end

% Since these arguments defaults to [], we don't have to go through the long procedure with them
if nargin>=5; u=varargin{5}; else; u=[]; end;

% Default t, Ts
t=simobj.t;			% Use t stored in the simulator object
Ts=(t:T:(t+(steps-1)*T))';	% Calculate sample points

% Fetch t or Ts, if they are supplied and non-empty
if nargin>=6;			% Ts or t was supplied by the user
	t=varargin{6};
	if length(t)>1
		if length(t)~=steps
			error('Incorrect size of supplied Ts vector');
		else
			% A sample point vector Ts was supplied. Use it!
			if size(t,2)>1	% Ts is a row vector (we expect only one-dimensional vectors)
				Ts=t';	% Transpose and copy it!
			else
				Ts=t;	% Ts is a column vector. Just copy it!
			end
		end
	elseif length(t)	% Don't overwrite old Ts if the new t=[]
		Ts=(t:T:(t+(steps-1)*T))'; % Generate new sample points with respect to t
	end
end

% Check if any filter object is empty. If so, use the model of the simulator object.
legendcell=cell(filters,1);
for j=1:filters
	if isempty(cfilters{j})
		filtobj=cfilters{j};	% set.m needs a named variable
		cfilters{j}=set(filtobj,'model',simobj.model);
	end;
	legendcell{j}=class(cfilters{j});
end		

% Start the action
hf=figure('Name','RMSE');
wbar = waitbar(0,['Run 1 of ',num2str(runs)]);

msevec=zeros(filters,steps);
for i=1:runs
        hold off;
	[y xtrue]=simulate(simobj,steps,u,Ts);
	for j=1:filters
		figure(wbar);
		drawnow
		waitbar((i-1)/runs,wbar,['Run ',num2str(i),' of ',num2str(runs)]);
		% The assignin construction introduces some "redundant" code, since
		% the filter command isn't able to operate on the cell array directly.
		filtobj=cfilters{j};
		[xhat xpred]=filter(filtobj,y,u,Ts);
		cfilters{j}=filtobj;
		figure(hf);
		msevec(j,:)=msevec(j,:)+sum((xhat-xtrue).^2,1);
		plot(Ts,sqrt(msevec(j,:)/i),getcolors(j));
                hold on;
%		drawnow;
		cfilters{j}=reset(cfilters{j});
	end;
	simobj=reset(simobj);
end;
rmsevec =  sqrt(msevec./runs);

figure(wbar);
waitbar(1,wbar,'Complete!');
drawnow;

figure(hf);
hold off;
%for j=1:filters
%	plot(Ts,rmsevec(j,:),getcolors(j));
%	hold on
%end
%FOR PRINTING
close(hf);
figure
plot(Ts,rmsevec(1,:),'b:',Ts,rmsevec(2,:),'r');

xlabel(sprintf('Average RMSE of %g runs', runs));

legend(legendcell{:})
hold off

close(wbar);