function mpc_3

% model predictive control demo 
%
% for a description of the problem see the slides on
%
% www.math.uni-bayreuth.de/~lgruene/mpccourse/
%
% Version 3: regional terminal constraints + terminal cost
%
% (C) Lars Gruene 2009


% output of optimizer (0=no output, 1=output)
output = 0;


% Parameter set 3: stabilizing regional constraints, norm^2 distance

% Initial value
x = [0 1/2];

% Optimization horizon
N=5; % 10, 8, 5

% distance function
dist = @norm2dist;

% u-weight
lambda = 1;

% lower bound for u
lb = zeros(1,N);

% upper bound for u
ub = 0.2*ones(1,N);

% constraints
nlconstr = @regionconstraint;

% terminal cost
terminal = @terminaldist; % also: terminal0

% initial guess for u
u = 0.2*ones(1,N);



% create figure and plot half ellipse and initial value
figure;
plot(sin(0:pi/20:pi), cos(0:pi/20:pi)/2,'Color',[0.8 0.8 0.8]);
hold on;
plot(0, -0.5,'ok','MarkerSize',8);
axis([-0.5 1.5 -1 1]);
axis square;
ht = 0;
plot(x(1),x(2),'or','MarkerFaceColor','r');


% define void constraints in optimization
A = [];
b = [];
Aeq = [];
beq = [];


% set optimizer options
options = optimset('LargeScale','off', ...
                   'MaxIter',inf,'MaxFunEvals',2000);
               
if (output==0)
    options = optimset(options, 'Display','off');
end;


% main loop
while 1
    
    % mouse button handling 
    [dummyx, dummyy, button] = ginput(1);
    if (button==3) 
        break
    end;
    
    
    % MPC optimization
    [uopt, gval] = fmincon(@(u) J(x,u,N,lambda,dist,terminal), u, ...
                    A, b, Aeq, beq, lb, ub, @(u) nlconstr(x,u,N), ...
                    options);
    x = f(x,uopt(1));
    u = [uopt(2:N),0.0];

    
    % plotting
    plot(x(1),x(2),'or','MarkerFaceColor','r');
    if (ht~=0) 
        delete(ht);
    end;
    ht=text(0.4,0.8,['F_N(x) = ',num2str(uopt(1))]);
end


% functional J_N(x,u)
function J = J(x,u,N,lambda,dist,terminal)

J = 0.0;
for i=1:N
    J = J + dist(x) + lambda*u(i)^2;
    x = f(x,u(i));
end;
J = J + terminal(x);
      
% distance function ||x-x^*||^2
function d = norm2dist(x)

d = x(1)^2+(x(2)+0.5)^2;
  
% modified distance function 
function d = betterdist(x)

d = x(1)^2+5*(x(2)+0.5)^2;
  
% dummy terminal cost  
function t = terminal0(x)

t = 0;

% terminal cost 3||x-x^*||^2
function t = terminaldist(x)

t = 3*norm2dist(x);

% dummy terminal constraint
function [c,ceq] = dummyconstraint(x,u,N)

ceq = 0;
c = -1;

% terminal constraint x(N)=x^*
function [c,ceq] = pointconstraint(x,u,N)

c = -1;
for i=1:N
    x = f(x,u(i));
end;
ceq = norm2dist(x);

% terminal constraint x_2(N)<0.1
function [c,ceq] = regionconstraint(x,u,N)

ceq = 0;

for i=1:N
    x = f(x,u(i));
end;
c = x(2)+0.1;


% dynamics of the system
function y = f(x,u)

xx = [x(1), 2*x(2)];

n = norm(xx,2);

if (n==0.0)  
    y=[0.0 0.0];
else
    phi = real(acos(xx(2)/n));
    if (xx(1)<0) 
        phi = 2*pi-phi;
    end;

    y(1) = n*sin(phi+u);
    y(2) = n*cos(phi+u)/2;
end;
