function [F, Q, R, x_T, P_T] = em_algo(y, G)
    % y_t = G_t'*x_t + v_t    1*1 = 1*p p*1
    % x_t = F*x_t-1 + w_t     p*1 = p*p p*1
    % G is T*p
    p = size(G,2); % p = nb assets ; G = T*p
    q = size(y,2); % q = nb observations ; y = T*q
    T = size(y,1); % y is T*1
    F = eye(p); % = Transition matrix  p*p
    Q = eye(p); % innovation (v) covariance matrix p*p
    R = eye(q); % noise (w) covariance matrix q x q
    x_T_old = zeros(p,T);
    mu0 = zeros(p,1);
    Sigma = eye(p); % Initial state covariance matrix p*p
    converged = 0;
    i = 0;
    max_iter = 60; % only for testing purposes
    while ~converged
        if i > max_iter
            break;
        end
        % E step = smoothing
        fprintf('Iteration %d\n',i);
        [x_T,P_T,P_Tm2] = smoother(G,F,Q,R,mu0,Sigma,y);
        %x_T

        % M step
        A = zeros(p,p);
        B = zeros(p,p);
        C = zeros(p,p);
        R = eye(q);

        for t = 2:T % eq (9) in EM paper
            A = A + (P_T(:,:,t-1) + (x_T(:,t-1)*x_T(:,t-1)'));
        end

        for t = 2:T % eq (10)
            %B = B + (P_Tm2(:,:,t-1) + (x_T(:,t)*x_T(:,t-1)'));
            B = B + (P_Tm2(:,:,t) + (x_T(:,t)*x_T(:,t-1)'));
        end

        for t = 1:T %eq (11)
            C = C + (P_T(:,:,t) + (x_T(:,t)*x_T(:,t)'));
        end

        F = B*inv(A); %eq (12)
        Q = (1/T)*(C - (B*inv(A)*B')); % eq (13)  pxp

        for t = 1:T
            bias = y(t) - (G(t,:)*x_T(:,t));
            R = R + ((bias*bias') + (G(t,:)*P_T(:,:,t)*G(t,:)'));
        end
        R = (1/T)*R;

        if i>1
            err = norm(x_T-x_T_old)/norm(x_T_old);
            if err < 1e-4
                converged = 1;
            end
        end
        x_T_old = x_T;
        i = i+1;
    end
    fprintf('EM algorithm iterated %d times\n',i);
end