importanceweights.m

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

% function q = importanceweights(Xs_fft,id_v,xu,thite);
function q = importanceweights(Xs_fft,id_v,xu);
% PURPOSE : Computes the normalised importance ratios for the 
%           model described in the file sirdemo1.m.
% INPUTS  : - xu = The predicted state samples.
%           - y = The output measurements.
%           - R = The measurement noise covariance.
% OUTPUTS : - q = The normalised importance ratios.

% AUTHOR  : Nando de Freitas - Thanks for the acknowledgement :-)
% DATE    : 08-09-98
if nargin < 3, error('Not enough input arguments.'); end

thite=xu(1,:);
sigma=1;
M=length(id_v);
P=6;
K=1;
rows = length(thite);
q = zeros(rows,1);
d_a=0.04;
vs=345;
L=6;
fs=48e3;
num1=1024;
f_s=fs/num1;
id_l=length(id_v);

for s=1:rows
     
  [Yw,Aw,Sw] = matrix_solve(Xs_fft,id_v,thite(s));
   
  
% %   
%       F_y=0;
%       
%    for lp2 = 1:id_l
%          Xw=Xs_fft(id_v(lp2),:).';
%          fk=id_v(lp2)*f_s;
%          af=steering_f(fk,d_a,vs,L,thite);
% %          Se=L;
%            Se=inv(af'*af)*af'*Xw;
% %          Se=af'*af;
% % -af*Se
% % -af*Se
% % (1/(pi*sima^2)^P)*
% % exp
% %          fy=exp(((Xw-af*Se)'*(Xw-af*Se)))
%          fy=log(1/(pi*sima^2)^P)+(-(1/sima^2)*((Xw-af*Se)'*(Xw-af*Se)));
% %          fy=((Xw-af*Se)'*(Xw-af*Se))
% %          fy=real(Xw'*(ones(L,L)-af*inv(af'*af)*af')*Xw)
%          F_y=F_y+fy;
%          
%      end
%      
%   q(s,1)=(2*pi*sima^2)^(-M*P)*exp(-(Yw-Aw*Sw)'*(Yw-Aw*Sw)/(2*sima^2));
%          q(s,1)=(F_y)
%     q(s,1) = [Yw'*(eye(M*P,M*P)-Aw*inv(Aw'*Aw)*Aw')*Yw].^(-(M*(P-K)));
     q(s,1) =exp((-(Yw'*(eye(M*P,M*P)-Aw*inv(Aw'*Aw)*Aw')*Yw)./sigma^2));
  
end;
q_sum=sum(q)
q = (q)./q_sum
% q=exp(q)./sum(exp(q))
% subplot(224); 
% plot(thite,q,'+')      
% ylabel('Likelihood function','fontsize',15);
% xlabel('Hidden state support','fontsize',15)
% axis([-30 30 0 0.03]);