% A variation of the msi_ml_estimator script, where we use two
% parameters instead of one.

M = @(a, b) sin(a + pi/6 * [0 1 2]') + b

% Note, if you want to see this script produce useful estimates,
% then the data would have to be drawn from a distribution with
% a lot smaller variance. (That is: C should be smaller.)
C = diag(0.5^2 * [1 1 1])

S = sqrtm(C)

y = [-0.1, 0.6, 0.9]'
%y = M(0, 0) + S * randn(size(M(0, 0)))

R = @(a, b) inv(S) * (M(a, b) - y)

p = @(a, b) exp(-0.5*sum(R(a, b).^2))

as = -pi:0.01:pi;
bs = -3:0.01:3;

ps = zeros(length(as), length(bs));
for i=1:length(as)
  for j=1:length(bs)
    ps(i,j) = p(as(i), bs(j));
  end
end

figure(1)
% Plot integral of p over all b values, as function of a
plot(as, sum(ps, 2))

figure(2)
% Plot integral of p over all a values, as function of b
plot(bs, sum(ps, 1))

figure(3)
% Plot p as function of a and b
contour(bs, as, ps)

[pmax, ix] = max(sum(ps, 2))
a = as(ix)

[pmax, ix] = max(sum(ps, 1))
b = bs(ix)

t = [a, b]'

J = @(t) inv(S) * [cos(a + pi/6 .* [0 1 2]'), [1 1 1]']
R2 = @(t) R(t(1), t(2))

t = t - J(t) \ R2(t)
t = t - J(t) \ R2(t)
t = t - J(t) \ R2(t)
t = t - J(t) \ R2(t)

C_est = (R2(t)'*R2(t))/(length(y)-2) * inv(J(t)'*J(t))

sigma_est = sqrt(diag(C_est))