function P = gaussdensity(hsvData, Mean, Covar) %get the gaussian density for each data point given % a mean and covariance [rows, cols] = size(hsvData); constant = 1 / (sqrt((2 * pi)^ cols) * det(Covar)); Diff = [hsvData(:,1) - Mean(1), hsvData(:,2) - Mean(2), hsvData(:,3) - Mean(3)]; FirstMult = Diff * inv(Covar); DiffTrans = Diff'; %multiply just the diagonals first = FirstMult(:,1) .* DiffTrans(1,:)'; second = FirstMult(:,2) .* DiffTrans(2,:)'; third = FirstMult(:,3) .* DiffTrans(3,:)'; Power = -.5 * (first + second + third); %Power = -.5 * (Diff * inv(Covar) * Diff'); %should be array of powers to raise %PowerDiag = diag(Power); P = constant * exp(Power);