plotsim.m

kalman滤波的matlab程序

function plotSim(T,S,cda,cdm,cm)

scrsz = get(0,'ScreenSize');
h = figure(2)
set(h,'Position',[1 scrsz(4)/2 scrsz(3)/2 scrsz(4)]);

% Level
pane = 1;
h = subplot(3,1,pane);
switch cda
	case 'C'
		plot(T.ts,S.Xc(1,:),T.ts,T.L.C)
	case 'F'
		plot(T.ts,S.Xc(1,:),T.ts,T.L.F)
	case 'S'
		plot(T.ts,S.Xc(1,:),T.ts,T.L.S)
	case 'FS'
		plot(T.ts,S.Xc(1,:),T.ts,T.L.FS)
	otherwise
		error('Unknown dynamic model type.');
end
min_x = 0;
max_x = T.stime;
min_y = 0;
max_y = T.L_max;
axis([min_x max_x min_y max_y]);
xlabel('t [second]');
ylabel('L [meter]');
title(sprintf('Actual (%s) vs. Estimated (%s)',cda,cdm));

% Covariance
pane = 2;
h = subplot(3,1,pane);
semilogy(T.ts,sqrt(squeeze(S.Pc(1,1,:))));
min_x = 0;
max_x = T.stime;
min_y = 0;
max_y = T.L_max;
axis([min_x max_x min_y max_y]);
xlabel('t [second]');
ylabel('sqrt(P) [meter]');
title(sprintf('Process deviation',cda,cdm));

% Measurement residuals
pane = 3;
h = subplot(3,1,pane);
switch cda
	case 'C'
		if (cm == 'L')
			plot(T.ts,T.m.l.C-S.Zp)
		else
			plot(T.ts,T.m.a.C-S.Zp)
		end
	case 'F'
		if (cm == 'L')
			plot(T.ts,T.m.l.F-S.Zp)
		else
			plot(T.ts,T.m.a.F-S.Zp)
		end
	case 'S'
		if (cm == 'L')
			plot(T.ts,T.m.l.S-S.Zp)
		else
			plot(T.ts,T.m.a.S-S.Zp)
		end
	case 'FS'
		if (cm == 'L')
			plot(T.ts,T.m.l.FS-S.Zp)
		else
			plot(T.ts,T.m.a.FS-S.Zp)
		end
	otherwise
		error('Unknown dynamic model type.');
end
min_x = 0;
max_x = T.stime;
min_y = 0;
max_y = T.L_max;
%axis([min_x max_x min_y max_y]);
xlabel('t [second]');
if (cm == 'L')
	ylabel('dL [meter]');
else
	ylabel('Angle [degree]');
end
title(sprintf('Measurement Residual (%s)',cm));

return