clear
echo on
%% Tutorial 9
%
% In this tutorial we demonstrate the use of Lagrange multipliers to
% calculate the critical point of a function subject to an equality
% constraint.  The idea behind Lagrange multipliers is that the objective
% function is augmented by the multipliers times the equality constraints,
% thus you satisfy the equality constraints when the gradient of the
% augmented objective function (including the derivative with respect to
% the Lagrange multipliers) is zero.  We can thus solve the problem using
% fsolve or other multidimensional solver.

%% Part 1: Definition of the objective function and multipliers.
% We shall use as our example finding the maximum of the function:
% 
% F = x(1)^2 + 2*x(2)^2 + 3*x(3)^2
%
% but we shall restrict this to the surface of a sphere of radius 3:
%
% g = x(1)^2 + x(2)^2 + x(3)^2 - 9
%
% which should be equal to zero.  We thus introduce a new variable x(4),
% and define Fstar:
%
% Fstar = F + x(4)*g
%
% and thus we simply look for the critical points of Fstar!

F = @(x) x(1)^2 + 2*x(2)^2 + 3*x(3)^2;
g = @(x) x(1)^2 + x(2)^2 + x(3)^2 - 9;
Fstar = @(x) F(x) + x(4)*g(x);

%% Part 2: Determining the gradient:
% We need to take the gradient of Fstar with respect to all the variables
% (including the Lagrange multiplier x(4)).  We can do this numerically if
% we do it in a function, or (since we are doing it here in a script) we do
% it using a center difference anonymous function.  Note that the last row
% is just g(x), the equality constraint.

dx = 1e-6;
gradfstar = @(x) [(Fstar(x+dx*[1;0;0;0])-Fstar(x-dx*[1;0;0;0]))/2/dx; ...
                  (Fstar(x+dx*[0;1;0;0])-Fstar(x-dx*[0;1;0;0]))/2/dx; ...
                  (Fstar(x+dx*[0;0;1;0])-Fstar(x-dx*[0;0;1;0]))/2/dx; ...
                   g(x)];

% we could have done it analytically too, as the expression is really
% simple.  In that case we would have had:
%
% gradfstar = @(x) [2*x(1)+x(4)*2*x(1); 2*2*x(2)+x(4)*2*x(2); ...
%    3*2*x(3)+x(4)*2*x(3); g(x)];
%
% Note that the first three rows contain the derivatives of g(x) with
% respect to the first three variables too - don't forget that!

%% Part 3: Determining the critical point:
% The critical point is obtained by starting the iterative procedure off at
% some initial point and using fsolve or other iterative root finding
% algorithm:

x0=[1;2;3;-4]

x=fsolve(gradfstar,x0)

% As you can see, we converge to the vector [0;0;3] for the first three
% elements of x (which was the solution by inspection, actually).  The last
% element of x is just the value of the Lagrange multiplier at the
% solution.

% We can also look at the value of the gradient and functions:

g(x) %Which is satisfied

F(x) %The expected value of 27

gradfstar(x) %Which is zero.

%Note that this goes to a critical point, which can be either a minimum, a
%maximum, or even a saddle point!  You would need to do further checking to
%see which it is, and you need to start close to the desired value to
%converge to what you are looking for!  You would use exactly the same code
%to find the minimum of F on a circle - starting closer to the critical
%point [3;0;0;-1] would converge to the minimum!

echo off