predict_states.asv

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

function  xx = predict_states(xu,xuvar);

thite = xu(1);%% the angle of arrival;
Qv = xu(2);%% the vector of v/r;
fai = 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;

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;

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

xx = [xu_thite;xu_Q;xu_fai];