my_importanceweights3.m

来自「用matlab程序编写的一个三维雷达跟踪粒子滤波器」· M 代码 · 共 37 行

function q =my_importanceweights3(xu,y,R);
% 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.
% how to invoke   t=1:49

% q(:,t+1) = importanceweights(xu(:,t),y(t+1,1),R);  y is a number here.


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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

syms F_beta  F_r  F_alpha  r alpha  beta  w v xc yc zc;
F_r=sqrt(xc.^2+yc.^2+zc.^2);
F_alpha=atan(yc./xc);
F_beta=atan(zc./sqrt(xc.^2+yc.^2));
w=[F_r;F_alpha;F_beta];
v=[xc,yc,zc];

[rows,S] = size(xu);          % xu 5-500 Matrix
q = zeros(size(xu));           % q 1-500 
for i=1:S,
    h_xu(:,i)=subs(w,{xc,yc,zc},{xu(1,i) xu(3,i) xu(5,i)});   % h_xu 3-500;      
    unormalized_q(1,i)=exp(-.5*(y-h_xu(:,i))'* R^(-1) *(y- h_xu(:,i)));  %Note here y is a number not a vector
end;
q_sum=sum(unormalized_q);
q=unormalized_q./q_sum;