% Function to use sigma point theory to generate predicted observation
% values by a weighted sum of sigma points passed through the measurement
% model. It produces the mean and covar. matrix of the predicted
% observations.
% Author : Saurabh Prasad
% Last Modified : November 2, 2005

function [mu, sigma] = sigmawt(sigma,L)
lambda = 3 - L;

% Defining the weights:
W_m = zeros(1,(2*L+1));
W_c = zeros(1,(2*L+1));

W_m(1)=lambda/(L+lambda);
W_c(1)=(lambda/(L+lambda));

for j = 2 : 2*L+1
    W_m(j)=1/(2*(L+lambda));
    W_c(j)=1/(2*(L+lambda));
end;

% Compute the function Y = g(X) on the sigma points:
% Let the sample nonlinearity be y = x;
for i = 1 : L
    for j = 1 : 2*L+1
        Y(j,i) = sigma(j,i);
    end;
end;

% compute the weighted mean of the function on the sigma points
mu_y = zeros(1,L);
for i = 1 : 2*L+1
    for j = 1 : L
        mu_y(j) = mu_y(j) + W_m(i)*Y(i,j);
    end;
end;

P_y = zeros(L,L);
for i = 1 : 2*L+1
    P_y = P_y + W_c(i)*((Y(i,:)-mu_y)')*((Y(i,:)-mu_y));
end;

mu = mu_y;
sigma = P_y;
