linmod2.m

数字通信第四版原书的例程

function [A,B,C,D,apert,bpert,cpert,dpert]=linmod2(fun,x,u,para,apert,bpert,cpert,dpert,coeff1,coeff2,coeff3,coeff4,coeff5)   
%LINMOD2 Obtains linear models from systems of ODEs using an advanced method. 
%	LINMOD2 uses an advanced method to reduce truncation error.
%
%	[A,B,C,D]=LINMOD2('SFUNC') obtains the state space linear model of 
%	the system of ordinary differential equations described in the 
%	S-function 'SFUNC' when  the state variables and inputs are set 
%	to zero. 
%
%	[A,B,C,D]=LINMOD2('SFUNC',X,U) allows the state vector, X, and 
%	input, U, to be specified. A linear model will then be obtained
%	at this operating point. 
%
%	[A,B,C,D]=LINMOD2('SFUNC',X,U,PARA) allows a vector of parameters
%	to be set.  PARA(1) sets the smallest perturbation level 
%	that is allowed for obtaining the linear model (default PARA(1)=1e-8).
%	For systems that are functions of time PARA(2) may be set with the 
%	value of t at which the linear model is to be obtained (default t=0).
%
%	[A,B,C,D,APERT,BPERT,CPERT,DPERT]=LINMOD2('SFUNC',X,U,PARA) 
%	returns the set of perturbation sizes used to obtain the linear 
%	model for each of the corresponding state-space matrices. 
%
%	To see more help, enter TYPE LINMOD2.
%	See also LINMOD, SFUNC, DSFUNC, TRIM and the M-file LINMOD2.M.

%	Copyright (c) 1990-94 by The MathWorks, Inc.
%	Andrew Grace 11-21-90.
%	Based on ideas by James H. Taylor, GE Corporate R&D.
%	Ref: Linearization Algorithms and Heuristics for CACE, Porc. 
%	CACSD'92, Napa, California, March 17-19, 1992.

%
%	[A,B,C,D]=LINMOD2('SFUNC',X,U,PARA,APERT,BPERT,CPERT,DPERT) 
%	allows the perturbation levels for obtaining the 
%	levels for all of the elements of X and U to be set. 
%	The default is otherwise XPERT=PARA(1)+1e-3*PARA(1)*abs(X),
%	UPERT=PARA(1)+1e-3*PARA(1)*abs(U).
%
%	[A,B,C,D]=LINMOD2('SFUNC',X,U,PARA,XPERT,UPERT,COEFF) allows 
%	coefficients, COEFF, to be passed directly to SFUNC at each call from 
%	LINMOD2: [X,T]=SFUNC(X,T,U,FLAG,COEFF).
%	FLAG should be ignored by SFUNC.
 

% ---------------Options--------------------
eval(['sizes =',fun, ';']);
sizes=[sizes(:); zeros(6-length(sizes),1)];
nxz=sizes(1)+sizes(2); nu=sizes(4); ny=sizes(3);
nx=sizes(1);
if nargin<2, x=[]; end
if nargin<3, u=[]; end
if nargin<4, para=[]; end
if nargin<5, apert=[]; end
if nargin<6, bpert=[]; end
if nargin<7, cpert=[]; end
if nargin<8, dpert=[]; end
if isempty(u), u=zeros(nu,1); end
if isempty(x), x=zeros(nxz,1); end
if isempty(para) , para=[0;0]; end
if para(1)==0, para(1)=1e-8; end
if isempty(apert), apert=1e-2*ones(nx,nx); end
if isempty(bpert), bpert=1e-2*ones(nx,nu); end
if isempty(cpert), cpert=1e-2*ones(ny,nx); end
if isempty(dpert), dpert=1e-2*ones(ny,nu); end
if length(para)>1, t=para(2); else t=0; end

% ------------------String for eval--------------- 
outstr=[];
if  exist('coeff1')  % Put in coeffs
		for i=1:nargin-8
			outstr=[outstr,',coeff',num2str(i)];
		end
end
outstr=[outstr,')'];
fun1=['dx=',fun,'(t, x,u,1',outstr, ';'];
fun2=['y=',fun, '(t, x,u,3',outstr, ';'];

A=zeros(nxz,nxz); B=zeros(nxz,nu); C=zeros(ny,nxz); D=zeros(ny,nu);

% Initialization
y=0;
eval(fun1);
if ny > 0, eval(fun2); end
olddx=dx; oldu=u; oldy=y; oldx=x;

% ------------------ A Matrix ------------------
for i=1:nx
	for j=1:nx
% Try perturbing it at default level
		pert = apert(j,i);
		x(i)=x(i)+pert;
		eval(fun1);
 		Aterm = (dx(j)-olddx(j))./pert;
		x=oldx;
% Re-estimate perturbation level to minimize roundoff error
		pert = max(abs(1e-2*Aterm),para(1));
		x(i)=x(i)+pert;
		eval(fun1);
 		Aterm = (dx(j)-olddx(j))./pert;
		x=oldx;
		diff = 1e20;
% Start reducing perturbation size until is settles in on a value
		while abs(diff) > sqrt(eps)*abs(Aterm) + eps & pert > para(1)
			pert = pert/2;
			aold = Aterm;
			x(i)=x(i)+pert;
			eval(fun1);
 			Aterm = (dx(j)-olddx(j))./pert;
			x=oldx;
			diff = Aterm - aold;
		end
		apert(j,i) = pert;
		if pert <= para(1), diff = para(1); end
% Come from the other side to check for discontinuity
		pert = -pert;
		x(i)=x(i)+pert;
		eval(fun1);
 		Ao=(dx(j)-olddx(j))./pert;
		x=oldx;
		if abs(Ao - Aterm) > 100*abs(diff) + sqrt(eps) + eps*abs(Aterm)
			disp(['Warning: Discontinuity detected at A(',num2str(j),',', num2str(i),')']);
			A(j,i) = sign(Aterm)*Inf;
		else 
			A(j,i) = Aterm;
		end
	end
end

% ----------------- C Matrix ----------------------
for i=1:nx
	for j=1:ny
% Try perturbing it at default level
		pert = cpert(j,i);
		x(i)=x(i)+pert;
		eval(fun2);
 		Cterm = (y(j)-oldy(j))./pert;
		x=oldx;
% Re-estimate perturbation level to minimize roundoff error
		pert = max(abs(1e-2*Cterm),para(1));
		x(i)=x(i)+pert;
		eval(fun2);
 		Cterm = (y(j)-oldy(j))./pert;
		x=oldx;
		diff = 1e20;
% Start reducing perturbation size until is settles in on a value
		while abs(diff) > sqrt(eps)*abs(Cterm) + eps & pert > para(1)
			pert = pert/2;
			cold = Cterm;
			x(i)=x(i)+pert;
			eval(fun2);
 			Cterm = (y(j)-oldy(j))./pert;
			x=oldx;
			diff = Cterm - cold;
		end
		cpert(j,i) = pert;
		if pert <= para(1), diff = para(1); end
% Come from the other side to check for discontinuity
		pert = -pert;
		x(i)=x(i)+pert;
		eval(fun2);
 		Co=(y(j)-oldy(j))./pert;
		x=oldx;
		if abs(Co - Cterm) > 100*abs(diff) + sqrt(eps) + eps*abs(Cterm)
			disp(['Warning: Discontinuity detected in evaluating C(',num2str(j), ',', num2str(i),')']);
			C(j,i) = sign(Cterm)*Inf;
		else 
			C(j,i) = Cterm;
		end
	end
end

% ----------------- B Matrix ----------------------
for i=1:nu
	for j=1:nx
% Try perturbing it at default level
		pert = bpert(j,i);
		u(i)=u(i)+pert;
		eval(fun1);
 		Bterm = (dx(j)-olddx(j))./pert;
		u = oldu;
% Re-estimate perturbation level to minimize roundoff error
		pert = max(abs(1e-2*Bterm),para(1));
		u(i)=u(i)+pert;
		eval(fun1);
 		Bterm = (dx(j)-olddx(j))./pert;
		u = oldu;
		diff = 1e20;
% Start reducing perturbation size until is settles in on a value
		while abs(diff) > sqrt(eps)*abs(Bterm) + eps & pert > para(1)
			bold = Bterm;
			pert = pert/2;
			u(i)=u(i)+pert;
			eval(fun1);
 			Bterm = (dx(j)-olddx(j))./pert;
			u = oldu;
			diff = Bterm - bold;
		end
		bpert(j,i) = pert;
		if pert <= para(1), diff = para(1); end
% Come from the other side to check for discontinuity
		pert = -pert;
		u(i)=u(i)+pert;
		eval(fun1);
 		Bo = (dx(j)-olddx(j))./pert;
		u = oldu;
		if abs(Bo - Bterm) > 100*abs(diff) + sqrt(eps) + eps*abs(Bterm)
			disp(['Warning: Discontinuity detected in evaluating B(',num2str(j), ',', num2str(i),')']);
			B(j,i) = sign(Bterm)*Inf;
		else 
			B(j,i) = Bterm;
		end
	end
end

% ----------------- D Matrix ----------------------
for i=1:nu
	for j=1:ny
% Try perturbing it at default level
		pert = dpert(j,i);
		u(i)=u(i)+pert;
		eval(fun2);
 		Dterm = (y(j)-oldy(j))./pert;
		u=oldu;
% Re-estimate perturbation level to minimize roundoff error
		pert = max(abs(1e-2*Dterm),para(1));
		diff = 1e20;
% Start reducing perturbation size until is settles in on a value
		while abs(diff) > sqrt(eps)*abs(Dterm) + eps & pert > para(1)
			pert = pert/2;
			dold = Dterm;
			u(i)=u(i)+pert;
			eval(fun2);
 			Dterm = (y(j)-oldy(j))./pert;
			u=oldu;
			diff = Dterm - dold;
		end
		dpert(j,i) = pert;
		if pert <= para(1), diff = para(1); end
% Come from the other side to check for discontinuity
		pert = -pert;
		u(i)=u(i)+pert;
		eval(fun2);
 		Do = (y(j)-oldy(j))./pert;
		u=oldu;
		if abs(Do - Dterm) > 100*abs(diff) + sqrt(eps) + eps*abs(Dterm)
			disp(['Warning: Discontinuity detected in evaluating D(',num2str(j), ',', num2str(i),')']);
			D(j,i) = sign(Dterm)*Inf;
		else 
			D(j,i) = Dterm;
		end
	end
end