Created
July 24, 2014 18:43
-
-
Save moorepants/3f122bbad42ca7e910f8 to your computer and use it in GitHub Desktop.
Parameter ID for a simple pendulum using direct collocation.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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:411424 | |
% 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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment