inital_states.m

对运动声目标进行航迹跟踪

function  xx = inital_states(ini_xu,xuvar,samnum);
% function  xx = inital_states(samnum,xuvar);
% 
thite = ini_xu(1);%% the angle of arrival;
Qv = ini_xu(2);%% the vector of v/r;
fai = ini_xu(3);%% the heading direction;

Rthite = xuvar(1);%% the variance of angel-of-arrival;
RQv = xuvar(2);%% the variance of Q;
Rfai = xuvar(3);%% the variance of heading direction;

% samnum = size(xu,2);
T=9600*1/4800e3;

nthite = sqrt(Rthite)*randn(1,samnum);
thite_a=sin(thite)+exp(Qv).*T.*cos(fai);
thite_b=cos(thite)+exp(Qv).*T.*sin(fai);
xu_thite = atan(thite_a/thite_b) + nthite;
% xu_thite =nthite;

nQv = sqrt(RQv)*randn(1,samnum);
Q_a=2*exp(Qv).*T.*sin(thite+fai)+(exp(Qv)*T).^2;
xu_Q = Qv-0.5*log(1+Q_a) + nQv;
% xu_Q =nQv;

nfai = sqrt(Rfai)*randn(1,samnum);
xu_fai = fai + nfai;
% xu_fai = nfai;

xx = [mean(xu_thite);mean(xu_Q);mean(xu_fai)];