function [mse_u, mse_v] = TheoryMSE(uInit,vInit,t,vs,m0,Q,fc,c)
% [mse_u, mse_v] = TheoryMSE(uInit,vInit,t,vs,m0,Q,fc,c)
%
% Theoretical performance in MSE
%
% Input:
% uInit:    initial guess of object position.
% vInit:    initial guess of object velocity.
% t:        (Dim x M), sensor positions when emitting signals, M = num. of sensors.
% vs:       (Dim x M), sensor velocities
% m0:       (2M x 1), true values of time delay and Doppler shift
%           measurements.
% Q:        (2M x 2M), covariance matrix structure of measurement noise.
% fc:       (M x 1), sensor carrier frequencies.
%
% Output:
% mse_u:     theoretical mse of object position estimate.
% mse_v:     theoretical mse of object velocity estimate.
%
% The program can be used for 2-D(Dim=2) or 3-D(Dim=3) localization directly.
%
% Reference:
% T. Jia, K. C. Ho, H. Wang and X. Shen, "Localization of a moving object
% with sensors in motion by time delays and Doppler shifts," IEEE Trans.
% Signal Process., vol. 68, pp. 5824-5841, 2020.
%
% Tianyi Jia and K. C. Ho   11-08-2020
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

warning off;

[K,M]=size(t);          % K=localization dimension, M=number of sensors

%% only time measurement is available
if length(Q)==M
    mk=zeros(M,1);   % time measurements in the k-th step
    % Construct measurements in the k-th step
    ukmt=uInit-t;    % uk-t
    L=zeros(1,M);      % L(1,i)=norm(uki-ti) and L(2,i)=norm(uki-si)
    for i=1:M
        L(1,i)=norm(ukmt(:,i));    % L(1,i)=norm(uki-ti)
        mk(i)=(2*L(1,i))/c;
    end
    %% Compute the parameter
    Gk=zeros(K,M);
    for i=1:M
        Gk(:,i)=2*ukmt(:,i)/L(i)/c; %dt/duk
    end
    %% Gauss-Newton iteration
    Pt=inv(Gk*inv(Q)*Gk')*Gk*inv(Q);
    mse_u=trace((Pt*(m0*m0'+Q+mk*mk'-m0*mk'-mk*m0')*Pt'));
    mse_v=0;
else
    %% hybrid time and Doppler localization
    xk=[uInit;vInit];
    mk=zeros(2*M,1);   % time measurements in the k-th step
    
    % Construct measurements in the k-th step
    ukmt=xk(1:K)-t;    % uk-t
    L=zeros(1,M);      % L(1,i)=norm(uki-ti) and L(2,i)=norm(uki-si)
    for i=1:M
        L(1,i)=norm(ukmt(:,i));    % L(1,i)=norm(uki-ti)
        mk(i)=(2*L(1,i))/c;
        mk(i+M)=2*ukmt(:,i)'/L(1,i)*(xk(K+1:2*K)-vs(:,i))*fc(i)/c;
    end
    %% Compute the parameter
    Gk=zeros(2*K,2*M);
    for i=1:M
        Gk(1:K,i)=2*ukmt(:,i)/L(i)/c; %dt/duk
        Gk(K+1:2*K,i)=zeros(K,1);   %dt/dvk
        Gk(1:K,i+M)=(2*(xk(K+1:2*K)-vs(:,i))/L(i)-2*ukmt(:,i)'*(xk(K+1:2*K)-vs(:,i))*ukmt(:,i)/L(i)^3)*fc(i)/c;
        Gk(K+1:2*K,i+M)=(2*ukmt(:,i)/L(i))*fc(i)/c;
    end
    
    %% Gauss-Newton iteration
    Ptf=inv(Gk*inv(Q)*Gk')*Gk*inv(Q);
    MSEseta=Ptf*(m0*m0'+Q+mk*mk'-m0*mk'-mk*m0')*Ptf';
    mse_u=trace(MSEseta(1:K,1:K));
    mse_v=trace(MSEseta(K+1:2*K,K+1:2*K));
end

end

