kalman3.m

realize analysis and design for computer-controlled system

function [ret,x0,str,ts,xts]=Kalman3(t,x,u,flag);
%KALMAN3	is the M-file description of the SIMULINK system named KALMAN3.
%	KALMAN3 has a the following characteristics:
%		3 continuous states
%		3 discrete states
%		0 outputs
%		0 inputs
%		does not have direct feedthrough
%		3 sample times
%
%	The block-diagram can be displayed by typing: KALMAN3.
%
%	SYS=KALMAN3(T,X,U,FLAG) returns depending on FLAG certain
%	system values given time point, T, current state vector, X,
%	and input vector, U.
%	FLAG is used to indicate the type of output to be returned in SYS.
%
%	Setting FLAG=1 causes KALMAN3 to return state derivatives, FLAG=2
%	discrete states, FLAG=3 system outputs and FLAG=4 next sample
%	time. For more information and other options see SFUNC.
%
%	Calling KALMAN3 with a FLAG of zero:
%	[SIZES]=KALMAN3([],[],[],0),  returns a vector, SIZES, which
%	contains the sizes of the state vector and other parameters.
%		SIZES(1) number of states
%		SIZES(2) number of discrete states
%		SIZES(3) number of outputs
%		SIZES(4) number of inputs
%		SIZES(5) number of roots (currently unsupported)
%		SIZES(6) direct feedthrough flag
%		SIZES(7) number of sample times
%
%	For the definition of other parameters in SIZES, see SFUNC.
%	See also, TRIM, LINMOD, LINSIM, EULER, RK23, RK45, ADAMS, GEAR.

% Note: This M-file is only used for saving graphical information;
%       after the model is loaded into memory an internal model
%       representation is used.

% the system will take on the name of this mfile:
sys = mfilename;
new_system(sys)
simver(1.3)
if (0 == (nargin + nargout))
     set_param(sys,'Location',[71,184,579,524])
     open_system(sys)
end;
set_param(sys,'algorithm',     'RK-23')
set_param(sys,'Start time',    '0.0')
set_param(sys,'Stop time',     '999999')
set_param(sys,'Min step size', '0.0001')
set_param(sys,'Max step size', '10')
set_param(sys,'Relative error','1e-3')
set_param(sys,'Return vars',   '')
set_param(sys,'AssignWideVectorLines','on');


%     Subsystem  'Kalman3'.

new_system([sys,'/','Kalman3'])
set_param([sys,'/','Kalman3'],'Location',[173,542,730,746])

add_block('built-in/Demux',[sys,'/','Kalman3/Demux'])
set_param([sys,'/','Kalman3/Demux'],...
		'hide name',0,...
		'position',[360,59,405,101])

add_block('built-in/Outport',[sys,'/','Kalman3/xhat'])
set_param([sys,'/','Kalman3/xhat'],...
		'position',[485,65,505,85])

add_block('built-in/Mux',[sys,'/','Kalman3/Mux1'])
set_param([sys,'/','Kalman3/Mux1'],...
		'hide name',0,...
		'inputs','3',...
		'position',[435,58,465,92])

add_block('built-in/Discrete State-Space',[sys,'/','Kalman3/Discrete State-Space'])
set_param([sys,'/','Kalman3/Discrete State-Space'],...
		'A','[a11 a12 a13; a21 a22 a23; a31 a32 a33]',...
		'B','[b1 k1; b2 k2; b3 k3]',...
		'C','[1 0 0;0 1 0; 0 0 1; c1 c2 c3]',...
		'D','[0 0; 0 0; 0 0; 0 0]',...
		'Sample time','h')
set_param([sys,'/','Kalman3/Discrete State-Space'],...
		'position',[225,58,340,102])

add_block('built-in/Mux',[sys,'/','Kalman3/Mux'])
set_param([sys,'/','Kalman3/Mux'],...
		'hide name',0,...
		'inputs','2',...
		'position',[170,61,200,94])

add_block('built-in/Sum',[sys,'/','Kalman3/Sum'])
set_param([sys,'/','Kalman3/Sum'],...
		'hide name',0,...
		'inputs','+-',...
		'position',[105,86,125,119])

add_block('built-in/Zero-Order Hold',[sys,'/',['Kalman3/Zero-Order',13,'Hold']])
set_param([sys,'/',['Kalman3/Zero-Order',13,'Hold']],...
		'hide name',0,...
		'position',[45,29,80,61])

add_block('built-in/Inport',[sys,'/','Kalman3/y'])
set_param([sys,'/','Kalman3/y'],...
		'Port','2',...
		'position',[10,85,30,105])

add_block('built-in/Inport',[sys,'/','Kalman3/u'])
set_param([sys,'/','Kalman3/u'],...
		'position',[10,35,30,55])

add_block('built-in/Zero-Order Hold',[sys,'/',['Kalman3/Zero-Order',13,'Hold1']])
set_param([sys,'/',['Kalman3/Zero-Order',13,'Hold1']],...
		'hide name',0,...
		'position',[45,79,80,111])
add_line([sys,'/','Kalman3'],[205,80;220,80])
add_line([sys,'/','Kalman3'],[345,80;355,80])
add_line([sys,'/','Kalman3'],[410,95;420,95;420,150;90,150;100,110])
add_line([sys,'/','Kalman3'],[130,105;150,105;150,85;165,85])
add_line([sys,'/','Kalman3'],[410,85;430,85])
add_line([sys,'/','Kalman3'],[410,75;430,75])
add_line([sys,'/','Kalman3'],[410,65;430,65])
add_line([sys,'/','Kalman3'],[470,75;480,75])
add_line([sys,'/','Kalman3'],[35,45;40,45])
add_line([sys,'/','Kalman3'],[85,45;125,45;125,70;165,70])
add_line([sys,'/','Kalman3'],[35,95;40,95])
add_line([sys,'/','Kalman3'],[85,95;100,95])


%     Finished composite block 'Kalman3'.

set_param([sys,'/','Kalman3'],...
		'BackGround',6,...
		'Drop Shadow',4,...
		'position',[110,136,140,194])


%     Subsystem  'Robot'.

new_system([sys,'/','Robot'])
set_param([sys,'/','Robot'],'Location',[-7,279,404,460])

add_block('built-in/S-Function',[sys,'/','Robot/S-Function1'])
set_param([sys,'/','Robot/S-Function1'],...
		'hide name',0,...
		'Drop Shadow',4,...
		'function name','robmech',...
		'position',[145,63,200,87])

add_block('built-in/Demux',[sys,'/','Robot/Demux'])
set_param([sys,'/','Robot/Demux'],...
		'position',[225,59,265,91])

add_block('built-in/Inport',[sys,'/','Robot/v'])
set_param([sys,'/','Robot/v'],...
		'Port','2',...
		'position',[30,85,50,105])

add_block('built-in/Mux',[sys,'/','Robot/Mux2'])
set_param([sys,'/','Robot/Mux2'],...
		'hide name',0,...
		'Drop Shadow',4,...
		'inputs','2',...
		'position',[90,56,120,89])

add_block('built-in/Inport',[sys,'/','Robot/u'])
set_param([sys,'/','Robot/u'],...
		'position',[30,40,50,60])

add_block('built-in/Mux',[sys,'/','Robot/Mux'])
set_param([sys,'/','Robot/Mux'],...
		'inputs','3',...
		'position',[305,64,335,96])

add_block('built-in/Outport',[sys,'/','Robot/y'])
set_param([sys,'/','Robot/y'],...
		'position',[350,25,370,45])

add_block('built-in/Outport',[sys,'/','Robot/x'])
set_param([sys,'/','Robot/x'],...
		'Port','2',...
		'position',[350,70,370,90])
add_line([sys,'/','Robot'],[55,95;60,95;60,80;85,80])
add_line([sys,'/','Robot'],[125,75;140,75])
add_line([sys,'/','Robot'],[270,90;300,90])
add_line([sys,'/','Robot'],[270,70;300,70])
add_line([sys,'/','Robot'],[205,75;220,75])
add_line([sys,'/','Robot'],[270,80;300,80])
add_line([sys,'/','Robot'],[55,50;60,50;60,65;85,65])
add_line([sys,'/','Robot'],[270,60;295,60;295,35;345,35])
add_line([sys,'/','Robot'],[340,80;345,80])
set_param([sys,'/','Robot'],...
		'Mask Display','')


%     Finished composite block 'Robot'.

set_param([sys,'/','Robot'],...
		'Drop Shadow',4,...
		'position',[260,140,290,195])


%     Subsystem  'dstfb3'.

new_system([sys,'/','dstfb3'])
set_param([sys,'/','dstfb3'],'Location',[596,480,952,730])

add_block('built-in/Inport',[sys,'/','dstfb3/uc'])
set_param([sys,'/','dstfb3/uc'],...
		'position',[20,40,40,60])

add_block('built-in/Inport',[sys,'/','dstfb3/x'])
set_param([sys,'/','dstfb3/x'],...
		'Port','2',...
		'position',[20,140,40,160])

add_block('built-in/Demux',[sys,'/','dstfb3/Demux'])
set_param([sys,'/','dstfb3/Demux'],...
		'BackGround',6,...
		'hide name',0,...
		'outputs','3',...
		'position',[70,134,110,166])

add_block('built-in/Zero-Order Hold',[sys,'/','dstfb3/ZoH6'])
set_param([sys,'/','dstfb3/ZoH6'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Sample time','[h t0]',...
		'position',[140,135,180,165])

add_block('built-in/Gain',[sys,'/','dstfb3/l4'])
set_param([sys,'/','dstfb3/l4'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Gain','l2',...
		'position',[200,135,230,165])

add_block('built-in/Zero-Order Hold',[sys,'/','dstfb3/ZoH5'])
set_param([sys,'/','dstfb3/ZoH5'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Sample time','[h t0]',...
		'position',[140,85,180,115])

add_block('built-in/Sum',[sys,'/','dstfb3/Sum2'])
set_param([sys,'/','dstfb3/Sum2'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'inputs','+---',...
		'position',[280,82,300,128])

add_block('built-in/Zero-Order Hold',[sys,'/','dstfb3/ZoH4'])
set_param([sys,'/','dstfb3/ZoH4'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Sample time','[h t0]',...
		'position',[140,35,180,65])

add_block('built-in/Gain',[sys,'/','dstfb3/l3'])
set_param([sys,'/','dstfb3/l3'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Gain','l1',...
		'position',[200,85,230,115])

add_block('built-in/Gain',[sys,'/','dstfb3/l5'])
set_param([sys,'/','dstfb3/l5'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Gain','l3',...
		'position',[200,185,230,215])

add_block('built-in/Zero-Order Hold',[sys,'/','dstfb3/ZoH7'])
set_param([sys,'/','dstfb3/ZoH7'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Sample time','[h t0]',...
		'position',[140,185,180,215])

add_block('built-in/Outport',[sys,'/','dstfb3/u'])
set_param([sys,'/','dstfb3/u'],...
		'position',[330,95,350,115])

add_block('built-in/Gain',[sys,'/','dstfb3/l6'])
set_param([sys,'/','dstfb3/l6'],...
		'BackGround',6,...
		'hide name',0,...
		'Drop Shadow',4,...
		'Gain','lc',...
		'position',[200,35,230,65])
add_line([sys,'/','dstfb3'],[305,105;325,105])
add_line([sys,'/','dstfb3'],[45,50;135,50])
add_line([sys,'/','dstfb3'],[185,50;195,50])
add_line([sys,'/','dstfb3'],[235,50;255,50;255,90;275,90])
add_line([sys,'/','dstfb3'],[185,100;195,100])
add_line([sys,'/','dstfb3'],[185,150;195,150])
add_line([sys,'/','dstfb3'],[235,150;255,150;255,110;275,110])
add_line([sys,'/','dstfb3'],[235,100;275,100])
add_line([sys,'/','dstfb3'],[185,200;195,200])
add_line([sys,'/','dstfb3'],[235,200;265,200;275,120])
add_line([sys,'/','dstfb3'],[45,150;65,150])
add_line([sys,'/','dstfb3'],[115,140;125,140;135,100])
add_line([sys,'/','dstfb3'],[115,150;135,150])
add_line([sys,'/','dstfb3'],[115,160;125,160;135,200])


%     Finished composite block 'dstfb3'.

set_param([sys,'/','dstfb3'],...
		'BackGround',6,...
		'Drop Shadow',4,...
		'position',[190,125,220,180])

add_block('built-in/Step Fcn',[sys,'/','Step Input'])
set_param([sys,'/','Step Input'],...
		'hide name',0,...
		'Drop Shadow',4,...
		'Time','0',...
		'position',[35,65,55,85])


%     Subsystem  'Pulse'.

new_system([sys,'/','Pulse'])
set_param([sys,'/','Pulse'],'Location',[100,215,335,385])

add_block('built-in/Step Fcn',[sys,'/','Pulse/Step Input1'])
set_param([sys,'/','Pulse/Step Input1'],...
		'hide name',0,...
		'Time','tstep',...
		'After','height',...
		'position',[65,50,85,70])

add_block('built-in/Step Fcn',[sys,'/','Pulse/Step Input2'])
set_param([sys,'/','Pulse/Step Input2'],...
		'hide name',0,...
		'Time','tstep+width',...
		'After','height',...
		'position',[65,105,85,125])

add_block('built-in/Sum',[sys,'/','Pulse/Sum'])
set_param([sys,'/','Pulse/Sum'],...
		'inputs','+-',...
		'position',[145,75,165,95])

add_block('built-in/Outport',[sys,'/','Pulse/out_1'])
set_param([sys,'/','Pulse/out_1'],...
		'position',[205,75,225,95])
add_line([sys,'/','Pulse'],[170,85;200,85])
add_line([sys,'/','Pulse'],[90,60;105,60;105,80;140,80])
add_line([sys,'/','Pulse'],[90,115;115,115;115,90;140,90])
set_param([sys,'/','Pulse'],...
		'Mask Display','plot(0,0,100,100,[60,35,35,20,20,10],[20,20,80,80,20,20])',...
		'Mask Type','Pulse',...
		'Mask Dialogue','Pulse Generator.|Pulse width:|Pulse height:|Pulse start time:')
set_param([sys,'/','Pulse'],...
		'Mask Translate','width=@1;height=@2;tstep=@3;')
set_param([sys,'/','Pulse'],...
		'Mask Help','Pulse generator which ensures pulse\ntransitions are hit. Provides a vector of pulses when the height is entered as a vector.\nUnmask to see how it works.',...
		'Mask Entries','0.1\/-10\/25\/')


%     Finished composite block 'Pulse'.

set_param([sys,'/','Pulse'],...
		'Drop Shadow',4,...
		'position',[65,257,90,303])

add_block('built-in/To Workspace',[sys,'/','To Workspace'])
set_param([sys,'/','To Workspace'],...
		'hide name',0,...
		'Drop Shadow',4,...
		'mat-name','yout',...
		'buffer','10000',...
		'position',[425,87,475,103])

add_block('built-in/Clock',[sys,'/','Clock'])
set_param([sys,'/','Clock'],...
		'hide name',0,...
		'position',[270,30,290,50])

add_block('built-in/Mux',[sys,'/','Mux1'])
set_param([sys,'/','Mux1'],...
		'hide name',0,...
		'Drop Shadow',4,...
		'position',[365,72,395,118])
add_line(sys,[400,95;420,95])
add_line(sys,[295,40;325,40;325,80;360,80])
add_line(sys,[60,75;160,75;160,140;185,140])
add_line(sys,[225,155;255,155])
add_line(sys,[95,280;240,280;240,180;255,180])
add_line(sys,[60,75;235,75;235,90;360,90])
add_line(sys,[225,155;235,155;235,100;360,100])
add_line(sys,[295,155;325,155;325,110;360,110])
add_line(sys,[295,155;345,155;345,230;80,230;80,180;105,180])
add_line(sys,[225,155;235,155;235,100;90,100;90,150;105,150])
add_line(sys,[145,165;185,165])

drawnow

% Return any arguments.
if (nargin | nargout)
	% Must use feval here to access system in memory
	if (nargin > 3)
		if (flag == 0)
			eval(['[ret,x0,str,ts,xts]=',sys,'(t,x,u,flag);'])
		else
			eval(['ret =', sys,'(t,x,u,flag);'])
		end
	else
		[ret,x0,str,ts,xts] = feval(sys);
	end
else
	drawnow % Flash up the model and execute load callback
end