echo on
% 	In this demo we shall demonstrate how to use several the SITB to 
%	estimate paramaters in state-space models. We shall investigate 
%	data produced by a (simulated) dc-motor. We first load the data:

load dcm-data

% 	The matrix y contains the two outputs: y1 is the angular position of 
%	the motor shaft and y2 is the angular velocity. There are 400 data 
%	points and the sampling interval is 0.1 seconds. The input is 
% 	contained in the vector u. It is the input voltage to the motor.
%	Press a key for plots of the input-output data.
 
z=[y u];

pause,idplot(z,[],0.1,2),pause

clc

% 	We shall build a model of the dc-motor. The dynamics of the motor is 
%	well known. If we choose x1 as the angular position and x2 as the 
%	angular velocity it is easy to set up a state-space model of the 
%	following character neglecting disturbances:
%	(see page 84 in Ljung(1987):

%           | 0     1  |      | 0   |
%  d/dt x = |          | x  + |     | u
%           | 0   -th1 |      | th2 |
%
%
%          | 1  0 |
%     y =  |      | x
%          | 0  1 |  
%
%
% 	The parameter th1 is here the inverse time-constant of the motor and 
%	th2 is such that th2/th1 is the static gain from input to the angular
%	velocity. (See Ljung(1987) for how th1 and th2 relate to the physical 
%	parameters of the motor.)
% 	We shall estimate these two parameters from the observed data. Strike 
%	a key to continue.

pause

% 	1. FREE PARAMETERS
%
% 	We first have to define the structure in terms of the general 
%	description
%
% 	d/dt x = A x + B u + K e
%
%    	y   = C x + D u + e
%
% 	Strike a key to continue.

pause

%
% 	Any parameter to be estimated is entered as NaN (Not a Number). 
%	Thus we have
A=[0 1
   0 NaN];
B=[0
   NaN];
C=[1 0
   0 1];
D=[0
   0];
K=[0 0
   0 0];
X0=[0
    0];   %X0 is the initial value of the state vector; it could also be
          %entered as parameters to be identified.

% 	The model structure is now defined by

ms = modstruc(A,B,C,D,K,X0);pause     %	Strike a key to continue

% 	We shall produce an initial guess for the parameters. Let us guess 
%	that the time constant is one second and that the static gain is 0.28.
%	This gives:

th_guess = [-1 0.28];

% 	We now collect all this information into the "theta-format" by

dcmodel = ms2th(ms,'c',th_guess,[],0.1);

% 	'c' denotes that the parametrization refers to a continuous-time model
% 	The last argument, 0.1,is the sampling interval for the collected data

% 	The prediction error (maximum likelihood) estimate of the parameters 
%	is now computed by (Press a key to continue)

pause, dcmodel = pem(z,dcmodel); 
 
% 	We can display the result by the 'present' command.
% 	The imaginary parts denote standard deviation. 
%	Strike a key to continue.

pause, present(dcmodel), pause

% 	The estimated values of the parameters are quite close to those used 
%	when the data were simulated (-4 and 1).

% 	To evaluate the model's quality we can simulate the model with the 
%	actual input by and compare it with the actual output.
%	Strike a key to continue.

pause, compare(z,dcmodel); pause

% 	We can now, for example plot zeros and poles and their uncer-
% 	tainty regions. We will draw the regions corresponding to 10 standard
% 	deviations, since the model is quite accurate. Note that the pole at 
%	the origin is absolutely certain, since it is part of the model 
%	structure; the integrator from angular velocity to position. 
%	Strike a key to continue.

pause, zpplot(th2zp(dcmodel),10), pause

% 	2. COUPLED PARAMETERS
%
% 	Suppose that we accurately know the static gain of the dc-motor (from
% 	input voltage to angular velocity, e.g. from a previous step-response
% 	experiment. If the static gain is G, and the time constant of the 
%	motor is t, then the state-space model becomes
%
%            |0     1|    |  0  |
%  d/dt x =  |       |x + |     | u
%            |0  -1/t|    | G/t | 
%
%            |1   0|
%     y   =  |     | x
%            |0   1|
%

pause % Press a key to continue

% 	With G known, there is a dependance between the entries in the 
%	different matrices. In order to describe that, the earlier used way 
%	witn "NaN" will not be sufficient. We thus have to write an m-file 
%	which produces the A,B,C,D,K and X0 matrices as outputs, for each 
%	given parameter vector as input. It also takes auxiliary arguments as 
%	inputs, so that the user can change certain things in the model 
%	structure, without having to edit the m-file. In this case we let the 
%	known static gain G be entered as such an argument. The m-file that 
%	has been written has the name 'motor'. Press a key for a listing.

pause, type motor

pause % Press a key to continue.

% 	We now create a "THETA-structure" corresponding to this model 
%	structure:The assumed time constant will be

tc_guess = 1;

% 	We also give the value 0.25 to the auxiliary variable G (gain)
% 	and sampling interval

aux = 0.25;

dcmm = mf2th('motor','c',tc_guess,aux,[],0.1);

pause   % Press a key to continue

% 	The time constant is now estimated by

dcmm = pem([y u],dcmm);

% 	We have thus now estimated the time constant of the motor directly. 
%	Its value is in good agreement with the previous estimate. 
%	Strike a key to continue.

pause, present(dcmm),  pause

% 	With this model we may now proceed to test various aspects as before. 
%	The syntax of all the commands is identical to the previous case.


% 	3. MULTIVARIATE ARX-MODELS.
%
% 	The state-space part of the toolbox also handles multivariate (several
% 	outputs) ARX models. By a multivariate ARX-model we mean the 
%	following: Strike a key to continue.

pause

%  	A(q) y(t) = B(q) u(t) + e(t)
%
% 	Here A(q) is a ny | ny matrix whose entries are polynomials in the 
%	delay operator 1/q. The k-l element is denoted by a_kl(q):
%
%                          -1                   -nakl
% 	a_kl(q) = 1 + a-1 q    + .... + a-nakl q	
%
% 	It is thus a polynomial in 1/q of degree nakl.

% 	Similarily B(q) is a ny | nu matrix, whose kj-element is

%                      -nkkj         -nkkj-1                 -nkkj-nbkj
% 	b_kj(q) = b-0 q      +  b-1 q        + ... + b-nbkj q

% 	There is thus a delay of nkkj from input number j to output number k

% 	The most common way to create those would be to use the ARX-command.
% 	The orders are specified as
% 	nn = [na nb nk]
% 	with na being a ny | ny matrix whose kj-entry is nakj; nb and nk are
% 	defined anlogously.  Strike a key to continue.

pause

% 	Let's test some ARX-models on the dc-data. First we could simply build
% 	a general second order model:

na = [ 2 2; 2 2];
nb = [2; 2];
nk = [1;1];

dcarx1 = arx([y u],[na nb nk]);

% 	The result, dcarx1, is stored in the THETA-format, and all previous 
%	commands apply. We could for example explicitly determine the 
%	ARX-polynomials by 

[A,B] = th2arx(dcarx1);

%  	Strike a key to continue.

pause, A, B, pause

% 	We could also test a structure, where we know that y1 is obtained by
% 	filtering y2 through a first order filter. (The angle is the integral
% 	of the angular velocity). We could then also postulate a first order
% 	dynamics from input to output number 2:

na = [1 1; 0 1];
nb = [0 ; 1];
nk = [1 ; 1];

dcarx2 = arx(z,[na nb nk]);
dcarx2 = sett(dcarx2,0.1);   % Setting the sampling interval to 0.1 seconds
[Am, Bm] = th2arx(dcarx2);

Am, Bm

% 	Strike e key to continue

pause

% 	Finally, we could compare the bodeplots obtained from the input to
% 	output one for the different models by

g1 = th2ff(dcmodel); % The first calculated model
g2 = th2ff(dcmm);    % The second model (known dc-gain)
g3 = th2ff(dcarx2);  % The second arx-model

pause,bodeplot([g1 g2 g3]),pause

% 	The two first models are in more or less exact agreement. The 
%	arx-models are not so good, due to the bias caused by the non-white 
%	equation error noise. (We had white measurement noise in the 
%	simulations).
pause
set(gcf,'NextPlot','replace');

