%???????????????????????????????????????????????????????????%?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