inital_states.asv

来自「对运动声目标进行航迹跟踪」· ASV 代码 · 共 31 行

% function  xu = inital_states(xu,xuvar);
function  xu = inital_states(samnum,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);

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 = arctan(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;

xu = [xu_thite;xu_Q;xu_fai];