predictstates.asv

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

function  xu = predictstates(x,Q);
% PURPOSE : Performs the prediction step of the sequential SIR algorithm for 
%         : the model described in the file sirdemo1.m.
% INPUTS  : - x = The state samples.
%           - t = The current time step.
%           - Q = The variance of the process noise.
% OUTPUTS : - xu = The state samples after the prediction step.

% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98

%% x = [thite;q;fai];
%% Q = [Q_thite;Q_q;Q_fai];

if nargin < 2, error('Not enough input arguments.'); end


thite=x(1,:);
qq=x(2,:);
fai=x(3,:);


Q_thite=Q(1,:);
Q_qq=Q(2,:);
Q_fai=Q(3,:);

T=9600*1/4800e3;

Ns=size(x,2);
% n_fai = sqrt(Q_fai)*randn(1,Ns);
% fai_k = fai+n_fai;
fai_k = fai;
n_qq = sqrt(Q_qq)*randn(1,Ns);
% qq_k = qq.*exp(n_qq)./(sqrt(1+2*qq.*T.*sin(thite+fai_k)+qq.^2*T^2));
qq_k = qq./(sqrt(1+2*qq.*T.*sin(thite+fai_k)+qq.^2*T^2));

n_thite = sqrt(Q_thite)*randn(1,Ns);
thite_k = atan((sin(thite)+qq.*T.*cos(fai_k))./(cos(thite)+qq.*T.*sin(fai_k))) + n_thite;

xu = [thite_k;qq_k;fai_k];


% w = sqrt(Q)*randn(size(x));
% xu = 0.5.*x + 25.*x./(1+x.^(2)) + 8*cos(1.2*(t)).*ones(size(x)) + w;