function [llk, penalty,AcP, BcP, AX, BX, K0Q_cP, K1Q_cP, rho0_cP, rho1_cP, cP,llkQ] = ...
    Q_likelihood(yields_o, W, K1Q_X, rinfQ, Sigma_cP, mats, dt,L,sigma_e)
%function [llk, AcP, BcP, AX, BX, K0P_cP, K1P_cP, sigma_e, K0Q_cP, K1Q_cP, rho0_cP, rho1_cP, cP, llkP, llkQ] = ...
%    jszLLK(yields_o, W, yields_woe, K1Q_X, rinfQ, Sigma_cP, mats, dt, K0P_cP, K1P_cP, sigma_e, rankRP)
%
%
% This function computest the likelihood for a Gaussian term structure.
% See "A New Perspective on Gaussian Dynamic Term Structure Models" by Joslin, Singleton and Zhu
%
% INPUTS:
% yields_o   : (T+1)*J,  matrix of observed yields (first row are t=0 observations, which likelihood conditions on)
% mats       : 1*J,      maturities in years
% dt         : scalar,   length of period in years
%
% W          : N*J,      vector of portfolio weights to fit without error.
% K1Q_X      : N*N,      normalized latent-model matrix (does not have to be diagonal, see form below)
% rinfQ      : scalar,   long-run mean under Q, of the annualized short rate
% Sigma_cP   : N*N,      positive definite matrix that is the covariance of innovations to cP
%
% OPTIONAL INPUTS:
% K0P_cP     : N*1,      OPTIONAL (supply [] to omit)
% K1P_cP     : N*N,      OPTIONAL (supply [] to omit)
% sigma_e    : scalar,   standard error of yield observation errors
%
%
% Compute likelihood conditioned on first observation!
%
% llk        : T*1       time series of -log likelihoods (includes 2-pi constants)
% AcP        : 1*J       yt = AcP' + BcP'*Xt  (yt is J*1 vector)
% BcP        : N*J       AcP, BcP satisfy internal consistency condition that AcP*W' = 0, BcP*W' = I_N
% AX         : 1*J       yt = AX' + BX'*Xt  
% BX         : N*J       Xt is the 'jordan-normalized' latent state
%
%
% The model takes the form:
%   r(t) = rho0_cP + rho1_cP'*cPt
%        = rinfQ + 1'*Xt  (Xt is the 'jordan-normalized' state
%        = 1 period discount rate (annualized)
%
% Under Q:
%   X(t+1) - X(t)   =          K1Q_X*X(t)  + eps_X(t+1),   cov(eps_X(t+1)) = Sigma_X
%   cP(t+1) - cP(t) = K0Q_cP + K1Q_cP*X(t) + eps_cP(t+1),  cov(eps_cP(t+1)) = Sigma_cP
%   where Sigma_X is chosen to match Sigma_cP 
%
% Under P:
%   cP(t+1) - cP(t) = K0P_cP + K1P_cP*X(t) + eps_cP(t+1),  cov(eps_cP(t+1)) = Sigma_cP
%
% Model yields are given by:
%   yt^m = AcP' + BcP'*cPt  (J*1)
% And observed yields are given by:
%  yt^o = yt^m + epsilon_e(t)
% where V*epsilon_e~N(0,sigma_e^2 I_(J-N))
% and V is an (J-N)*J matrix which projects onto the span orthogonal to the
% row span of W.  This means errors are orthogonal to cPt and cPt^o = cPt^m.
%
    
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Setup 
[T,J] = size(yields_o(L+1:end,:));
% Setup W if we are using individual yields without error:

N = size(W,1);
cP = yields_o*W'; % (T+L)*N, cP stands for math caligraphic P.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% COMPUTE THE Q-LIKELIHOOD:
% First find the loadings for the model:
% yt = AcP' + BcP'*cPt, AcP is 1*J, BcP is N*J
[BcP, AcP, AX, BX, DUMMY,penalty] = jszLoadings(W, K1Q_X, rinfQ, Sigma_cP(1:3,1:3), mats, dt);
[K0Q_cP, K1Q_cP, rho0_cP, rho1_cP] = jszRotation(W, K1Q_X, rinfQ, dt, [], [], BX, AX);
yields_m =  ones(T+L,1)*AcP + cP*BcP; % (T+1)*J, model-implied yields
yield_errors = yields_o(L+1:end,:) - yields_m(L+1:end,:); % T*J
square_orthogonal_yield_errors = (yield_errors).^2; % T*J, but N-dimensional projection onto W is always 0, so effectively (J-N) dimensional


llkQ = .5*sum(square_orthogonal_yield_errors.')/sigma_e^2 + (J-N)*.5*log(2*pi) + .5*(J-N)*log(sigma_e^2); % 1*T

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
llk = (llkQ);


