importanceweights.asv

采用误差校正算法改进的粒子滤波器

function q = importanceweights(xPre,y,R,qPre);
% 目标  ：  根据SIR算法权值递推公式得到当前每个采样粒子的归一化权值
% 输入  ：  - xPre = 当前状态预测值
%           - y = 当前测量值
%           - R = 测量噪声方差
%           - qPre = 前一时刻的权值
% 输出  ：  - q = 归一化权值




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

% 计算每个采样粒子对应的权值
% ==========================

[rows,cols] = size(xPre);
q = zeros(size(xPre));

m = (xPre.^(2))./20;
qWeight = exp(-.5*R^(-1)*(y.*ones(rows,1)-m).^(2));
% qWeight = abs(y.*ones(rows,1)-m);
% 高斯概率密度分布函数,当状态初始值很大时，sum(qWeight)可能趋近于零，产生溢出



q(:,1) = qPre(:,1).*(qWeight/sum(qWeight));
qSum = sum(q(:,1));

% 权值归一化
% ==========

q(:,1) = q(:,1) / qSum;