% Defining a nonlinear model M
M = @(a) sin(a + pi/6 * [0 1 2]')

% Our noise covariance matrix
C = diag(0.5^2 * [1 1 1])

% Square root of the covariance
S = sqrtm(C)

% Measurements
y = [-0.1, 0.6, 0.9]'

% Alternatively, we can generate a "measurement"
% by using M(0) as the "ground truth" and adding
% random noise to it.
% (Uncomment the line below to expermient with
% estimating a from a different measurement.)
% y = M(0) + S * randn(size(M(0)))

% The optimally weighted residual function
R = @(a) inv(S) * (M(a) - y)

% Likelihood
% (During the lecure, I forgot the "0.5" below. Sorry about that! /Per)
p = @(a) exp(-0.5*sum(R(a).^2))

% A vector of values to try for the angle a
as = -pi:0.01:pi;

% Evaluate p for each element of the vector
% Note: Matlab automatically "broadcasts" opearations like exp, * and + across dimensions
ps = p(as);

% Alternatively, we could have written:
% ps = arrayfun(p, as);

% Another way would be:
% ps = zeros(size(as));
% for i=1:length(ps)
%    ps(i) = p(as(i));
% end

% Plot p as funcition of a
plot(as, ps)

% Find the maximum and its location:
[pmax, ix] = max(ps)
a = as(ix)

% The Jacobian of the R function:
J = @(a) inv(S) * cos(a + pi/6 .* [0 1 2]')

% Iterations of the Gauss-Newton algoritm:
a = a - J(a) \ R(a)
a = a - J(a) \ R(a)
a = a - J(a) \ R(a)
a = a - J(a) \ R(a)
% ...after a few iterations we should have a good estimate of the
% argmin of norm(R(a))^2

% Heuristic estimate of the standard deviation uncertainty
% in the estimated a.
% (Note that this does not depend on our estimate of measurement noise.)
sigma_est = sqrt( (R(a)'*R(a))/(length(y)-1) * inv(J(a)'*J(a)) )
