text:
function [xg, yg, zg, tg] = scimat_ndgrid(scimat, ri, ci, si, fi)
% SCIMAT_NDGRID  Generation of arrays for 2D to 4D SCIMAT image volumes.
%
% [XG, YG, ZG] = scimat_ndgrid(SCIMAT,RI,CI,SI,FI)
%
%   SCIMAT is a struct with an image volume (see "help scimat" for
%   details).
%
%   XG, YG, ZG are arrays with the x-, y-, z-coordinates of the voxels in
%   SCIMAT.
%
%   Note that XG values change with columns, and YG values change with
%   rows, to accommodate the usual coordinate frame convention.
%
% [XG, YG, ZG] = scimat_ndgrid(SCIMAT, RI, CI, SI, FI)
%
%   RI, CI, SI, FI are vectors of voxel indices (rows, columns, slices and frames).
%   The output grid will be generated only for the corresponding image block.
%   For example, RI=1:4, CI=3:6, SI=5:7 means that the grid of coordinates
%   will be generated only for the block of voxels (1:4, 3:6, 5:7).
%
% See also: ndgrid, scimat_index2world.

% Author(s): Ramon Casero <rcasero@gmail.com>,
% Ben Villard <b.016434@gmail.com>
% Copyright © 2010-2015 University of Oxford
% Version: 0.4.1
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
D = length(scimat.axis);
narginchk(1, 5);
nargoutchk(0, D);

% simplify notation
NR = size(scimat.data, 1);
NC = size(scimat.data, 2);
NS = size(scimat.data, 3);
NF = size(scimat.data, 4);

% defaults
if (nargin < 2 || isempty(ri))
    ri = 1:NR; % Creates indices of data length (rows).
end
if (nargin < 3 || isempty(ci))
    ci = 1:NC; % Creates indices of data length (columns).
end
if (nargin < 4 || isempty(si))
    si = 1:NS; % Creates indices of data length (number of slices).
end
if (nargin < 5 || isempty(fi))
    fi = 1:NF; % Creates indices of data length (number of time frames).
end

% generate 3D grid of coordinates. X and Y have to be swapped so that the
% grid has the right number of rows and columns
if (D == 2)

    [rg, cg] = ndgrid(ri, ci);
    xyzt = scimat_index2world([rg(:), cg(:)], scimat); % Converts indices to world coordinates
    xg = reshape(xyzt(:, 1), size(rg));
    yg = reshape(xyzt(:, 2), size(cg));

elseif (D == 3)

    [rg, cg, sg] = ndgrid(ri, ci, si);
    xyzt = scimat_index2world([rg(:), cg(:), sg(:)], scimat);% Converts indices to world coordinates
    xg = reshape(xyzt(:, 1), size(rg));
    yg = reshape(xyzt(:, 2), size(cg));
    zg = reshape(xyzt(:, 3), size(sg));

elseif (D == 4)
    [rg, cg, sg, fg] = ndgrid(ri, ci, si, fi);
    xyzt = scimat_index2world([rg(:), cg(:), sg(:), fg(:)], scimat);% Converts indices to world coordinates
    xg = reshape(xyzt(:, 1), size(rg));
    yg = reshape(xyzt(:, 2), size(cg));
    zg = reshape(xyzt(:, 3), size(sg));
    tg = reshape(xyzt(:, 4), size(fg));

else
    error('Wrong number of dimensions in scimat.data')
end

end

author: vigente

repo: gerardus

save_path: github-repos/MATLAB/vigente-gerardus


text:
% Demonstration for EKF using a random sine signal model.
%
% A Very simple demonstration for extended Kalman filter (EKF), which is
% used to track a random single-component sinusoid signal,
% which is modelled as x_k = a_k*sin(\theta_k), dtheta/dt = omega_k.
% The signal is also filtered with unscented Kalman filter (UKF) for
% comparison.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

clc;
disp('Filtering the signal with EKF...');

save_plots = 1;

% Measurement model and it's derivative
f_func = @ekf_sine_f;
h_func = @ekf_sine_h;
dh_dx_func = @ekf_sine_dh_dx;
d2h_dx2_func = @ekf_sine_d2h_dx2;

% Initial values for the signal.
f = 0;
w = 10;
a = 1;

% Number of samples and stepsize.
d = 5;
n = 500;
dt = d/n;
x = 1:n;

% Check the derivative of the measurement function.
der_check(h_func, dh_dx_func, 1, [f w a]');

% Dynamic state transition matrix in continous-time domain.
F = [0 1 0;
     0 0 0;
     0 0 0];

% Noise effect matrix in continous-time domain.
L = [0 0;
     1 0;
     0 1];

% Spectral power density of the white noise.
q1 = 0.2;
q2 = 0.1;
Qc = diag([q1 q2]);

% Discretize the plant equation.
[A,Q] = lti_disc(F,L,Qc,dt);

% Generate the real signal.
X = zeros(3, n);
X(:,1) = [f w a]';
for i = 2:n
   X(:,i) = A*X(:,i-1) + gauss_rnd([0 0 0]', Q);
end

% Generate the observations with Gaussian noise.
sd = 1;
R = sd^2;

Y = zeros(1,n);
Y_real = feval(h_func,X);
Y = Y_real + gauss_rnd(0,R,n);

plot(x,Y,'.',x,Y_real)

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM = zeros(size(M,1),size(Y,2));
PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(1),h_func);
   MM(:,k)   = M;
   PP(:,:,k) = P;
end

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM2 = zeros(size(M,1),size(Y,2));
PP2 = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update2(M,P,Y(:,k),dh_dx_func,d2h_dx2_func,R*eye(1),h_func);
   MM2(:,k)   = M;
   PP2(:,:,k) = P;
end

clf; clc;
disp('The filtering results using the 1st order EKF is now displayed')

% Project the estimates to measurement space
Y_m = feval(h_func, MM);
Y_m2 = feval(h_func, MM2);
plot(x,Y,'.', x,Y_real,'--',x,Y_m);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with extended Kalman filter.');

if save_plots
    print -dpsc demo2_f1.ps
end

clc;
disp('The filtering result using the 1st order EKF is now displayed.');
fprintf('Smoothing the estimates using the RTS smoother...');

[SM1,SP1] = erts_smooth1(MM,PP,A,Q);

[SM2,SP2] = etf_smooth1(MM,PP,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

[SM1_2,SP1_2] = erts_smooth1(MM2,PP2,A,Q);

[SM2_2,SP2_2] = etf_smooth1(MM2,PP2,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

fprintf('ready.\n');
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_s1 = feval(h_func, SM1);
plot(x,Y,'.', x, Y_real,'--',x,Y_s1);
legend('Measurements','Real signal','Smoothed estimate');
xlim([0 ceil(max(x))]);
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (RTS) smoother.']);

if save_plots
    print -dpsc demo2_f2.ps
end

clc;
disp('The smoothing results using the ERTS smoother is now displayed.');
disp(' ');
disp('Push any button to see the smoothing results of a ETF smoother.');
pause

Y_s2 = feval(h_func, SM2);
plot(x,Y,'.', x, Y_real,'--',x,Y_s2);
legend('Measurements','Real signal','Smoothed estimate');
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (Two Filter) smoother.']);
xlim([0 ceil(max(x))]);

if save_plots
    print -dpsc demo2_f3.ps
end

clc;
disp('The smoothing results using the ETF smoother is now displayed.');

Y_s1_2 = feval(h_func, SM1_2);
Y_s2_2 = feval(h_func, SM2_2);
% Errors.
EMM_Y = sum((Y_m-Y_real).^2)/n;
EMM_Y2 = sum((Y_m2-Y_real).^2)/n;
ESM1_Y = sum((Y_s1-Y_real).^2)/n;
ESM2_Y = sum((Y_s2-Y_real).^2)/n;
ESM1_2_Y = sum((Y_s1_2-Y_real).^2)/n;
ESM2_2_Y = sum((Y_s2_2-Y_real).^2)/n;

disp(' ');
fprintf('Filtering now with UKF...');

% In the rest the signal is filtered with UKF for comparison.

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
U_MM = zeros(size(M,1),size(Y,2));
U_PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with UKF
for k=1:size(Y,2)
   [M,P,X_s,w] = ukf_predict3(M,P,f_func,Q,R*eye(1),dt);
   [M,P] = ukf_update3(M,P,Y(:,k),h_func,R*eye(1),X_s,w,[]);
   U_MM(:,k)   = M;
   U_PP(:,:,k) = P;
end
[U_SM, U_SP] = urts_smooth1(U_MM,U_PP,f_func,Q,dt);

fprintf('ready.\n')
disp(' ');
disp('Push any button to see the filtering results.');
pause

Y_m_u = feval(h_func, U_MM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_u);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman filter.');

clc;
disp('The filtering results of a UKF are now displayed.')
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_m_su = feval(h_func, U_SM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_su);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman smoother (RTS).');

clc;
disp('The smoothing results of a ERTS smoother are now displayed');

UKF_EMM_Y = sum((Y_m_u-Y_real).^2)/n;
URTS_EMM_Y = sum((Y_m_su-Y_real).^2)/n;

disp(' ');
disp('Mean square errors of all estimates:');
fprintf('EKF1-MSE = %.4f\n',sqrt(EMM_Y));
fprintf('ERTS-MSE = %.4f\n',sqrt(ESM1_Y));
fprintf('ETF-MSE = %.4f\n',sqrt(ESM2_Y));
fprintf('EKF2-MSE = %.4f\n',sqrt(EMM_Y2));
fprintf('ERTS2-MSE = %.4f\n',sqrt(ESM1_2_Y));
fprintf('ETF2-MSE = %.4f\n',sqrt(ESM2_2_Y));
fprintf('UKF-RMSE = %.4f\n',sqrt(UKF_EMM_Y));
fprintf('URTS-RMSE = %.4f\n',sqrt(URTS_EMM_Y));

author: EEA-sensors

repo: ekfukf

save_path: github-repos/MATLAB/EEA-sensors-ekfukf


text:
%--- help for rsindex ---
%
% RSINDEX Relative Strength Index (RSI).
%
%  Syntax:
%
%    index = rsindex(Data)
%    index = rsindex(Data,WindowSize)
%
%  Description:
%
%    RSINDEX calculates the Relative Strength Index (RSI) from the series of
%    closing stock prices. By default, RSI values are based on a 14-period window.
%
%  Input Argument:
%
%    Data    - A vector, table, or timetable. For matrix input, Data is an
%              M-by-1 vector of closing prices. Timetables and tables with M
%              rows contain variables named 'Close' (case insensitive).
%
%  Optional Input Argument:
%
%    WindowSize          - Positive integer scalar indicating the moving window
%                          size for relative strength index. The default is 14.
%
%  Output Argument:
%
%    index   - Relative StrengthIndex with the same number of rows (M) and
%              type as the input data.
%
%  Note:
%    The RS factor is calculated by dividing the average of the gains by the
%    average of the losses within a specified period.
%
%          RS = (average gains) / (average losses)
%
%    Also, the first value of RSI, RSI(1), is a NaN in order to preserve the
%    dimensions of CLOSEP.
%
%  Example:
%               load SimulatedStock.mat
%               index = rsindex(TMW)
%               index = rsindex(TMW,14)
%
%    See also NEGVOLIDX, POSVOLIDX.
%
%    Reference: Murphy, John J., Technical Analysis of the Futures Market,
%               New York Institute of Finance, 1986, pp. 295-302
%
%    Reference page in Doc Center
%       doc rsindex
%
%    Other functions named rsindex
%
%       fints/rsindex
%

author: jmaih


text:
%  INTERNAL FUNCTION
%
%  Examples:
%
%  test=(date2serial(1990):date2serial(1995))
%  serial2date(test)
%
%  test=(date2serial('1990'):date2serial('1995'))
%  serial2date(test)
%
%  test=(date2serial('1990h1'):date2serial('1995h2'))
%  serial2date(test)
%
%  test=(date2serial('1990q1'):date2serial('1995q4'))
%  serial2date(test)
%
%  test=(date2serial('1990m1'):date2serial('1995m6'))
%  serial2date(test)
%


text:
function [results] = vdpgm(given_data, opts)
%
% function [results] = vdpgm(given_data, opts)
%

start_time = clock;
if nargin == 1
  opts = struct();
end
if issparse(given_data)
  given_data = full(given_data);
end
if ~ isfield(opts, 'algorithm')
  % algorithm can be one of 'vdp', 'bj', 'cdp', 'csb' and 'non_dp'
  % vdp : variational DP
  % bj : Blei and Jordan
  % cdp : collapsed Dirichlet prior
  % csb : collapsed stick-breaking
  % non_dp : variational Bayes for Gaussian mixtures
  opts.algorithm = 'vdp';
end
if ~ isfield(opts, 'collapsed_means')
  opts.collapsed_means = 0;
end
if ~ isfield(opts, 'do_sort')
  opts.do_sort = '0';
end
if ~ isfield(opts, 'get_q_of_z')
  opts.get_q_of_z = 0;
end
if ~ isfield(opts, 'weight')
  opts.weight = 1;
end
if ~ isfield(opts, 'get_log_likelihood')
  opts.get_log_likelihood = 0;
end
if ~ isfield(opts, 'use_kd_tree')
  opts.use_kd_tree = 1;
end
if ~ isfield(opts, 'threshold')
  opts.threshold = 1.0e-5;
end
if ~ isfield(opts, 'sis')
  opts.sis = 0;
end
if ~ isfield(opts, 'initial_depth')
  opts.initial_depth = 3;
end
if ~ isfield(opts, 'initial_K')
  opts.initial_K = 1;
end
if ~ isfield(opts, 'ite')
  opts.ite = inf;
end
if ~ isfield(opts, 'do_split')
  opts.do_split = 0;
end
if ~ isfield(opts, 'do_merge')
  opts.do_merge = 0;
end
if ~ isfield(opts, 'do_greedy')
  opts.do_greedy = 1;
end
if ~ isfield(opts, 'max_target_ratio')
  opts.max_target_ratio = 0.5;
end
if ~ isfield(opts, 'init_of_split')
  % 'pc', 'rnd', 'rnd_close' or 'close_f'
  opts.init_of_split = 'pc';
end
if ~ isfield(opts, 'recursive_expanding_depth')
  opts.recursive_expanding_depth = 2;
end
if ~ isfield(opts, 'recursive_expanding_threshold')
  opts.recursive_expanding_threshold = 1.0e-1;
end
if ~ isfield(opts, 'recursive_expanding_frequency')
  opts.recursive_expanding_frequency = 3;
end
if isfield(opts, 'seed')
  rand('state', opts.seed);
else
  seed = rand('state');
  results.seed = seed;
end

data.given_data = given_data;
if opts.use_kd_tree
  partitions = init_kdtree_partitions(given_data, opts.initial_depth);
  data.kdtree = partitions;
end

% the hyperparameters of priors
hp_prior = mk_hp_prior(data, opts);

if isfield(opts, 'hp_posterior')
  opts.use_kd_tree = 0;
  if opts.get_q_of_z
    results.q_of_z = mk_q_of_z(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if opts.get_log_likelihood
    results.log_likelihood = mk_log_likelihood(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if isfield(opts, 'test_data')
    results.predictive_posterior = log_predictive_dist(data, opts.q_of_z, opts.hp_posterior, ...
                                                      opts.hp_prior, opts);
  end
  return
end

if opts.sis > 0
  q_of_z = sequential_importance_sampling(data, hp_prior, opts);
elseif isfield(opts, 'q_of_z')
  q_of_z = opts.q_of_z;
else
  q_of_z = rand_q_of_z(data, opts.initial_K, opts);
end

hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
if opts.do_greedy
  [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
else
  [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
end

results.algorithm = opts.algorithm;
results.elapsed_time = etime(clock, start_time);
results.free_energy = free_energy;
results.hp_prior = hp_prior;
results.hp_posterior = hp_posterior;
results.K = length(hp_posterior.eta);
results.opts = opts;
if opts.get_q_of_z
  results.q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
  if opts.use_kd_tree
    results.q_of_z = mk_non_kdtree_q_of_z(data, results.q_of_z);
  end
end
if opts.get_log_likelihood
  results.log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
[D,N] = size(data.given_data);
K = size(hp_posterior.m, 2);
log_likelihood = zeros(K,N);
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
for c=1:K
  mu = hp_posterior.m(:,c);
  f = hp_posterior.eta(c) + 1 - D;
  Sigma = (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f * hp_posterior.B{c};
  log_likelihood(c,:) = log_no_w(E_pi(c)) + logmvtpdf(data.given_data, mu, f, Sigma);
end
log_likelihood = log_sum_exp(log_likelihood, 1); % 1 by N
log_likelihood = sum(log_likelihood, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_predictive_dist(data, q_of_z, hp_posterior, hp_prior, opts);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0, 1, size(sum_x,2))) ./ repmat(Nc, D, 1);
E_pi(find(Nc==0)) = 0;
n = size(opts.test_data, 2);
log_prob = zeros(size(m,2), n);
for c=1:size(m,2)
  f = hp_posterior.eta(c) + 1 - D;
  sigma = hp_posterior.B{c} * (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f;
  if E_pi(c) > 0
    log_prob(c,:) = log(E_pi(c)) + log_T_dist(opts.test_data, m(:,c), ...
                                              sigma, f);
  else
    log_prob(c,:) = -inf;
  end
end
log_prob = log_sum_exp(log_prob,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_T_dist(X, m, sigma, f);
% X is a D by n matrix
[D,n] = size(X);
diff = X-repmat(m,1,n);
log_prob = gammaln((f+D)/2) ...
    - D/2*log(f*pi) ...
    - gammaln(f/2) ...
    - 0.5*detln(sigma) ...
    - (f+D)/2*log(1 + sum( diff .* ((f*sigma) \ diff), 1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function quad_term = mk_quad_term(data, q_of_z, hp_prior, opts)
% calculate the quad_term in marginalizing out means
% quad_term : D by D by K
%
[N, K] = size(q_of_z);
D = size(data.given_data, 1);
Nc = sum(q_of_z, 1); % 1 by K
xi = Nc + hp_prior.xi0;
f0 = 1 ./ (xi.^2);
f1 = - 2 ./ (xi.^3);
f2 = 3 ./ (xi.^4);
v = q_of_z.*(1 - q_of_z); % N by K
c = v.*((1-q_of_z).^2 + q_of_z.^2);
q = v.*((1-q_of_z).^3 + q_of_z.^3);
p_x = data.given_data * q_of_z; % D by K
v_x = data.given_data * v;      % D by K
c_x = data.given_data * c;      % D by K
v_p_x = data.given_data * (q_of_z + v); % D by K
c_p_x = data.given_data * (q_of_z + c); % D by K
for t=1:K
  first_term = f0(t) ...
      *((repmat(v(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + p_x(:,t)*p_x(:,t)');
  second_term = f1(t) ...
      *((repmat(c(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + v_p_x(:,t)*v_p_x(:,t)' - v_x(:,t)*v_x(:,t)' - p_x(:,t)*p_x(:,t)');
  tmp = q(:,t) - 3*v(:,t).^2 + v(:,t)*sum(v(:,t));
  third_term = f2(t) ...
      *((repmat(tmp', D, 1).*data.given_data)*data.given_data' ...
        + c_p_x(:,t)*c_p_x(:,t)' - c_x(:,t)*c_x(:,t)' - p_x(:,t)*p_x(:,t)' ...
        + 2*v_x(:,t)*v_x(:,t)' ...
        + sum(v(:,t))*p_x(:,t)*p_x(:,t)');
  quad_term(:,:,t) = first_term + second_term + third_term;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
% returns E[log p(z_n|Z^-n)]_q(Z^-n)
% E_log_p_of_z_given_other_z : N by K
q_of_z = hp_posterior.q_of_z;
[N,K] = size(q_of_z);
[E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z); % N by K
use_variance = 1;
if isequal(opts.algorithm, 'cdp')
  if use_variance
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - 0.5*V_Nc_minus_n./((E_Nc_minus_n+hp_prior.alpha/K).^2) ...
        - log(N - 1 + hp_prior.alpha);
  else
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - log(N - 1 + hp_prior.alpha);
  end
elseif isequal(opts.algorithm, 'csb')
  E_Nc_minus_n_cumsum_geq = fliplr(cumsum(fliplr(E_Nc_minus_n), 2));
  q_of_z_cumsum_geq = fliplr(cumsum(fliplr(q_of_z), 2));
  dummy = q_of_z_cumsum_geq.*(1-q_of_z_cumsum_geq);
  v_Nc_cumsum_geq = repmat(sum(dummy, 1), N, 1) - dummy;

  E_Nc_minus_n_cumsum = E_Nc_minus_n_cumsum_geq - E_Nc_minus_n;
  q_of_z_cumsum = q_of_z_cumsum_geq - q_of_z;
  dummy = q_of_z_cumsum.*(1-q_of_z_cumsum);
  v_Nc_cumsum = repmat(sum(dummy, 1), N, 1) - dummy;

  if use_variance
    first_term = ...
        (log(1+E_Nc_minus_n) ...
         - 0.5*V_Nc_minus_n./((1+E_Nc_minus_n).^2) ...
         - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
         + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2));
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - 0.5*v_Nc_cumsum./((hp_prior.alpha+E_Nc_minus_n_cumsum).^2) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
            + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2);
    second_term = cumsum(dummy, 2) - dummy;
  else
    first_term = log(1+E_Nc_minus_n) - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    second_term = cumsum(dummy, 2) - dummy;
  end
  E_log_p_of_z_given_other_z = first_term + second_term;
else
  error('unsupported algorithm');
end
% Gaussian approximation may not give a proper distribution.
% note. E[log p] is not need to be proper

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z)
% returns E[Nc^-n]; the expected value of Nc-n; N by K
%         V[Nc^-n]; the variance of Nc-n;       N by K
% q_of_z : N by K
N = size(q_of_z, 1);
E_Nc_minus_n = repmat(sum(q_of_z, 1), N, 1) - q_of_z;
tmp = q_of_z.*(1-q_of_z);
V_Nc = sum(tmp, 1);
V_Nc_minus_n = repmat(V_Nc, N, 1) - tmp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
if isequal(opts.algorithm, 'cdp') | isequal(opts.algorithm, 'non_dp')
  E_pi = hp_posterior.tilde_alpha / sum(hp_posterior.tilde_alpha);
elseif isequal(opts.algorithm, 'bj')
  second_term = [0 cumsum(log(hp_posterior.gamma(2,:)) - log(sum(hp_posterior.gamma)),2)]; % 1 by K
  E_pi = exp([(log(hp_posterior.gamma(1,:)) ...
               - log(sum(hp_posterior.gamma,1)) ...
               + second_term(1:end-1)) ...
              , second_term(end)]); % 1 by K
elseif isequal(opts.algorithm, 'vdp')
  opts_tmp = opts;
  opts_tmp.algorithm = 'bj';
  E_pi = mk_E_pi(hp_posterior, hp_prior, opts_tmp);
  E_pi(end) = max(1 - sum(E_pi(1:end-1)), 0);
elseif isequal(opts.algorithm, 'csb')
  [N,K] = size(hp_posterior.q_of_z);
  a = 1 + hp_posterior.Nc; % 1 by K
  b = hp_prior.alpha + N - cumsum(hp_posterior.Nc, 2); % 1 by K
  first_term = a ./ (a+b);
  first_term(end) = 1;
  second_term = cumprod(b./(a+b)) ./ (b./(a+b));
  E_pi = first_term .* second_term;
else
  error('not supported algorithm')
end

%
% p_TSB(z_test|Z)
%
% \begin{align*}
% p_{\text{TSB}}(Z) =&
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t,Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \left\{
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \right\}
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t|Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \end{align*}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = sequential_importance_sampling(data, hp_prior, opts);
disp('start SIS.')
N = size(data.given_data, 2);
for r = 1:opts.sis
  I = randperm(N);
%   I = I(1:ceil(N*0.1));
  data_r.given_data = data.given_data(:,I);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   data_r_sub.given_data = data_r.given_data(:,1);
%   q_of_z = ones(1,opts.initial_K) / opts.initial_K;
%   hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   for n = 1:N
%     data_r_sub.given_data = data_r.given_data(:,1:n);
%     q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
%     hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  q_of_z = [];
  for n = 1:size(data_r.given_data,2)
    data_r_sub.given_data = data_r.given_data(:,1:n);
    q_of_z(n,:) = ones(1, opts.initial_K) / opts.initial_K;
    hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
    q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  hist(r).hp_posterior = hp_posterior;
  hist(r).free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
  [min_f, best_r] = min([hist(:).free_energy]);
  hp_posterior = hist(best_r).hp_posterior;
  disp(['SIS: ' num2str(r) ';  best f = ' num2str(min_f) ';  best Nc = ' num2str(hp_posterior.Nc)])
end
disp('SIS done.')
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function nonkdtree_q_of_z = mk_non_kdtree_q_of_z(data, q_of_z);
nonkdtree_q_of_z = zeros(size(data.given_data,2), size(q_of_z, 2)); % N*K
for a=1:length(data.kdtree);
  nonkdtree_q_of_z(data.kdtree(a).indices,:) = repmat(q_of_z(a,:), length(data.kdtree(a).indices), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
disp_status(free_energy, hp_posterior, opts);
while 1
  disp('finding the best one....')
  [new_free_energy, new_hp_posterior, new_data, c] = find_best_splitting(data, ...
                                                    hp_posterior, ...
                                                    hp_prior, opts);
  if c == -1
    break
  end
  disp(['finding the best one.... done.  component ' num2str(c) ' was split.'])
  disp_status(new_free_energy, new_hp_posterior, opts);
  [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                    new_hp_posterior, ...
                                                    hp_prior, opts, 1, opts.ite);
  if free_energy_improved(free_energy, new_free_energy, 0, opts) == 0
    break
  end
  free_energy = new_free_energy;
  hp_posterior = new_hp_posterior;
  data = new_data;
end
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, c] = find_best_splitting(data, ...
                                                  hp_posterior, ...
                                                  hp_prior, opts);
c_max = 10;
K = size(hp_posterior.m, 2);
candidates = find(hp_posterior.Nc>2);
if isempty(candidates)
  free_energy = 0;
  c = -1;
  return
end
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
new_free_energy = ones(1,max(candidates))*inf;
%%%%%%%%%%%%%%%%%%%%%
fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
%%%%%%%%%%%%%%%%%%%%%
for c = candidates(1:min(c_max, length(candidates)))
  disp(['splitting ' num2str(c) '...'])
  [new_data(c), new_q_of_z, info] = split(c, data, q_of_z, hp_posterior, hp_prior, opts, 1);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  new_c = info.new_c;
  relating_n = find(sum(new_q_of_z(:,[c new_c]),2) > 0.5);
  if isempty(relating_n)
    continue
  end
  new_K = size(new_q_of_z, 2);
  sub_q_of_z = new_q_of_z(relating_n, [c new_c new_K]);
  if opts.use_kd_tree
    sub_data.kdtree = new_data(c).kdtree(relating_n);
  else
    sub_data.given_data = new_data(c).given_data(:,relating_n);
  end
  sub_hp_posteior = mk_hp_posterior(sub_data, sub_q_of_z, hp_prior, opts);
  [sub_f, sub_hp_posteior, dummy, sub_q_of_z] = update_posterior2(sub_data, ...
                                                    sub_hp_posteior, ...
                                                    hp_prior, opts, 0, 10, 0);
  if size(sub_q_of_z,2) < 3
    continue
  end
  if length(find(sum(sub_q_of_z,1)<1.0e-10)) > 1
    continue
  end
  new_log_lambda = log_lambda;
  if opts.use_kd_tree
    updated_data.kdtree = new_data(c).kdtree(info.updated_I);
    new_log_lambda(info.updated_I,:) = mk_log_lambda(updated_data, hp_posterior, hp_prior, opts);
  end
  sub_log_lambda = mk_log_lambda(new_data(c), sub_hp_posteior, hp_prior, opts);
  insert_indices = [c new_c new_K:(new_K+size(sub_q_of_z,2)-3)];
  new_log_lambda(:,insert_indices) = sub_log_lambda;
  new_fc = fc;
  new_fc(insert_indices) = mk_E_log_q_p_eta(sub_data, sub_hp_posteior, hp_prior, opts);
  new_free_energy(c) = mk_free_energy(new_data(c), sub_hp_posteior, hp_prior, opts, new_fc, new_log_lambda);
  new_q_of_z(relating_n,:) = 0;
  new_q_of_z(relating_n,insert_indices) = sub_q_of_z;
  new_q_of_z_cell{c} = new_q_of_z;
end
[free_energy, c] = min(new_free_energy);
if isinf(free_energy)
  c = -1;
  return
end
data = new_data(c);
hp_posterior = mk_hp_posterior(data, new_q_of_z_cell{c}, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
%
% split:
% tdp: if an empty cluster exists, split is available.
%      otherwise, make a new empty cluster. =>  K<-K+1.
%
% non-dp: if
%
c_max = 3;
[free_energy, hp_posterior] = update_posterior2(data, hp_posterior, hp_prior, opts, 0, opts.ite);
while 1
  K = size(hp_posterior.m, 2);
  new_free_energy = inf;
  %%% split
  if ~ opts.do_split
    break
  end
  disp(['### start splitting'])
  [dummy, candidates_for_splliting] = sort(hp_posterior.Nc, 2, 'descend');
  candidates_for_splliting(find(hp_posterior.Nc(candidates_for_splliting)<1)) = [];
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  for c=candidates_for_splliting(1:min(c_max, length(candidates_for_splliting)))
    disp(['### start splitting k=' num2str(c) '...'])
    [new_data, new_q_of_z] = split(c, data, q_of_z, hp_posterior, hp_prior, opts);
    new_hp_posterior = mk_hp_posterior(new_data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, ...
                                                      opts, 1);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(['### splitting k=' num2str(c) ' improved the free energy.  ' ...
           num2str(free_energy) ' -> ' num2str(new_free_energy)])
      break
    else
      disp(['### splitting k=' num2str(c) ' did not improve the free energy.'])
    end
  end
  disp('### end splitting')
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    data = new_data;
    continue
  end
  if ~ opts.do_merge
    break
  end
  %%% merge
  disp(['### start merging'])
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  candidates_for_merging = mkcandidates_to_merge(q_of_z'); % 2*#pairs
  for i=1:min(c_max, size(candidates_for_merging,2))
    pair = candidates_for_merging(:,i);
    c1 = pair(1); c2 = pair(2);
    disp(sprintf('### start merging c1=%d, c2=%d ...', c1, c2))
    new_q_of_z = q_of_z;
    new_q_of_z(:,c1) = new_q_of_z(:,c1) + new_q_of_z(:,c2);
    new_q_of_z(:,c2) = zeros(size(new_q_of_z,1),1);
    new_hp_posterior = mk_hp_posterior(data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior] = update_posterior2(data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, opts);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(sprintf('### merging c1=%d, c2=%d improved the free energy.  %0.5g -> %0.5g', ...
                   c1, c2, free_energy, new_free_energy))
      break
    else
      disp(sprintf('### merging c1=%d, c2=%d did not improve the free energy.', c1, c2))
    end
  end
  disp(['### end merging'])
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    continue
  end
  break
end % while 1
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_p_of_x_given_c = mk_map_log_p_of_x_given_c(data, clusters, hp_posterior, opts);
% clusters: e.g [1:K]
[D,N] = size(data);
K = length(clusters);
log_p_of_x_given_c = zeros(K,N);
for i = 1:K
  c = clusters(i);
  m = hp_posterior.m(:,c);
  precision = hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  d = data - repmat(m, 1, N);
  log_p_of_x_given_c(i,:) = (-D*0.5)*log(2*pi)+0.5*detln(precision)-0.5*sum(d.*(precision*d),1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, info] = split(c, data, q_of_z, ...
                                              hp_posterior, hp_prior, opts, update_kdtree)
% q_of_z: N*K
if nargin < 7
  update_kdtree = 1;
end
new_data = data;
if opts.use_kd_tree & update_kdtree
  [dummy, indices] = max(q_of_z,[],2);
  target_partitions = find(indices==c)';
  if ~ isempty(target_partitions)
    m = [data.kdtree(target_partitions).mean];
    log_p_of_m_given_c = mk_map_log_p_of_x_given_c(m, c, hp_posterior, opts); % 1*|target_partitions|
    [dummy, I] = sort(log_p_of_m_given_c, 2, 'descend');
    target_partitions = target_partitions(I(1:ceil(length(I)*opts.max_target_ratio)));
    %   target_partitions = find(q_of_z(:,c)>0.01)';
    [new_data, q_of_z, updated_I] = expand_all_nodes(new_data, q_of_z, hp_posterior, ...
                                                     hp_prior, target_partitions, opts);
    info.updated_I = updated_I;
  else
    info.updated_I = [];
  end
end
if isequal(opts.init_of_split, 'pc')  % principal eigenvector
  if opts.use_kd_tree
    arg1_data = [new_data.kdtree(:).mean];
  else
    arg1_data = new_data.given_data;
  end
  dir = divide_by_principal_component(arg1_data, ...
                                      hp_posterior.B{c}/hp_posterior.eta(c), ...
                                      hp_posterior.m(:,c));
  q_of_z_c1 = zeros(size(q_of_z,1),1);
  q_of_z_c2 = q_of_z(:,c);
  I = find(dir>=0);
  q_of_z_c1(I) = q_of_z(I,c);
  q_of_z_c2(I) = 0;
else
  q_of_z_c = q_of_z(:,c);
  if isequal(opts.init_of_split, 'rnd')  % random
    r = rand(size(q_of_z,1),1);
  elseif isequal(opts.init_of_split, 'rnd_close')  % make close clusters
    r = 0.5 + (rand(size(q_of_z,1),1)-0.5)*0.01;
  elseif isequal(opts.init_of_split, 'close_f')  % one is almost zero.
    r = 0.98 + rand(size(q_of_z,1),1)*0.01;
  else
    init_of_split = opts.init_of_split
    error('unknown option')
  end
  q_of_z_c1 = q_of_z_c.*r;
  q_of_z_c2 = q_of_z_c.*(1-r);
end
new_q_of_z = zeros(size(q_of_z,1), size(q_of_z,2)+1);
new_q_of_z(:,[1:end-2 end]) = q_of_z;
new_q_of_z(:,c) = q_of_z_c1;
new_c = size(new_q_of_z, 2) - 1;
new_q_of_z(:,new_c) = q_of_z_c2;
info.new_c = new_c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, updated_I] = expand_all_nodes(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, ...
                                                  target_partitions, opts);
if size(target_partitions, 1) ~= 1
  error('target_partitions must be a row vector.')
end
new_data = data;
for a=target_partitions
  if isempty(data.kdtree(a).children)
    children = mk_children_of_partition(data.kdtree(a));
    data.kdtree(a).children = children;
  else
    children = data.kdtree(a).children;
  end
  if length(children) == 1
    continue
  end
  new_data.kdtree(a) = children(1);
  new_data.kdtree(end+1) = children(2);
end % while a <= length(data.kdtree)
updated_I = [target_partitions length(data.kdtree)+1:length(new_data.kdtree)];
sub_data.given_data = data.given_data;
sub_data.kdtree = new_data.kdtree(updated_I);
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts);
new_q_of_z = zeros(length(new_data.kdtree),size(q_of_z,2));
new_q_of_z(1:size(q_of_z,1),:) = q_of_z;
new_q_of_z(updated_I,:) = sub_q_of_z;
disp(['### building kd-tree done; #partition ' num2str(length(data.kdtree)) ...
      ' -> ' num2str(length(new_data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts);
kdtree_size = length(data.kdtree);
for a=1:length(data.kdtree)
  [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                    q_of_z, hp_posterior, ...
                                                    hp_prior, opts, ...
                                                    opts.recursive_expanding_depth);
end
[prob, best_c] = max(q_of_z, [], 2);
% for c=1:size(q_of_z,2);
%   I_n =
% end
disp(['### building kd-tree done; #partition ' num2str(kdtree_size) ...
      ' -> ' num2str(length(data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth);
% q_of_z : N*K
if depth == 0
  return
end
if isempty(data.kdtree(a).children)
  children = mk_children_of_partition(data.kdtree(a));
  data.kdtree(a).children = children;
else
  children = data.kdtree(a).children;
end
if length(children) == 1
  return
end
sub_data.given_data = data.given_data;
sub_data.kdtree = children;
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts); % 2*K
diff = sub_q_of_z - repmat(q_of_z(a,:), 2, 1);
if sum(sum(diff.*diff, 1), 2)/prod(size(sub_q_of_z)) < opts.recursive_expanding_threshold
  return
end
b = length(data.kdtree)+1;
data.kdtree(a) = children(1);
data.kdtree(b) = children(2);
q_of_z([a b],:) = sub_q_of_z;
[data, q_of_z] =  expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);
[data, q_of_z] =  expand_recursively_until_convergence2(data, b, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, q_of_z] = update_posterior2(data, hp_posterior, hp_prior, opts, upkdtree, ite, do_sort);
% update q_of_z: N*K
disp(['### updating posterior ...'])
free_energy = inf;
if nargin == 4
  upkdtree = 0;
end
if nargin < 6
  ite = inf;
end
if nargin < 7
  do_sort = 1;
end
i = 0;
last_Nc = 0;
start_sort = 0;
while 1
  i = i+1;
  [new_free_energy, log_lambda] = mk_free_energy(data, hp_posterior, hp_prior, opts);
  disp_status(new_free_energy, hp_posterior, opts);
  if (~isinf(ite) && i>=ite) || ...
        (isinf(ite) && free_energy_improved(free_energy, new_free_energy, 0, opts) == 0)
    free_energy = new_free_energy;
    if do_sort && opts.do_sort && ~ start_sort
      start_sort = 1;
    else
      break
    end
  end
  last_Nc = hp_posterior.Nc;
  free_energy = new_free_energy;
  [q_of_z, data] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
  freq = opts.recursive_expanding_frequency;
  if opts.use_kd_tree & upkdtree & (freq==1 | mod(i,freq)==1)
    [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                      q_of_z, ...
                                                      hp_posterior, ...
                                                      hp_prior, opts);
  end
  % check if the last component is small enough
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end)) > 1.0e-20
    q_of_z(:,end+1) = 0;
  end
  if start_sort
    q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end-1)) < 1.0e-10
    q_of_z(:,end-1) = [];
  end
  hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
end
% disp_status(free_energy, hp_posterior, opts);
disp(['### updating posterior ... done.'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, I] = sort_q_of_z(data, q_of_z, opts);
disp('sorting...')
if opts.use_kd_tree
  Nc = [data.kdtree(:).N]*q_of_z; % 1*K
else
  Nc = sum(q_of_z, 1); % 1*K
end
if isequal(opts.algorithm, 'vdp')
  [dummy,I] = sort(Nc(1:end-1), 2, 'descend');
  I(end+1) = length(Nc);
else
  [dummy,I] = sort(Nc, 2, 'descend');
end
q_of_z = q_of_z(:,I);
disp('sorting... done.')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function disp_status(free_energy, hp_posterior, opts);
if isequal(opts.algorithm, 'vdp')
  Nc = hp_posterior.true_Nc;
else
  Nc = hp_posterior.Nc;
end
disp(['F=' num2str(free_energy) ...
      ';    Nc=[' num2str(Nc, ' %0.5g ') ...
      '];'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bool = free_energy_improved(free_energy, new_free_energy, warn_when_increasing, opts);
diff = new_free_energy - free_energy;
if abs(diff/free_energy) < opts.threshold
  bool = 0;
elseif diff > 0
  if warn_when_increasing
    if abs(diff/free_energy) > 1.0e-3
      error(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    else
      warning(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    end
  end
  bool = 0;
elseif diff == 0
  bool = 0
else
  bool = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = init_kdtree_partitions(given_data, depth);
[D,N] = size(given_data);
root = mk_kdtree_partition(given_data, [1:N]);
partitions = expand_recursively(root, depth);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = expand_recursively(partition, depth);
children = mk_children_of_partition(partition);
if depth == 1 | length(children) == 1
  partitions = children;
else
  partitions1 = expand_recursively(children(1), depth-1);
  partitions2 = expand_recursively(children(2), depth-1);
  partitions = [partitions1 partitions2];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function children = mk_children_of_partition(partition);
if partition.N == 1
  children = partition;
  return
end
dir = divide_by_principal_component(partition.given_data(:,partition.indices), ...
                                    partition.sigma, ...
                                    partition.mean);
positive_I = find(dir>=0);
negative_I = find(dir<0);
if length(positive_I) == 0 | length(negative_I) == 0
  children = partition;
  return
end
children(1) = mk_kdtree_partition(partition.given_data, partition.indices(positive_I));
children(2) = mk_kdtree_partition(partition.given_data, partition.indices(negative_I));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function direction = divide_by_principal_component(data, covariance, mean);
N = size(data, 2);
if size(data,1) <= 16
  [V,D] = eig(covariance);
  [eig_val, principal_component_i] = max(diag(D));
  principal_component = V(:,principal_component_i);
else
  [principal_component,eig_val] = power_method(covariance);
end
direction = sum((data - repmat(mean, 1, N)).*repmat(principal_component, 1, N), 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partition = mk_kdtree_partition(given_data, indices);
partition.N = length(indices);
if partition.N == 0
  error('indices must have at least one index.')
end
data_a = given_data(:,indices);
partition.given_data = given_data;
partition.indices = indices;
partition.sum_x = sum(data_a, 2); % D*1
mean = partition.sum_x / partition.N; % D*1
partition.mean = mean;
partition.sum_xx = data_a*data_a'; % D*D
partition.sigma = partition.sum_xx / partition.N - mean*mean';
partition.children = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = rand_q_of_z(data, K, opts);
% q_of_z: N*K
if opts.use_kd_tree
  N = length(data.kdtree);
else
  N = size(data.given_data, 2);
end
if isequal(opts.algorithm, 'vdp')
  q_of_z = zeros(N, K+1);
else
  q_of_z = zeros(N, K);
end
q_of_z(:,1:K) = rand(N, K);
q_of_z = normalize(q_of_z, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
% the last component of q_of_z represents the infinite rest of components
% the last component is the prior.
% q_of_z: N*K
% q_of_z(:,end) is the rest of responsibilities.
threshold_for_N = 1.0e-200;
K = size(q_of_z, 2);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
I = find(Nc>threshold_for_N);
inv_Nc = zeros(1,K);
inv_Nc(I) = 1./Nc(I);
hp_posterior.eta = hp_prior.eta0 + Nc;
hp_posterior.xi = hp_prior.xi0 + Nc;
means = sum_x .* repmat(inv_Nc, D, 1); % D*K
hp_posterior.inv_B = zeros(D,D,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  for c=1:K
    v0 = means(:,c) - hp_prior.m0;
    q_of_z_c = reshape(q_of_z(:,c), 1, 1, N);
    S = sum(repmat(q_of_z_c,[D,D,1]).*t1, 3) - Nc(c)*means(:,c)*means(:,c)';
    hp_posterior.B{c} = hp_prior.B0 ...
        + S ...
        + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
  end
else
  if opts.collapsed_means
    quad_term = mk_quad_term(data, q_of_z, hp_prior, opts);
  end
  for c=1:K
    if opts.collapsed_means
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*data.given_data)*data.given_data' ...
          + quad_term(:,:,c);
    else
      v = data.given_data - repmat(means(:,c),1,N); % D*N
      v0 = means(:,c) - hp_prior.m0;
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*v)*v' ...
          + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
    end
  end
end
for c=1:K
  hp_posterior.inv_B(:,:,c) = inv(hp_posterior.B{c});
end
hp_posterior.m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0,1,K)) ...
    ./ repmat(Nc+hp_prior.xi0,D,1);
if isequal(opts.algorithm, 'vdp')
  % gamma: 2*K
  hp_posterior.gamma = zeros(2,K);
  hp_posterior.gamma(1,:) = 1 + true_Nc;
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(true_Nc) - cumsum(true_Nc,2);
elseif isequal(opts.algorithm, 'bj')
  hp_posterior.gamma = zeros(2,K-1);
  hp_posterior.gamma(1,:) = 1 + Nc(1:K-1);
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(Nc) - cumsum(Nc(1:K-1),2);
elseif isequal(opts.algorithm, 'non_dp') | isequal(opts.algorithm, 'cdp')
  hp_posterior.tilde_alpha = hp_prior.alpha/K + Nc;
elseif isequal(opts.algorithm, 'csb')
  1;
else
  error('unknown algorithm')
end

hp_posterior.Nc = Nc;
if isequal(opts.algorithm, 'vdp')
  hp_posterior.true_Nc = true_Nc;
end
hp_posterior.q_of_z = q_of_z; % q_of_z is a N by K matrix where N is
                              % #given_data or #nodes in a kd-tree.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
% returns E[ log q(eta)/p(eta) ]_q
% fc : 1 by K
D = size(hp_posterior.m, 1);
K = size(hp_posterior.eta, 2);
log_det_B = zeros(1,K);
for c=1:K
  log_det_B(c) = detln(hp_posterior.B{c});
  d = hp_posterior.m(:,c)-hp_prior.m0; % D*1
  term_eta(1,c) = sum(sum(hp_posterior.inv_B(:,:,c).*(hp_prior.xi0*d*d'),1),2);
  term_eta(2,c) = sum(sum(hp_posterior.inv_B(:,:,c).*hp_prior.B0,1),2) - D;
end
E_log_q_p_mean = ...
    + 0.5*D*(hp_prior.xi0./hp_posterior.xi ...
             - log(hp_prior.xi0./hp_posterior.xi) ...
             - 1) ...
    + 0.5*(hp_posterior.eta).* term_eta(1,:);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
E_log_q_p_cov = ...
    0.5*hp_prior.eta0*(log_det_B-detln(hp_prior.B0)) ...
    + 0.5*hp_posterior.Nc.*psi_sum ...
    + 0.5*(hp_posterior.eta).* term_eta(2,:) ...
    + gamma_multivariate_ln(hp_prior.eta0*0.5,D) ...
    - gamma_multivariate_ln(hp_posterior.eta*0.5,D);

%debug
if length(find(E_log_q_p_mean<-1.0e-8,1)) > 0
  E_log_q_p_mean
  error('E_log_q_p_mean is negative.')
end
if length(find(E_log_q_p_cov<-1.0e-8,1)) > 0
  E_log_q_p_cov
  error('E_log_q_p_mean is negative.')
end

fc = E_log_q_p_mean + E_log_q_p_cov;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, log_lambda] = mk_free_energy(data, hp_posterior, ...
                                                  hp_prior, opts, ...
                                                  fc, log_lambda);
if nargin == 4
  fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts); % 1*K
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts); % N*K
end
[N,K] = size(log_lambda);
if isequal(opts.algorithm, 'vdp') || isequal(opts.algorithm, 'bj')
  % note when bj,  if full hp_posterior is given, len_gamma = K - 1.
  len_gamma = size(hp_posterior.gamma, 2);
  if isequal(opts.algorithm, 'bj') && len_gamma ~= K - 1
    error('invalid length')
  end
  E_log_p_of_V = ...
      gammaln(sum(hp_posterior.gamma, 1)) ...
      - gammaln(1+hp_prior.alpha) ...
      - sum(gammaln(hp_posterior.gamma), 1) ...
      + gammaln(hp_prior.alpha) ...
      + ((hp_posterior.gamma(1,:)-1) ...
         .*(psi(hp_posterior.gamma(1,:))-psi(sum(hp_posterior.gamma,1)))) ...
      + ((hp_posterior.gamma(2,:)-hp_prior.alpha) ...
         .*(psi(hp_posterior.gamma(2,:))-psi(sum(hp_posterior.gamma,1))));
  extra_term = sum(E_log_p_of_V);
elseif isequal(opts.algorithm, 'non_dp')
  E_log_p_of_pi = ...
      sum(gammaln(hp_prior.alpha/K) ...
          - gammaln(hp_posterior.tilde_alpha) ...
          + hp_posterior.Nc.*(psi(hp_posterior.tilde_alpha) ...
                              - psi(sum(hp_posterior.tilde_alpha)))) ...
      + gammaln(sum(hp_prior.alpha+size(data,2))) - gammaln(hp_prior.alpha);
  extra_term = E_log_p_of_pi;
elseif isequal(opts.algorithm, 'cdp') || isequal(opts.algorithm, 'csb')
  E_log_p_of_z_given_other_z = ...
      mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts); % N by K
  q_of_z = mk_q_of_z(data,hp_posterior,hp_prior,opts,log_lambda);
%   q_of_z = hp_posterior.q_of_z;
  E_Nc = hp_posterior.Nc;
  V_Nc = sum(q_of_z.*(1-q_of_z), 1); % 1 by K
  if isequal(opts.algorithm, 'cdp')
    E_log_p_of_z = gammaln(hp_prior.alpha) - gammaln(N+hp_prior.alpha) ...
        + sum(gammaln(hp_prior.alpha/K + E_Nc) ...
              - 0.5*psi(1, hp_prior.alpha/K + E_Nc).*V_Nc - gammaln(hp_prior.alpha/K));
  else % csb
    E_Nc_geq_to_i = cumsum(E_Nc, 2);
    q_of_z_geq_to_i = cumsum(q_of_z, 2);
    V_Nc_geq_to_i = sum(q_of_z_geq_to_i.*(1-q_of_z_geq_to_i), 1);

    E_Nc_greater_than_i = cumsum(E_Nc, 2) - E_Nc;
    q_of_z_greater_than_i = q_of_z_geq_to_i - q_of_z;
    V_Nc_greater_than_i = sum(q_of_z_greater_than_i.*(1-q_of_z_greater_than_i), 1);

    tmp = gammaln(1 + E_Nc) - 0.5*psi(1, 1 + E_Nc).*V_Nc ...  % E[log p(1+Nc)]
          + (gammaln(1 + E_Nc_greater_than_i) ...             % E[log p(1+N_{>c})]
             - 0.5*psi(1, 1 + E_Nc_greater_than_i).*V_Nc_greater_than_i) ...
          - (gammaln(1 + hp_prior.alpha + E_Nc_geq_to_i) ...  % E[log p(1+alpha+N_{>=c})]
             - 0.5*psi(1, 1 + hp_prior.alpha + E_Nc_geq_to_i).*V_Nc_geq_to_i);
    E_log_p_of_z = sum(tmp(1:end-1));
  end
  extra_term = sum(sum(E_log_p_of_z_given_other_z.*q_of_z, 2), 1) - E_log_p_of_z;
else
  error('unknown algorithm')
end
if opts.use_kd_tree
  free_energy = extra_term + sum(fc) - [data.kdtree(:).N]*log_sum_exp(log_lambda, 2);
else
  free_energy = extra_term + sum(fc) - sum(log_sum_exp(log_lambda, 2), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
% log_lambda: N*K
% q(z_n=c|x_n) = lambda_n_c / sum_c lambda_n_c

if isequal(opts.algorithm, 'vdp')
  if abs(hp_posterior.gamma(2,end) - hp_prior.alpha) > 1.0e-5
    hp_posterior.gamma(2,end)
    hp_prior.alpha
    diff = hp_prior.alpha - hp_posterior.gamma(2,end)
    error('must be alpha')
  end
end

if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
else
  [D,N] = size(data.given_data);
end
K = size(hp_posterior.eta, 2);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
log_lambda = zeros(N,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  sum_x = repmat(reshape([data.kdtree(:).sum_x],D,1,N),[1,D,1]);
  Na = repmat(reshape([data.kdtree(:).N],1,1,N),[D,D,1]);
end
if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
  E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
end
for c=1:K
%   Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
%   if isequal(opts.algorithm, 'vdp')
%     constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%         + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%   elseif isequal(opts.algorithm, 'bj')
%     if c < K
%       constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%           + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     else
%       constant = sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     end
%   elseif isequal(opts.algorithm, 'non_dp')
%     % E[log pi] ; pi is the weight of mixtures.
%     constant = psi(hp_posterior.tilde_alpha(c)) - psi(sum(hp_posterior.tilde_alpha));
%   elseif isequal(opts.algorithm, 'cdp')
%     constant = 0;
%   elseif isequal(opts.algorithm, 'csb')
%     constant = 0;
%   else
%     error('unknown algorithm')
%   end
%   constant = constant - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
%       + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
%   if opts.use_kd_tree
%     t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
%     term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
%         repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
%     column_c = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + constant;
%   else
%     d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
%     column_c = - sum(d.*(Precision*d),1) + constant; % 1*N
%   end
%   if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
%     column_c = column_c + E_log_p_of_z_given_other_z(:,c)';
%   end
%   log_lambda(:,c) = column_c;
  if isequal(opts.algorithm, 'vdp')
    E_log_p_of_z_given_other_z_c = ...
        psi(hp_posterior.gamma(1,c)) ...
        - psi(sum(hp_posterior.gamma(:,c),1)) ...
        + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
  elseif isequal(opts.algorithm, 'bj')
    if c < K
      E_log_p_of_z_given_other_z_c = ...
          psi(hp_posterior.gamma(1,c)) ...
          - psi(sum(hp_posterior.gamma(:,c),1)) ...
          + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    else
      E_log_p_of_z_given_other_z_c = sum(psi(hp_posterior.gamma(2,[1:c-1])) ...
                                         - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    end
  elseif isequal(opts.algorithm, 'non_dp')
    % E[log pi] ; pi is the weight of mixtures.
    E_log_p_of_z_given_other_z_c = psi(hp_posterior.tilde_alpha(c)) ...
        - psi(sum(hp_posterior.tilde_alpha));
  elseif isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
    E_log_p_of_z_given_other_z_c = E_log_p_of_z_given_other_z(:,c)';
  else
    error('unknown algorithm')
  end

  Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  E_log_p_of_x = - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
      + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
  if opts.use_kd_tree
    t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
    term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
        repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
    E_log_p_of_x = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + E_log_p_of_x;
  else
    d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
    E_log_p_of_x = - sum(d.*(Precision*d),1) + E_log_p_of_x; % 1*N
  end
  log_lambda(:,c) = E_log_p_of_x + E_log_p_of_z_given_other_z_c;
end
if isequal(opts.algorithm, 'vdp')
  log_lambda(:,end) = log_lambda(:,end) - log(1- exp(psi(hp_prior.alpha) - psi(1+hp_prior.alpha)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, data, log_lambda] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
% q_of_z: N*K
if nargin == 4
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
end
q_of_z = exp(normalizeln(log_lambda, 1));
if opts.weight ~= 1
  q_of_z = q_of_z .* repmat(opts.weight, 1, size(q_of_z,2));
else
  q_of_z = q_of_z;
end

function M =normalizeln(M ,dimension);
M = lpt2lcpt(M, dimension);

function lcpt=lpt2lcpt(lpt,dimension);
% function lcpt=lpt2lcpt(lpt,dimension);
%
% make a log conditional probability table with log probability table
%
% lpt is a matrix.
% dimension must be either 1 or 2.
%
% lcpt = log( exp(lpt) / sum(exp(lpt)) )
%      = lpt - log(sum(exp(lpt)))
%

the_other_dimension=-(dimension-1.5)+1.5;
lpt=permute(lpt,[dimension,the_other_dimension]);
% now we can calculate as if dimension=1.

log_sum_exp_lpt = log_sum_exp(lpt,2); % Mx1
lcpt = lpt - repmat(log_sum_exp_lpt,1,size(lpt,2));

lcpt=permute(lcpt,[dimension,the_other_dimension]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_prior = mk_hp_prior(data, opts)
if isfield(opts, 'xi0')
  hp_prior.xi0 = opts.xi0;
else
  hp_prior.xi0 = 0.01;
end
if isfield(opts, 'eta_p')
  eta_p = opts.eta_p;
else
  eta_p = 1;
end
if isfield(opts, 'alpha')
  hp_prior.alpha = opts.alpha;
else
  hp_prior.alpha = 1;
end
[D,N] = size(data.given_data);
if opts.use_kd_tree
  sum_xx = sum(reshape([data.kdtree(:).sum_xx],D,D,length(data.kdtree)),3);
  sum_x = sum([data.kdtree(:).sum_x],2);
  covariance = sum_xx/N - sum_x*sum_x'/(N*N);
else
  covariance = cov(data.given_data');
end
hp_prior.m0 = mean(data.given_data, 2);
if D > 16
  [dummy, max_eig] = power_method(covariance);
else
  max_eig = max(eig(covariance));
end
hp_prior.eta0 = eta_p * D + 1;
hp_prior.B0 = hp_prior.eta0 * max_eig * eye(D) * hp_prior.xi0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [vec,value]=power_method(A, start, precision)

switch nargin
 case 1
  start = ones(length(A),1);
  precision = 1e-10;
 case 2
  precision = 1e-10;
 case 3
 otherwise
  error 'invalid number of arguments'
end

%
% xp = A^p * x
% x(p+1) = lambda * xp   when p is big enough
% lambda = x(p+1)'*xp / xp'*xp
%
diff=precision+1;
x=start;
n=norm(x)+diff;
i = 0;
while diff> precision
  i = i + 1;
  y=A*x;
  n2 = norm(x);
  diff=abs(n2-n);
  n=n2;
  if n < 1.0e-200
    x = zeros(length(A), 1);
    break
  else
    x=y/n;
  end
  if i > 100
    break
  end
end
n = norm(x);
if n < 1.0e-200
  vec = zeros(length(A), 1);
else
  vec = x/n;
end
value=n;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function m=normalize(m,dim)
% return m normalized with 'dimension'
%
% e.g.
% m : i by j by k by ...
% m = sum(normalize(m,2), 2);
% m(i, :, k, ...) = ones(1, J, 1, ...)
%

dims = ones(1, ndims(m));
dims(dim) = size(m, dim);
m = m ./ repmat(sum(m, dim), dims);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Y] = detln( X )
% Y = logdet( X )
% return a log determinant of X
[d err] = chol(X);
if err
  error('error in Choleski disposition for detln');
end
Y = sum(log(diag(d))) *2;

% Local Variables: ***
% mode: matlab ***
% End: ***

repo: REST


text:
function [Y,W,SetupStruc] = Process_maxSNR(s,Transfer,SetupStruc)
K = SetupStruc.maxSNR.K;
hop = SetupStruc.maxSNR.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.maxSNR.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(size(X,1),size(X,2),Num);
covMa = cal_covMa(SetupStruc.unS,K,hop,win);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
theta = 10^-4;
W = zeros(Num,N,K_m);
for i = 2:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% maxSNR
    W_f = zeros(N,Num);
    for j = 1:Num
        R_d = covMa(:,:,i,j);
        R_i = zeros(N);
        for k = 1:Num
            if(k==j) continue;end
            R_i = R_i+covMa(:,:,i,k);
        end
        if(rcond(R_i)<theta)
            R_i = R_i+eye(N)*min(diag(R_i))*theta;
        end
%         [E,~] = PCA(R_i\R_d,1,1);
        [E,D] = eig(R_i\R_d);
        [~,index] = max(diag(D));
        E = E(:,index);
        W_f(:,j) = E*sqrt(E'*R_i*R_i*E/N)/(E'*R_i*E);  %%%%%% WAN post filtering
    end
    W_f = W_f';
    W(:,:,i) = W_f;
    Y_ = W_f*X_f;
    Y_f(i,:,:) = Y_.';
    if(i~=K_m)
        Y_f(K+2-i,:,:) = Y_';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    Y(:,i) = overlapadd(real(ifft(Y_f(:,:,i)))',win,hop);
end

return;

repo: Voice-Separation-and-Enhancement


text:
function [doy, year] = mydatedoy (epoch, year)
    if (nargin < 2),  year = [];  end
    if (size(epoch,2) == 1)
        % epoch is in mydatenum format
        num = epoch;
        vec = mydatevec(num);
    else
        % epoch is in mydatevec format
        vec = epoch;
        num = mydatenum(vec);
    end
    clear epoch

    %% now define the epoch corresponding to the beginning to that year:
    if isempty(year),  year = vec(:,1);  end
    if ischar(year) && any(strcmpi(year, {'median','fixed'}))
        year = median(vec(:,1));
    end
    vec0 = zeros(size(vec));
    vec0(:,1) = year;
    num0 = mydatenum(vec0);

    %%
    doy = (num - num0) ./ (3600 .* 24);
end

%!test
%! d = [2000 1 1 0 0 0];
%! doy_correct = 1;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);

%!test
%! d = [2000 1 30 0 0 0];
%! doy_correct = 30;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);

%!test
%! d = [2000 2 1 0 0 0];
%! doy_correct = 32;
%! doy_answer = mydatedoy(mydatenum(d));
%! myassert (doy_answer, doy_correct);

author: Sable

repo: mcbench-benchmarks


text:
function D = spm_eeg_prep(S)
% Prepare converted M/EEG data for further analysis
% FORMAT D = spm_eeg_prep(S)
% S                 - configuration structure (optional)
% (optional) fields of S:
%   S.D             - MEEG object or filename of M/EEG mat-file
%   S.task          - action string. One of 'settype', 'defaulttype',
%                     'loadtemplate','setcoor2d', 'project3d', 'loadeegsens',
%                     'defaulteegsens', 'sens2chan', 'headshape',
%                     'coregister', 'sortconditions'
%
%   S.updatehistory - update history information [default: true]
%   S.save          - save MEEG object [default: false]
%
% D                 - MEEG object
%__________________________________________________________________________
% Copyright (C) 2008-2019 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_prep.m 7586 2019-05-03 14:57:34Z guillaume $

D = spm_eeg_load(S.D);

switch lower(S.task)
    %----------------------------------------------------------------------
    case 'setbadchan'
        %----------------------------------------------------------------------
        D = badchannels(D, D.selectchannels(S.channels), S.status);
        %----------------------------------------------------------------------
    case 'settype'
        %----------------------------------------------------------------------
        D = chantype(D, S.ind, S.type);

        %----------------------------------------------------------------------
    case 'defaulttype'
        %----------------------------------------------------------------------
        if isfield(S, 'ind')
            ind = S.ind;
        else
            ind = 1:D.nchannels;
        end

        dictionary = {
            'eog',           'EOG';
            'eeg',           'EEG';
            'ecg',           'ECG';
            'lfp',           'LFP';
            'emg',           'EMG';
            'meg',           'MEG';
            'ref',           'REF';
            'megref'         'REF';
            'megmag',        'MEGMAG';
            'megplanar',     'MEGPLANAR';
            'meggrad',       'MEGGRAD';
            'refmag',        'REFMAG';
            'refgrad',       'REFGRAD'
            'refplanar'      'REFPLANAR'
            };

        D = chantype(D, ind, 'Other');

        type = ft_chantype(D.chanlabels);

        % If there is useful information in the original types it
        % overwrites the default assignment
        if isfield(D, 'origchantypes')
            [sel1, sel2] = spm_match_str(chanlabels(D, ind), D.origchantypes.label);

            type(ind(sel1)) = D.origchantypes.type(sel2);
        end

        spmtype = repmat({'Other'}, 1, length(ind));

        [sel1, sel2] = spm_match_str(type(ind), dictionary(:, 1));

        spmtype(sel1) = dictionary(sel2, 2);

        D = chantype(D, ind, spmtype);

        %----------------------------------------------------------------------

    case 'bidschantype'

        dictionary = { % Should be updates with updates to BIDS specification
            'MEGMAG'   'MEGMAG'
            'MEGGRAD'  'MEGPLANAR'
            'MEG'      'MEG'
            'MEGREF'   'REF'
            'EEG'      'EEG'
            'EOG'      'EOG'
            'ECG'      'ECG'
            'EMG'      'EMG'
            'TRIG'     'Other'
            'AUDIO'    'Other'
            'PD'       'Other'
            'ET'       'Other'
            'MISC'     'Other'
            };

        bids_chan = spm_load(S.filename);

        [sel1, sel2] = spm_match_str(D.chanlabels, bids_chan.name);

        type = bids_chan.type(sel2);

        [sel3, sel4] = spm_match_str(type, dictionary(:, 1));

        type(sel3) = dictionary(sel4, 2);

        D = chantype(D, sel1, type);

    case 'bidschanstatus'
        bids_chan = spm_load(S.filename);

        [sel1, sel2] = spm_match_str(D.chanlabels, bids_chan.name);

        sel3 = strmatch('bad', bids_chan.status(sel2));

        D = badchannels(D, sel1, 0);

        D = badchannels(D, sel1(sel3), 1);

    case {'loadtemplate', 'setcoor2d', 'project3d'}
        %----------------------------------------------------------------------
        chanind = 1:D.nchannels;

        switch lower(S.task)
            case 'loadtemplate'
                template    = load(S.P); % must contain Cpos, Cnames
                xy          = template.Cpos;
                label       = template.Cnames;
            case 'setcoor2d'
                xy          = S.xy;
                label       = S.label;
            case 'project3d'
                if ~isfield(D, 'val')
                    D.val = 1;
                end
                if isfield(D, 'inv') && isfield(D.inv{D.val}, 'datareg')
                    datareg = D.inv{D.val}.datareg;
                    ind     = strmatch(S.modality, {datareg(:).modality}, 'exact');
                    sens    = datareg(ind).sensors;
                else
                    sens    = D.sensors(S.modality);
                end
                [xy, label] = spm_eeg_project3D(sens, S.modality);

                switch S.modality
                    case 'EEG'
                        chanind  = D.indchantype('EEG');
                    case 'MEG'
                        chanind  = D.indchantype('MEGANY');
                    case 'MEGCOMB'
                        chanind  = D.indchantype('MEGCOMB');
                end
        end

        megcombind   = D.indchantype('MEGCOMB');

        if ~isempty(megcombind)
            chanset = spm_eeg_planarchannelset(D.chanlabels);
            [sel1, sel2] = spm_match_str(lower(chanset(:, 1)), lower(label));
            [sel3, sel4] = spm_match_str(lower(chanset(:, 2)), lower(label));

            [sel5, sel6, sel7] = intersect(sel1, sel3);

            label = [label(:); chanset(sel5, 3)];

            xy    = [xy 0.5*(xy(:, sel2(sel6)) + xy(:, sel4(sel7)))];
        end

        [sel1, sel2] = spm_match_str(lower(D.chanlabels(chanind)), lower(label));
        sel1         = chanind(sel1);

        if ~isempty(sel1)

            megind = D.indchantype('MEG');
            eegind = D.indchantype('EEG');

            if ~isempty(intersect(megind, sel1)) && ~isempty(setdiff(megind, sel1))
                error('2D locations not found for all MEG channels');
            end

            if ~isempty(intersect(megcombind, sel1)) && ~isempty(setdiff(megcombind, sel1))
                error('2D locations not found for all MEGCOMB channels');
            end

            if ~isempty(intersect(eegind, sel1)) && ~isempty(setdiff(eegind, sel1))
                warning(['2D locations not found for all EEG channels, changing type of channels', ...
                    num2str(setdiff(eegind, sel1)) ' to ''Other''']);

                D = chantype(D, setdiff(eegind, sel1), 'Other');
            end

            D = coor2D(D, sel1, num2cell(xy(:, sel2)));
        end

        %----------------------------------------------------------------------
    case 'loadeegsens'
        %----------------------------------------------------------------------
        switch S.source
            case 'mat'
                senspos = load(S.sensfile);
                name    = fieldnames(senspos);
                senspos = getfield(senspos,name{1});

                label = chanlabels(D, D.indchantype('EEG'));

                if size(senspos, 1) ~= length(label)
                    error('To read sensor positions without labels the numbers of sensors and EEG channels should match.');
                end

                elec = [];
                elec.chanpos = senspos;
                elec.elecpos = senspos;
                elec.label = label;

                headshape = load(S.headshapefile);
                name    = fieldnames(headshape);
                headshape = getfield(headshape,name{1});

                shape = [];

                fidnum = 0;
                while ~all(isspace(S.fidlabel))
                    fidnum = fidnum+1;
                    [shape.fid.label{fidnum},S.fidlabel] = strtok(S.fidlabel);
                end

                if (fidnum < 3)  || (size(headshape, 1) < fidnum)
                    error('At least 3 labeled fiducials are necessary');
                end

                shape.fid.pnt = headshape(1:fidnum, :);

                if size(headshape, 1) > fidnum
                    shape.pnt = headshape((fidnum+1):end, :);
                else
                    shape.pnt = [];
                end
            case 'locfile'
                label = chanlabels(D, D.indchantype('EEG'));

                elec = ft_read_sens(S.sensfile);

                % Remove headshape points
                hspind = strmatch('headshape', elec.label);
                elec.chanpos(hspind, :) = [];
                elec.elecpos(hspind, :) = [];
                elec.chantype(hspind, :) = [];
                elec.chanunit(hspind, :) = [];
                elec.label(hspind)  = [];

                % This handles FIL Polhemus case and other possible cases
                % when no proper labels are available.
                if isempty(intersect(label, elec.label))
                    ind = str2num(strvcat(elec.label));
                    if length(ind) == length(label)
                        elec.label = label(ind);
                    else
                        error('To read sensor positions without labels the numbers of sensors and EEG channels should match.');
                    end
                end

                shape = spm_eeg_fixpnt(ft_read_headshape(S.sensfile));

                % In case electrode file is used for fiducials, the
                % electrodes can be used as headshape
                if ~isfield(shape, 'pnt') || isempty(shape.pnt) && ...
                        size(shape.fid.pnt, 1) > 3
                    shape.pnt = shape.fid.pnt;
                end

        end

        elec = ft_convert_units(elec, 'mm');
        shape= ft_convert_units(shape, 'mm');

        if isequal(D.modality(1, 0), 'Multimodal')
            if ~isempty(D.fiducials) && isfield(S, 'regfid') && ~isempty(S.regfid)
                M1 = coreg(D.fiducials, shape, S.regfid);
                elec = ft_transform_geometry(M1, elec);
            else
                error(['MEG fiducials matched to EEG fiducials are required '...
                    'to add EEG sensors to a multimodal dataset.']);
            end
        else
            D = fiducials(D, shape);
        end

        D = sensors(D, 'EEG', elec);

        %----------------------------------------------------------------------
    case 'defaulteegsens'
        %----------------------------------------------------------------------

        template_sfp = dir(fullfile(spm('dir'), 'EEGtemplates', '*.sfp'));
        template_sfp = {template_sfp.name};

        if ft_senstype(D.chanlabels(D.indchantype('EEG')), 'ext1020')
            ind = strmatch('ext1020.sfp', template_sfp, 'exact');
        else
            ind = strmatch([ft_senstype(D.chanlabels(D.indchantype('EEG'))) '.sfp'], template_sfp, 'exact');
        end

        if ~isempty(ind)
            fid = D.fiducials;

            if isequal(D.modality(1, 0), 'Multimodal') && ~isempty(fid)

                nzlbl = {'fidnz', 'nz', 'nas', 'nasion', 'spmnas'};
                lelbl = {'fidle', 'fidt9', 'lpa', 'lear', 'earl', 'le', 'l', 't9', 'spmlpa'};
                relbl = {'fidre', 'fidt10', 'rpa', 'rear', 'earr', 're', 'r', 't10', 'spmrpa'};

                [sel1, nzind] = spm_match_str(nzlbl, lower(fid.fid.label));
                if ~isempty(nzind)
                    nzind = nzind(1);
                end
                [sel1, leind] = spm_match_str(lelbl, lower(fid.fid.label));
                if ~isempty(leind)
                    leind = leind(1);
                end
                [sel1, reind] = spm_match_str(relbl, lower(fid.fid.label));
                if ~isempty(reind)
                    reind = reind(1);
                end

                regfid = fid.fid.label([nzind, leind, reind]);
                if numel(regfid) < 3
                    error('Could not automatically understand the MEG fiducial labels. Please use the GUI.');
                else
                    regfid = [regfid(:) {'spmnas'; 'spmlpa'; 'spmrpa'}];
                end

                S1 = [];
                S1.D = D;
                S1.task = 'loadeegsens';
                S1.source = 'locfile';
                S1.regfid = regfid;
                S1.sensfile = fullfile(spm('dir'), 'EEGtemplates', template_sfp{ind});
                S1.updatehistory = 0;
                D = spm_eeg_prep(S1);
            else
                elec = ft_read_sens(fullfile(spm('dir'), 'EEGtemplates', template_sfp{ind}));

                [sel1, sel2] = spm_match_str(lower(D.chanlabels), lower(elec.label));

                sens = elec;
                sens.chanpos  = sens.chanpos(sel2, :);
                sens.elecpos  = sens.elecpos(sel2, :);
                sens.chantype = sens.chantype(sel2, :);
                sens.chanunit = sens.chanunit(sel2, :);

                % This takes care of possible case mismatch
                sens.label = D.chanlabels(sel1);

                sens.label = sens.label(:);

                D = sensors(D, 'EEG', sens);

                % Assumes that the first 3 points in standard location files
                % are the 3 fiducials (nas, lpa, rpa)
                fid = [];
                fid.pnt = elec.elecpos;
                fid.fid.pnt = elec.elecpos(1:3, :);
                fid.fid.label = elec.label(1:3);

                [xy, label] = spm_eeg_project3D(D.sensors('EEG'), 'EEG');

                [sel1, sel2] = spm_match_str(lower(D.chanlabels), lower(label));

                if ~isempty(sel1)

                    eegind = strmatch('EEG', chantype(D), 'exact');

                    if ~isempty(intersect(eegind, sel1)) && ~isempty(setdiff(eegind, sel1))
                        warning(['2D locations not found for all EEG channels, changing type of channels ', ...
                            num2str(setdiff(eegind(:)', sel1(:)')) ' to ''Other''']);

                        D = chantype(D, setdiff(eegind, sel1), 'Other');
                    end

                    if any(any(coor2D(D, sel1) - xy(:, sel2)))
                        D = coor2D(D, sel1, num2cell(xy(:, sel2)));
                    end
                end

                if strcmp(D.modality(1, 0), 'Multimodal') && ~isempty(D.fiducials)...
                        && isfield(S, 'regfid') && ~isempty(S.regfid)
                    M1 = coreg(D.fiducials, fid, S.regfid);
                    D = sensors(D, 'EEG', ft_transform_geometry(M1, D.sensors('EEG')));
                else
                    D = fiducials(D, fid);
                end
            end
        end

        %----------------------------------------------------------------------
    case 'loadmegsens'
        %----------------------------------------------------------------------
        hdr  = ft_read_header(S.source);
        D = sensors(D, 'MEG', hdr.grad);
        D = fiducials(D, ft_convert_units(ft_read_headshape(S.source), 'mm'));

        if ~isempty(D.indchantype('MEG')) && ~isempty(D.sensors('MEG'))

            S1 = [];
            S1.task = 'project3D';
            S1.modality = 'MEG';
            S1.updatehistory = 0;
            S1.D = D;

            D = spm_eeg_prep(S1);
        end

        %----------------------------------------------------------------------
    case 'sens2chan'
        %----------------------------------------------------------------------
        if isfield(S, 'montage')
            montage = S.montage;
            if ischar(montage)
                montage = getfield(load(montage), 'montage');
            end
        elseif isfield(S, 'refelec');
            sens = sensors(D, 'EEG');
            if isempty(sens)
                error('The montage cannod be applied - no EEG sensors specified');
            end
            if ismember('all', S.refelec)
                refind = 1:numel(sens.label);
            else
                refind = spm_match_str(sens.label, S.refelec);
            end

            tra            = eye(numel(sens.label));
            tra(:, refind) = tra(:, refind) - 1/length(refind);

            montage.tra = tra;
            montage.labelorg = sens.label;
            montage.labelnew = sens.label;
        else
            error('Montage or list of reference sensors should be specified');
        end

        modalities = {'EEG', 'MEG'};
        for m = 1:numel(modalities)
            sens = sensors(D, modalities{m});
            if ~isempty(sens) && ~isempty(intersect(sens.label, montage.labelorg))
                sens = sensors(D, 'EEG');
                sens = ft_apply_montage(sens, montage, 'keepunused', 'no');
                D = sensors(D, modalities{m}, sens);
            end
        end

        %----------------------------------------------------------------------
    case 'headshape'
        %----------------------------------------------------------------------
        switch S.source
            case 'mat'
                headshape = load(S.headshapefile);
                name    = fieldnames(headshape);
                headshape = getfield(headshape,name{1});

                shape = [];

                fidnum = 0;
                while ~all(isspace(S.fidlabel))
                    fidnum = fidnum+1;
                    [shape.fid.label{fidnum},S.fidlabel] = strtok(S.fidlabel);
                end

                if (fidnum < 3)  || (size(headshape, 1) < fidnum)
                    error('At least 3 labeled fiducials are necessary');
                end

                shape.fid.pnt = headshape(1:fidnum, :);

                if size(headshape, 1) > fidnum
                    shape.pnt = headshape((fidnum+1):end, :);
                else
                    shape.pnt = [];
                end
            otherwise
                shape = spm_eeg_fixpnt(ft_read_headshape(S.headshapefile));

                % In case electrode file is used for fiducials, the
                % electrodes can be used as headshape
                if ~isfield(shape, 'pnt') || isempty(shape.pnt) && ...
                        size(shape.fid.pnt, 1) > 3
                    shape.pnt = shape.fid.pnt;
                end
        end

        shape = ft_convert_units(shape, 'mm');

        fid = D.fiducials;

        if ~isempty(fid) && isfield(S, 'regfid') && ~isempty(S.regfid)
            M1 = coreg(fid, shape, S.regfid);
            shape = ft_transform_geometry(M1, shape);
        end

        D = fiducials(D, shape);
        %----------------------------------------------------------------------
    case 'coregister'
        %----------------------------------------------------------------------
        [D, ok] = check(D, '3d');

        if ~ok
            error('Coregistration cannot be performed due to missing data');
        end

        try
            val = D.val;
            Msize = D.inv{val}.mesh.Msize;
        catch
            val = 1;
            Msize = 1;
        end

        D = spm_eeg_inv_mesh_ui(D, val, 1, Msize);
        D = spm_eeg_inv_datareg_ui(D, val);

    case 'sortconditions'
        if ischar(S.condlist)
            cl = getfield(load(S.condlist), 'condlist');
        else
            cl = S.condlist;
        end

        D = condlist(D, cl);
        %----------------------------------------------------------------------
    case 'loadbidsevents'
        if ~isequal(D.type, 'continuous')
            error('This operation can only be applied to continuous datasets');
        end

        ev_bids = spm_load(S.filename);

        if isfield(ev_bids,'trial_type')
            trial_type = 'trial_type';
        else
            % Compatibility with old BIDS-formatted datasets
            trial_type = 'stim_type';
        end
        ev_spm = struct(...
            'type',     repmat({'BIDS'}, length(ev_bids.onset), 1),...
            'value',    ev_bids.(trial_type),...
            'time',     num2cell(ev_bids.onset),...
            'duration', num2cell(ev_bids.duration));

        if S.replace
            D = events(D, 1, ev_spm);
        else
            D = events(D, 1, spm_cat_struct(D.events(1), ev_spm));
        end

        D = D.check;

    otherwise
        %----------------------------------------------------------------------
        fprintf('Unknown task ''%s'' to perform: Nothing done.\n',S.task);
end

% When prep is called from other functions with history, history should be
% disabled
if ~isfield(S, 'updatehistory') || S.updatehistory
    Stemp = S;
    Stemp.D = fullfile(D.path,D.fname);
    Stemp.save = 1;
    D = D.history('spm_eeg_prep', Stemp);
end

if isfield(S, 'save') && S.save
    save(D);
end

%==========================================================================
% function coreg
%==========================================================================
function M1 = coreg(fid, shape, regfid)
[junk, sel1] = spm_match_str(regfid(:, 1), fid.fid.label);
[junk, sel2] = spm_match_str(regfid(:, 2), shape.fid.label);

S = [];
S.targetfid = fid;
S.targetfid.fid.pnt = S.targetfid.fid.pnt(sel1, :);

S.sourcefid = shape;
S.sourcefid.fid.pnt = S.sourcefid.fid.pnt(sel2, :);
S.sourcefid.fid.label = S.sourcefid.fid.label(sel2);

S.targetfid.fid.label = S.sourcefid.fid.label;

S.template = 1;
S.useheadshape = 0;

M1 = spm_eeg_inv_datareg(S);

author: spm


text:
function res = chanlabels(this, varargin)
% Method for getting/setting the channel labels
% FORMAT res = chanlabels(this, ind, label)
% _______________________________________________________________________
% Copyright (C) 2008-2012 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: chanlabels.m 5933 2014-03-28 13:22:28Z vladimir $

if this.montage.Mind == 0
    if nargin == 3
        ind = varargin{1};
        label = varargin{2};

        if iscell(label) && length(label)>1
            if isnumeric(ind) && length(ind)~=length(label)
                error('Indices and values do not match');
            end

            if length(label)>1
                for i = 1:length(label)
                    for j = (i+1):length(label)
                        if strcmp(label{i}, label{j})
                            error('All labels must be different');
                        end
                    end
                end
            end

        end
    end

    res = getset(this, 'channels', 'label', varargin{:});
else
% case with an online montage applied
    if nargin == 3
        ind = varargin{1};
        label = varargin{2};

        if iscell(label) && length(label)>1
            if isnumeric(ind) && length(ind)~=length(label)
                error('Indices and values do not match');
            end

            if length(label)>1
                for i = 1:length(label)
                    for j = (i+1):length(label)
                        if strcmp(label{i}, label{j})
                            error('All labels must be different');
                        end
                    end
                end
            end

        end

        this.montage.M(this.montage.Mind) = getset(this.montage.M(this.montage.Mind), 'channels', 'label', varargin{:});
        res = this;
    else
        res = getset(this.montage.M(this.montage.Mind), 'channels', 'label', varargin{:});
    end

end


text:
function handles = enableDataLoaded(hObject, handles)

set(handles.btnSimGraphCreate, 'Enable', 'on');
set(handles.btnSimGraphLoad, 'Enable', 'on');
set(handles.btnDataNormalize, 'Enable', 'on');
set(handles.btnSaveData, 'Enable', 'on');

handles = togglePlotData(hObject, handles, 'on');

guidata(hObject, handles);

author: beckel

repo: nilm-eval

save_path: github-repos/MATLAB/beckel-nilm-eval


text:
clear all; clc;

%% moudle DIP5
syms  b1 b2 b3 real
syms  g1 g2 g3 real
syms  v1 v2 v3 real
syms  lambda real
syms norm_h norm_g real
B = [b1 b2 b3]';
V = [v1 v2 v3]';
G = [g1 g2 g3]';
% %G'*(V - B) - CONST

 f1= (V - B)' * (V - B) - norm_h^2;
 j1 = jacobian(f1, [B' norm_h lambda]);

 f2 = G' * (V - B) - norm_g * norm_h * lambda;
 j2 = jacobian(f2, [B' norm_h lambda]);

f1
j1

f2
j2

author: yandld


text:
function tutorial_raw(tutorial_dir)
% TUTORIAL_RAW: Script that reproduces the results of the online tutorials "MEG median nerve (Yokogawa)".
%
% DESCRIPTION:
%     It is based on a median nerve stimulation experiment recorded at the Montreal Neurological Institute in 2011
%     with a CTF MEG 275 system. The sample dataset contains 6 minutes of recordings at 1200Hz for one subject
%     and includes 100 stimulations of each arm.
%
% CORRESPONDING ONLINE TUTORIALS:
%     https://neuroimage.usc.edu/brainstorm/Tutorials/MedianNerveCtf
%
% INPUTS:
%     tutorial_dir: Directory where the sample_raw.zip file has been unzipped

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Author: Francois Tadel, 2013-2016

% ===== FILES TO IMPORT =====
% You have to specify the folder in which the tutorial dataset is unzipped
if (nargin == 0) || isempty(tutorial_dir) || ~file_exist(tutorial_dir)
    error('The first argument must be the full path to the tutorial dataset folder.');
end
% Build the path of the files to import
AnatDir = fullfile(tutorial_dir, 'sample_raw', 'Anatomy');
RawFile = fullfile(tutorial_dir, 'sample_raw', 'Data', 'subj001_somatosensory_20111109_01_AUX-f.ds');
% Check if the folder contains the required files
if ~file_exist(RawFile)
    error(['The folder ' tutorial_dir ' does not contain the folder from the file sample_raw.zip.']);
end

% ===== CREATE PROTOCOL =====
% The protocol name has to be a valid folder name (no spaces, no weird characters...)
ProtocolName = 'TutorialRaw';
% Start brainstorm without the GUI
if ~brainstorm('status')
    brainstorm nogui
end
% Delete existing protocol
gui_brainstorm('DeleteProtocol', ProtocolName);
% Create new protocol
gui_brainstorm('CreateProtocol', ProtocolName, 0, 0);
% Start a new report
bst_report('Start');

% ===== ANATOMY =====
% Subject name
SubjectName = 'Subject01';
% Process: Import anatomy folder
bst_process('CallProcess', 'process_import_anatomy', [], [], ...
    'subjectname', SubjectName, ...
    'mrifile',     {AnatDir, 'FreeSurfer'}, ...
    'nvertices',   15000, ...
    'nas',         [127, 212, 123], ...
    'lpa',         [ 55, 124, 119], ...
    'rpa',         [200, 129, 114]);

% ===== LINK CONTINUOUS FILE =====
% Process: Create link to raw file
sFilesRaw = bst_process('CallProcess', 'process_import_data_raw', [], [], ...
    'subjectname',    SubjectName, ...
    'datafile',       {RawFile, 'CTF'}, ...
    'channelreplace', 1, ...
    'channelalign',   1);
% Process: Snapshot: Sensors/MRI registration
bst_process('CallProcess', 'process_snapshot', sFilesRaw, [], ...
    'target',   1, ...  % Sensors/MRI registration
    'modality', 1, ...  % MEG (All)
    'orient',   1, ...  % left
    'Comment',  'MEG/MRI Registration');

% ===== REMOVE 60/180/240 Hz =====
% Process: Notch filter: 60Hz 120Hz 180Hz
sFilesClean = bst_process('CallProcess', 'process_notch', sFilesRaw, [], ...
    'freqlist',    [60, 120, 180], ...
    'sensortypes', 'MEG', ...
    'read_all',    0);

% Process: Power spectrum density (Welch)
sFilesPsd = bst_process('CallProcess', 'process_psd', [sFilesRaw, sFilesClean], [], ...
    'timewindow',  [0, 50], ...
    'win_length',  4, ...
    'win_overlap', 50, ...
    'clusters',    {}, ...
    'sensortypes', 'MEG', ...
    'edit', struct(...
         'Comment',         'Power', ...
         'TimeBands',       [], ...
         'Freqs',           [], ...
         'ClusterFuncTime', 'none', ...
         'Measure',         'power', ...
         'Output',          'all', ...
         'SaveKernel',      0));

% Process: Snapshot: Frequency spectrum
bst_process('CallProcess', 'process_snapshot', sFilesPsd, [], ...
    'target',   10, ...  % Frequency spectrum
    'modality', 1, ...   % MEG (All)
    'Comment',  'Power spectrum density');

% ===== CORRECT BLINKS AND HEARTBEATS =====
% Process: Detect heartbeats
sFilesClean = bst_process('CallProcess', 'process_evt_detect_ecg', sFilesClean, [], ...
    'channelname', 'EEG057', ...
    'timewindow',  [], ...
    'eventname',   'cardiac');

% Process: Detect eye blinks
sFilesClean = bst_process('CallProcess', 'process_evt_detect_eog', sFilesClean, [], ...
    'channelname', 'EEG058', ...
    'timewindow',  [], ...
    'eventname',   'blink');

% Process: Remove simultaneous
sFilesClean = bst_process('CallProcess', 'process_evt_remove_simult', sFilesClean, [], ...
    'remove', 'cardiac', ...
    'target', 'blink', ...
    'dt',     0.25, ...
    'rename', 0);

% % Process: SSP ECG: cardiac
% sFilesClean = bst_process('CallProcess', 'process_ssp_ecg', sFilesClean, [], ...
%     'eventname',   'cardiac', ...
%     'sensortypes', 'MEG', ...
%     'usessp',       1, ...
%     'select',       []);   % Force selection of some components

% Process: SSP EOG: blink
sFilesClean = bst_process('CallProcess', 'process_ssp_eog', sFilesClean, [], ...
    'eventname',   'blink', ...
    'sensortypes', 'MEG', ...
    'usessp',       1, ...
    'select',       1);   % Force selection of some components

% Process: Snapshot: SSP projectors
bst_process('CallProcess', 'process_snapshot', sFilesClean, [], ...
    'target',  2, ...  % SSP projectors
    'Comment', 'SSP projectors');

% ===== IMPORT EVENTS =====
% Process: Import MEG/EEG: Events
sFilesEpochs = bst_process('CallProcess', 'process_import_data_event', sFilesClean, [], ...
    'subjectname', SubjectName, ...
    'condition',   '', ...
    'eventname',   'left, right', ...
    'timewindow',  [], ...
    'epochtime',   [-0.1, 0.3], ...
    'createcond',  0, ...
    'ignoreshort', 1, ...
    'usectfcomp',  1, ...
    'usessp',      1, ...
    'freq',        [], ...
    'baseline',    [-0.1, 0]);

% Process: Add time offset: -4.20ms
sFilesEpochs = bst_process('CallProcess', 'process_timeoffset', sFilesEpochs, [], ...
    'offset',    -0.0042, ...
    'overwrite', 1);

% Process: Average: By condition (subject average)
sFilesAvg = bst_process('CallProcess', 'process_average', sFilesEpochs, [], ...
    'avgtype',    6, ...  % By trial groups (subject average)
    'avg_func',   1, ...  % Arithmetic average: mean(x)
    'keepevents', 0);

% Process: Snapshot: Recordings time series
bst_process('CallProcess', 'process_snapshot', sFilesAvg, [], ...
    'target',   5, ...  % Recordings time series
    'modality', 1, ...  % MEG (All)
    'Comment',  'Evoked response');

% ===== SOURCE MODELING =====
% Process: Compute head model
bst_process('CallProcess', 'process_headmodel', sFilesAvg, [], ...
    'comment',      '', ...
    'sourcespace',  1, ...
    'meg',          3);  % Overlapping spheres

% Process: Compute noise covariance
bst_process('CallProcess', 'process_noisecov', sFilesEpochs, [], ...
    'baseline',     [-0.104, -0.005], ...
    'sensortypes',  'MEG, EEG', ...
    'target',       1, ...  % Noise covariance     (covariance over baseline time window)
    'dcoffset',     1, ...
    'identity',     0, ...
    'copycond',     0, ...
    'copysubj',     0, ...
    'replacefile',  1);  % Replace

% Process: Snapshot: Noise covariance
bst_process('CallProcess', 'process_snapshot', sFilesAvg, [], ...
    'target',  3, ...  % Noise covariance
    'Comment', 'Noise covariance');

% Process: Compute sources
sFilesSrcAvg = bst_process('CallProcess', 'process_inverse', sFilesAvg, [], ...
    'comment', '', ...
    'method',  2, ...  % dSPM
    'wmne',    struct(...
         'NoiseCov',      [], ...
         'InverseMethod', 'dspm', ...
         'ChannelTypes',  {{}}, ...
         'SNR',           3, ...
         'diagnoise',     0, ...
         'SourceOrient',  {{'fixed'}}, ...
         'loose',         0.2, ...
         'depth',         1, ...
         'weightexp',     0.5, ...
         'weightlimit',   10, ...
         'regnoise',      1, ...
         'magreg',        0.1, ...
         'gradreg',       0.1, ...
         'eegreg',        0.1, ...
         'ecogreg',       0.1, ...
         'seegreg',       0.1, ...
         'fMRI',          [], ...
         'fMRIthresh',    [], ...
         'fMRIoff',       0.1, ...
         'pca',           1), ...
    'sensortypes', 'MEG, MEG MAG, MEG GRAD, EEG', ...
    'output',      1);  % Kernel only: shared

% Process: Snapshot: Sources (one time)
bst_process('CallProcess', 'process_snapshot', sFilesSrcAvg, [], ...
    'target',   8, ...  % Sources (one time)
    'modality', 1, ...  % MEG (All)
    'orient',   3, ...  % top
    'time',     0.035, ...
    'Comment',  'Source maps at 35ms');

% Save and display report
ReportFile = bst_report('Save', sFilesSrcAvg);
bst_report('Open', ReportFile);

author: brainstorm-tools