无迹粒子滤波算法(UPF)
基本粒子滤波 PF 的重要密度函数是先验概率密度，这种方式没有利用最新的观测信息 ，以致于状态变量的估计过分依赖于模型。假使人们建立的系统模型无法准确的表达系统，尤其是当噪声复杂多变时，滤波算法的精度将会大大降低。无味卡尔曼滤波（UKF）是一种基于递归思想的最小均方误差估计，它在计算均值和方差的过程中，会利用最新的观测信息；同时 UKF 利用无味变换，计算后验方 差的精度理论上能达到三阶，所以 UKF 如果能用来改善粒子滤波，通过将 PF 和 UKF 相结合， 标准粒子滤波算法被改进为无味粒子滤波（UPF）。
为了能够兼顾粒子滤波的精度、粒子退化及粒子贫化问题，本文从优化重采样及选择一个合适的重要密度函数两方面着手。
参考文献：
【1】非线性滤波技术研究及其在深空探测自主导航中的应用
【2】面向目标跟踪的非线性滤波算法性能分析研究
【3】Compass/INS 组合导航信息融合滤波算法研究
【4】粒子滤波算法研究与实现
【5】The Unscented Particle Filter
主函数 Unscented_Particle_Filter.m
%% 基于两个独立的雷达/声纳传感器对它的射程和射程速率的观察
close all;
clc;
clear;
tic;
global Ns na F Q R a b endk dT
%% 设置参数
endk = 50; % 采样时期数
Ns = 10; % 粒子数
na = 4;
% 状态转移矩阵
dT = 2; % 采样间隔（秒）
F = [ 1,0,dT,0;
0,1,0,dT;
0,0,1,0;
0,0,0,1 ];
% 过程噪声协方差矩阵
q = 0.5; % 过程噪声强度参数
Q = q * [ dT^3/3,0,dT^2/2,0;
0,dT^3/3,0,dT^2/2;
dT^2/2,0,dT,0;
0,dT^2/2,0,dT ];
% 传感器观测的协方差矩阵
R = diag([10,4,10,4]);
%% 初始化粒子、粒子协方差、粒子均值
X_0 = [0;0;10;150]; % 初始目标状态
P_0 = diag([1000,100,1000,100]); % 初始协方差矩阵
[P_xx,mx,Xp] = Initialize(P_0,X_0);
%% 生成真实状态和测量值
a = [0;2000]; % 传感器A的初始x,y坐标
b = [3000;1500]; % 传感器B的初始x,y坐标
[X,Zk] = Generate_real_measure_state(X_0,a,b);
%% 第一个估计值
XHat(:,1) = X_0 + sqrtm(P_0) * randn(na,1); % 采样目标状态的初始值,这是密度 P(X_0)的平均值
%% 开始无味卡尔曼粒子滤波
h = waitbar(0,' Time Loop ');
for k = 2 : endk
%% 向前投射粒子，用于计算 w2
Xp_in = F * Xp;
%% 赋值每步测量值
z = Zk(:,k);
%% 重要性采样，每个粒子Unscented变换
w3 = zeros(1,Ns);
for j = 1 : Ns
P_Particle = P_xx(:,:,j);
x_mean_Particle = mx(:,j);
[P_Particle,x_mean_Particle] = Important_sample_UPF(P_Particle,x_mean_Particle,z);
P_xx(:,:,j) = P_Particle;
mx(:,j) = x_mean_Particle;
Xp(:,j) = x_mean_Particle + real(sqrtm(P_Particle)) * randn(na,1);
w3(j) = exp(-0.5 * transpose(Xp(:,j) - x_mean_Particle) * inv(P_Particle) * (Xp(:,j) - x_mean_Particle) );
end
%% 计算权重 w1,w2,w3 -> w
% w3 from Unscented
% w2 from Xp-Xp_in
% w1 from compare z and Particles
da = sqrt( sum((Xp(1:2,:) - a * ones(1,Ns)).^2 ));
db = sqrt( sum((Xp(1:2,:) -b * ones(1,Ns)).^2 ));
va = diag(Xp(3:4,:)' * ( Xp(1:2,:) - a * ones(1,Ns)))' ./ da;
vb = diag(Xp(3:4,:)' * ( Xp(1:2,:) - b * ones(1,Ns)))' ./ db;
innov = z * ones(1,Ns) - [ da; va; db; vb ];
% Likelihood component
w1 = diag( exp(-0.5 * innov' * inv(R) * innov ) )';
% Kinematic prior component
w2 = diag( exp(-0.5 * ( Xp - Xp_in)'* inv(Q) * (Xp - Xp_in) ) )';
w = w1 .* w2 ./ w3;
%% 归一化
if (sum(w) == 0)
fprintf('Alarm weights = 0 at time step %d\n', k);
w = ones(1,Ns)/Ns;
end
w = w / sum(w);
%N_eff = 1 /(w * w');
%% 多项式重采样算法
[P_xx,mx,Xp,w] = Multinomial_resample(P_xx,mx,Xp,w);
%% 残差重采样算法
%[P_xx,mx,Xp,w] = Residual_resample(P_xx,mx,Xp ,w);
%% 计算估计值
XHat(:,k) = sum(Xp,2) / Ns;
%% 进度条
waitbar(k / endk, h, sprintf('%s%%', num2str(100 * k / endk)));
%% 画图分析
DrawFilter(X,XHat,Xp,k);
end
%% 计算均方根误差RMSE,并画图分析
DrawRMSE(XHat,X)
%% 结束计时
close(h);
toc;
Initialize.m
function [P_xx,mx,Xp] = Initialize(P_0,X_0)
global na Ns
%% Introduction-----------------------------------
% 函数功能 : 初始化采样粒子、粒子均值、粒子协方差
% Input : P_0 状态协方差矩阵初值
% X_0 初始状态
% Output : P_xx 初始粒子协方差
% mx 初始粒子均值
% Xp 初始粒子群
%------------------------------------------------------
%% 分配空间
P_xx = zeros(na,na,Ns);
%% 从初始密度采样粒子
Xp = X_0 * ones(1,Ns) + sqrtm(P_0) * randn(na,Ns);
% 粒子的均值
mx = mean(Xp ,2) * ones(1,Ns);
% 粒子的协方差
err_X = Xp - mx;
for j = 1:Ns
P_xx(:,:,j) = err_X(:,j) * err_X(:,j)';
end
end
Generate_real_maesure_state.m
function [X,Zk] = Generate_real_measure_state(X_0,a,b)
global F Q R endk na
%% Introduction-----------------------------------
% 函数功能 : 生成真实状态和测量值
% Input : a 传感器初始值
% b 传感器初始值
% X_0 初始状态
% Output : X 真实状态
% ZK 测量值
%------------------------------------------------------
%% 分配空间
X = zeros(na,endk);
Zk = zeros(na,endk);
X(:,1) = X_0;
Zk(:,1) = zeros(na,1);
%% 循环
for k = 2 : endk
% 生成真实状态
X(:,k) = F * X(:,k-1) + sqrtm(Q) * randn(na,1);
% 生成观测值
zeta = X(:,k);
da_true = norm(zeta(1:2) - a);
db_true = norm(zeta(1:2) - b);
va_true = transpose(zeta(3:4)) * (zeta(1:2) - a) / da_true;
vb_true = transpose(zeta(3:4)) * (zeta(1:2) - b) / db_true;
Zk(:,k) = transpose([da_true,va_true,db_true,vb_true]) + sqrtm(R) * randn (na,1);
end
end
Important_sample_UPF.m
function [P_Particle,x_mean_Particle] = Important_sample_UPF(P_Particle,x_mean_Particle,Zk)
global na F Q R a b
%% Introduction-----------------------------------
% 函数功能 : 每个粒子的 Unscented 变换
% Input : P_Particle 每个粒子的状态协方差矩阵
% x_mean_Particle 每个粒子的状态均值
% Zk 每一步测量值
% Output : P_Particle 每个粒子的状态协方差矩阵
% x_mean_Particle 每个粒子的状态均值
%------------------------------------------------------
%% UPF parameters
kappa = 4;
alpha = 1;
lambda = alpha^2 * (na + kappa) - na;
%% 每个粒子的 Unscented 变换
% Obtain the sigma points
sqrt_P_xx = real(sqrtm( (na + lambda ) * P_Particle));
chi_X = x_mean_Particle * ones(1 ,(2 * na + 1)) + [ zeros(na,1),sqrt_P_xx, - sqrt_P_xx ];
w_chi = [ kappa/(na + kappa),ones(1,2 * na)/2/(na + kappa) ];
% Propagate the sigma points 4x9
chi_X = F * chi_X + sqrtm(Q) * randn(na ,(2 * na + 1));
% Calculate mean and variance 4x1 4x4
x_mean_Particle = chi_X * transpose(w_chi);
err_chi_X = chi_X - x_mean_Particle * ones(1 ,(2 * na + 1));
P_Particle = err_chi_X * transpose(err_chi_X.*(ones(na,1) * w_chi ));
% Range and range rate of the sigma points as observed by the sensors
chi_X_da = sqrt( sum((chi_X([1,2],:) - a * ones(1,2*na+1)).^ 2 ) );
chi_X_db = sqrt( sum((chi_X([1,2],:) - b * ones(1,2*na+1)).^ 2 ) );
chi_X_va = diag( chi_X(3:4,:)' * ( chi_X(1:2,:) - a * ones(1,2*na+1)))' ./ chi_X_da;
chi_X_vb = diag( chi_X(3:4,:)' * ( chi_X(1:2,:) - b * ones(1,2*na+1)))' ./ chi_X_db;
chi_Z = [chi_X_da; chi_X_va; chi_X_db; chi_X_vb ];
% Calculate mean and variance 4x1 4x4
m_chi_Z = chi_Z * transpose(w_chi);
err_chi_Z = chi_Z - m_chi_Z * ones(1,2*na+1);
P_zz = err_chi_Z * (err_chi_Z.*( ones(na,1) * w_chi ))';
% Obtain P_xz
P_xz = err_chi_X * (err_chi_Z.*( ones(na,1) * w_chi ))';
% Innovations covariance and Kalman gain
P_eta = P_zz + R;
K = P_xz / P_eta;
% Update and Store for the next time step
P_Particle = P_Particle - K * P_eta * transpose(K);
x_mean_Particle = x_mean_Particle + K * (Zk - m_chi_Z);
end
Multinomial_resample.m
function [P_xx,mx,Xp,w] = Multinomial_resample(P_xx,mx,Xp,w)
global Ns
%% Introduction-----------------------------------
% 函数功能 : 重采样
% Input : P_xx 所有粒子的状态协方差矩阵
% mx 所有粒子的状态均值
% Xp 粒子群
% w 所有粒子权重
% Output : P_xx 所有粒子的状态协方差矩阵
% mx 所有粒子的状态均值
% Xp 粒子群
% w 所有粒子权重
%------------------------------------------------------
%% 基本重采样方法:
% 多项式重采样(Multinomial re-sampling)、分层重采样(Stratified re-sampling)、
% 系统重采样(Systematic re-sampling)、残差重采样(Residual re-sampling)
%% 多项式重采样算法 (输入：权重、粒子、粒子数)
for i = 1 : Ns
temp = 0;
u = rand;
for j = 1 : Ns % 循环直至找到权重满足 (temp >= u) 的粒子，结束循环
temp = temp + w(j); % 权重累计和
if (temp >= u) % if : temp[j] > u[i]; then : P(:, i) = Particle(:, j);
Xp(:, i) = Xp(:, j);
w(i) = 1 / Ns;
mx(:,i) = mx(:,j );
P_xx(:,:,i) = P_xx(:,:, j );
break;
end
end
end
end
Residual_resample.m
function [P_xx,mx,Xp,w] = Residual_resample(P_xx,mx,Xp ,w)
global Ns
%% Introduction-----------------------------------
% 函数功能 : 重采样
% Input : P_xx 所有粒子的状态协方差矩阵
% mx 所有粒子的状态均值
% Xp 粒子群
% w 所有粒子权重
% Output : P_xx 所有粒子的状态协方差矩阵
% mx 所有粒子的状态均值
% Xp 粒子群
% w 所有粒子权重
%------------------------------------------------------
%% 基本重采样方法:
% 多项式重采样(Multinomial re-sampling)、分层重采样(Stratified re-sampling)、
% 系统重采样(Systematic re-sampling)、残差重采样(Residual re-sampling)
%% 残差重采样算法--------------------------------------------------------
inindex = 1:Ns;
nki = Ns .* w; % 1x2500 sum(nki) = 2500 = n
nk = fix(nki); % 朝向0四舍五入 1x2500 sum(nk) < 2500 = n
nminus = Ns - sum(nk);
%%
if (nminus ~= 0)
nki = (nki - nk) / nminus; % 1x2500
p = cumsum(nki); % 计算累计和 1x2500
u = fliplr( cumprod( rand(1,nminus ).^(1./(nminus :-1:1))));%将数组从左向右翻转
j = 1;
for i = 1:nminus
while (u(1,i) > p(1,j))
j = j + 1;
end
nk(1,j) = nk(1,j) + 1;
end
end
%% output
outindex = zeros(1,Ns);
index = 1;
for i = 1:Ns
if (nk(1,i) > 0)
for j = index : index + nk(1,i) - 1
outindex(j) = inindex(i);
end
end
index = index + nk(1,i);
end
%%
for i = 1:Ns
j = outindex(i);
Xp(:,i) = Xp(:,j);
w(i) = 1 / Ns;
mx(:,i) = mx(:,j );
P_xx(:,:,i) = P_xx(:,:, j );
end
end
DrawFilter.m
function DrawFilter(X,XHat,Xp,k)
global endk dT Ns
%% 画图分析
figure(1);
clf;
plot(X(1,:), X(2,:),'b*-', XHat(1,:), XHat(2,:),'co-',Xp(1,:), Xp(2,:),'r.');
hold on;
title('X-Y position');
xlabel('X(m)');
ylabel('Y(m)');
legend('Target trajectory','Target estimate','Particles','Location','SouthEast');
grid on;
time = 0 : endk-1; %易于画图
each_time = 0 : k-1; %易于画图
figure(2);
clf;
subplot(2,1,1),
plot(time*dT,X(3,:),'b*-',each_time*dT,XHat(3,:),'co-',(k-1)*dT*ones(1,Ns),Xp(3,:),'r.');
title('Velocity');
ylabel('X m/s');
grid on;
subplot(2,1,2),
plot(time*dT , X(4,:),'b*-', each_time*dT , XHat(4,:),'co-',(k-1)*dT * ones(1,Ns), Xp(4,:),'r.');
xlabel('Sampling instant');
ylabel('Y m/s');
grid on;
end
DrawRMSE.m
function DrawRMSE(XHat,X)
global endk
%% 画图分析
Error = XHat - X;
RMSE = zeros(2,endk);
for i = 1 : endk
RMSE(1,i) = sqrt(Error(1,i)^2 + Error(2,i)^2);
RMSE(2,i) = sqrt(Error(3,i)^2 + Error(4,i)^2);
end
figure(3);
plot(0:endk-1,RMSE(1,:),'b-p');
grid on;
legend('RMSE Position');
figure(4);
plot(0:endk-1,RMSE(2,:),'b-p');
grid on;
legend('RMSE Velocity');
end
结果显示：
扫一扫
338
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
