无迹粒子滤波的matlab实现

基于无迹卡尔曼滤波ukf的粒子滤波matlab程序

%?????????????????????????????????????????????????????????? % unscented_particle_filter.m
%?????????????????????????????????????????????????????????? % Unscented particle filter application
%
% A particle filter based on the unscented transform that
% estimates the target state X_k = [x_k y_k x_dot_k y_dot_k]
% based on observations of its range and range rate from
% two independent radar/sonar sensors.
%
% Copyright Sandeep Sira 2008
%
%?????????????????????????????????????????????????????????? clear
close all;
clc;
clear all;

% Initialization
X_0 = [0 0 10 150]‘; % Initial target state
a = [0 2000]; % x?y coordinates of Sensor A
b = [3000 1500]; % x?y coordinates of Sensor B
dT = 2; % Sampling interval in seconds
endk = 50; % Number of sampling epochs
R = diag（[10 4 10 4]）; % Covariance matrix of sensor observation
% error
q = 0.5; % Process noise intensity parameter
Ns = 10; % Number of particles
P_0 = diag（[1000 100 1000 100]）;% Initial covariance
% UPF parameters
na = 4; kappa = 4; alpha = 1; beta = 0;
lambda = alpha^2*（ na+kappa） -na;
% State transition matrix
F = [1 0 dT 0;
0 1 0 dT;
0 0 1 0;
0 0 0 1];
% Process noise covariance matrix
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];
sqrtm_Q = sqrtm（Q）;
inv_Q = inv（Q）;
inv_R = inv（R）;
sqrtm_R = sqrtm（R）;
% Generate a target trajectory
X（:1） = X_0;
for k = 2: endk
X（:k） = F*X（:k-1） + sqrtm_Q * randn（4 1）;
end
% Sample an initial value of the target state. This is the mean of the
% density P（X_0）
XHat （:1） = X_0 + sqrtm（P_0）* randn（4 1）;
% Sample the particles from the initial density
Xp = XHat*ones（1Ns） + sqrtm（P_0）*randn（4Ns）;
% Mean and covariance of particles
mx = mean（Xp 2）* ones（1Ns）; err_X = Xp -mx;
for j = 1:Ns
P_xx（::j） = err_X（:j） * err_X（:j）‘;
end
% Commence unscented particle filtering loop
for k = 2: endk
% Project the particles forward
Xp_in = F* Xp;
% Get the observation
zeta = X（:k）;
da_true = norm（ zeta （[1:2]） -a‘）;
db_true = norm（ zeta （[1:2]） -b‘）;
va_true = zeta （[3:4]）‘ * （zeta （[1:2]） -a‘）/ da_true;
vb_true = zeta （[3:4]）‘ * （zeta （[1:2]） -b‘）/ db_true;
Zk = [ da_true; va_true; db_true; vb_true] + sqrtm_R * randn （4 1）;
for j = 1:Ns
% Obtain the sigma points
P（::） = P_xx（::j）;
sqrt_P_xx = real（sqrtm（ （na + lambda ）*P ））;
chi_X （::） = mx（:j）* ones （1 2* na+1） + ...
[ zeros（41）  sqrt_P_xx  -sqrt_P_xx ];
w_chi = [ kappa ones（12* na）/2 ] / （na + kappa ）;
% Propagate the sigma points
chi_X = F* chi_X + sqrtm_Q * randn （4 2* na +1）;
% Calculate mean and variance
mx（:j） = chi_X * w_chi‘;
err_chi_X = chi_X -mx（:j）* ones（1 2* na +1）;
PHat = err_chi_X *（err_chi_X.*（ ones（4 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 9））.^ 2 ） ）;
chi_X_db = sqrt（ sum（（chi_X（[1 2] :） -b‘* ones（1 9））.^ 2 ） ）;
chi_X_va = diag（chi_X（[3:4] :）‘ ...
*（ chi_X（[1:2]

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5531  2010-05-22 08:49  upf.m

----------- ---------  ---------- -----  ----

                 5531                    1