Parameter ID for a simple pendulum using direct collocation.

pendsi.m

function pendsi;
% System identification on a simple pendulum system
% The problem was described and solved by shooting in Vyasarayani et al., Multibody Syst Dyn (2011) 26:411–424
% Here we use Direct Collocation

	global problem
	close all
	tic
	
	% run a simulation to generate data
	problem.N = 5000;				% number of samples
	duration = 50;
	noise = 0;					% we will add this much noise to the data
	problem.tgrid = linspace(0, duration, problem.N);
	problem.h = duration/(problem.N-1);		% time step between nodes
	y0 = [pi/6 ; 0];				% initial state
	parameters.p = 10;				% value for parameter p	
	[t,y] = ode23(@odefun, problem.tgrid, y0);
	problem.ymeasured = y(:,1);		% we measure only the state variable y1
	problem.ymeasured = problem.ymeasured + noise*randn(size(problem.ymeasured));

	% initializations
	problem.Ncon = 2*(problem.N-1);			% number of constraints due to discretized dynamics
	problem.Nvar = 2*problem.N+1;			% number of unknowns: y1,y2 at N nodes and one unknown system parameter
	problem.iy1 = 2*(1:problem.N)-1;	 	% where the y1 variables are located within the X vector
	
	% solve the identification problem with a random initial guess
	X0 = 100*randn(problem.Nvar,1);
	problem.label = 'from random initial guess';
	figure(1);
	solve(X0);
	
	% solve the identification problem with measured y1(t) in the initial guess
	X0 = zeros(problem.Nvar,1);
	X0(problem.iy1) = problem.ymeasured;		% use the measured y1(t) in the initial guess (but not y2(t))
	X0(end) = 0.1;								% initial guess for parameter p 
	problem.label = 'using measurements in initial guess';
	figure(2);
	solve(X0);
	
	% start of embedded functions
	%=========================================================
	function solve(X0)
		% solve the optimization problem with IPOPT
		MaxIterations = 1000;
		funcs.objective = @objfun;
		funcs.gradient  = @objgrad;
		funcs.constraints = @confun;
		funcs.jacobian    = @conjac;
		funcs.jacobianstructure = @conjacstructure;
		options.lb = [repmat([-1;-10],problem.N,1) ; -100];		% lower bounds for the unknowns X, -1 rad for y1, -10 rad/s for y2, -100 for p
		options.ub = [repmat([ 1; 10],problem.N,1) ;  100];		% upper bounds for the unknowns X
		options.cl = zeros(problem.Ncon,1);
		options.cu = zeros(problem.Ncon,1);	
		options.ipopt.max_iter = MaxIterations;
		options.ipopt.hessian_approximation = 'limited-memory';
		[X, info] = ipopt(X0,funcs,options);
		
		% plot results
		show(X, confun(X));
	end
	%========================================================================
	function ydot = odefun(t,y)
		% pendulum dynamics using model parameter p
		ydot = zeros(2,1);
		ydot(1) = y(2);
		ydot(2) = -parameters.p * sin(y(1));
	end
	
end			% end of function pendsi
%=========================================================
function F = objfun(X);
	% objective function: integral of squared differences between data and simulation
	global problem
	F = problem.h * sum((X(problem.iy1) - problem.ymeasured).^2);
end
%=========================================================
function G = objgrad(X);
	% gradient of the objective function coded in objfun
	global problem
	G = zeros(problem.Nvar,1);
	G(problem.iy1) = 2 * problem.h * (X(problem.iy1) - problem.ymeasured);
end
%=========================================================
function c = confun(X)
% constraint function (dynamics constraints)
	global problem
	
	% size of constraint vector
	c = zeros(problem.Ncon,1);

	% dynamics constraints
	p = X(end);			% use last element of X as the current estimate of parameter p
	iy = [1 2];		% index for state variables y within X
	h = problem.h;
	for i=1:problem.N-1
		y 		= X(iy);		% state variables at node i 
		ynext 	= X(iy+2);		% state variables at node i+1
		% use trapezoidal integration formula as dynamics constraint
		c(2*i-1) 	= (ynext(1) - y(1))/h - (ynext(2)+y(2))/2;				% dy1/dt - y2 = 0
		c(2*i)		= (ynext(2) - y(2))/h + p*(sin(ynext(1))+sin(y(1)))/2;	% dy2/dt + p*sin(y1) = 0 
		iy = iy + 2;
	end

	% show current iterate, every second
	if toc > 1.0
		show(X,c);
		tic;
	end
			
end
%=========================================================
function J = conjac(X)
	global problem
	
	% size of Jacobian
	J = spalloc(problem.Ncon, problem.Nvar, 9*(problem.N-1));			% there are 9 nonzero elements for each node

	% Jacobian of dynamics constraints
	p = X(end);			% use last element of X as the current estimate of parameter p
	iy = [1 2];		% index for state variables y within X
	h = problem.h;
	for i=1:problem.N-1
		y 		= X(iy);		% state variables at node i 
		ynext 	= X(iy+2);		% state variables at node i+1
		% c(2*i-1) 	= (ynext(1) - y(1))/h - (ynext(2)+y(2))/2;				% dy1/dt - y2 = 0
		J(2*i-1, iy(1)) 	= -1/h;
		J(2*i-1, iy(1)+2) 	= 1/h;
		J(2*i-1, iy(2)) 	= -0.5;			
		J(2*i-1, iy(2)+2) 	= -0.5;			
		% c(2*i)	= (ynext(2) - y(2))/h + p*(sin(ynext(1))+sin(y(1)))/2;	% dy2/dt + p*sin(y1) = 0 
		J(2*i, iy(2)) 		= -1/h;
		J(2*i, iy(2)+2) 	= 1/h;
		J(2*i, iy(1))		= p*cos(y(1))/2;			
		J(2*i, iy(1)+2)		= p*cos(ynext(1))/2;			
		J(2*i, end)			= (sin(ynext(1))+sin(y(1)))/2;		% derivative of this constraint violation with respect to parameter p
		iy = iy + 2;
	end

	
end
%=========================================================
function J = conjacstructure(X)
% determine structure of the Jacobian calculated by function conjac
	global problem
	
	J = spalloc(problem.Ncon, problem.Nvar, 9*(problem.N-1));

	% dynamics constraints
	iy = [1 2];		% index for state variables y within X
	for i=1:problem.N-1
		J(2*i-1, iy(1)) 	= 1;
		J(2*i-1, iy(1)+2) 	= 1;
		J(2*i-1, iy(2)) 	= 1;			
		J(2*i-1, iy(2)+2) 	= 1;			
		J(2*i, iy(2)) 		= 1;
		J(2*i, iy(2)+2) 	= 1;
		J(2*i, iy(1))		= 1;			
		J(2*i, iy(1)+2)		= 1;			
		J(2*i, end)			= 1;		
		iy = iy + 2;
	end
	
end
%============================================================
function show(X,c)
	global problem
	% plot the current solution
	x = X(problem.iy1);
	subplot(2,1,1);
	plot(problem.tgrid, x*180/pi, problem.tgrid, problem.ymeasured*180/pi);
	xlabel('time(s)');
	ylabel('angle(deg)');
	legend('simulated','measured');
	title(['estimated p = ' num2str(X(end)) ' ' problem.label] );

	subplot(2,1,2);plot(c);title('constraint violations');
end