📄 test_pfnav_pcrb.m
字号:
%%
% Demo of particle fiter and Posterior Cram鑢 Rao (PCRB)
% Bounds for navigation (altimetry) with the following dynamic system :
%
% X_1 = N(X1 , Q1)
% X_{k+1} = FkXk + Uk + N(0 , Qk)
% Zk = h(Xk) + N(0 , Rk(Xk))
%
% If nb_ite variable is > 1, RMSE is compared versus PCRB
%
% Demo can be easily extented for multivariate measurements (m > 1)
%
% Author S閎astien PARIS (sebastien.paris@lsis.org)
% -------
%%
clear,clc , close all hidden;
options_affichage;
%% ---------------- Map --------------- %%
load colorado.mat
map = double(colorado);
clear colorado;
[nR , nC] = size(map);
%% ---------------- Parameters Simulation ---------------- %%
verbose = 2; %% 0 : nodisplay, 1 : map , 2 : map + measurements
N_part = 100000; % Number of particules
N_threshold = (8.0/10)*N_part;
nb_ite = 5; % Number of particule filter iteration to compute MSE & PCRB if nb_ite>1
M = 1000; % Number of paths used to evaluate PCRB
d = 4; % State dimension
m = 1; % Measurement dimension
alpha = 2.4477; % sqrt(chi2inv(0.95 , 2))
v = 100/3.6; % velocity (m/s)
cte_cov_map = 0; % Uniform measurement covariance
%% ------------ Non-Holonomic parameters ------------ %%
param.dist_th = 5000;
param.V_max = 50;
param.V_d = 0;
param.Kd = 0.03;
param.Kp = 0.0005;
param.sigmadelta = 0.00001;
param.R = 0.00001;
param.T = 400;
param.biais = 0.0;
param.D = 0.4;
param.maxite = 300;
param.maxK = 2000;
%% ------------- Measurement parameters -------------- %%
Y_min = 0.0 ; % m
Y_max = 1000000; % m
X_min = 0.0 ; % m
X_max = 1000000; % m
Z_min = 0; % Minimum elevation (m)
Z_max = 500; % Maximum elevation (m)
pas_x = 50000;
pas_y = 50000;
stepxmin = 200;
stepxmax = 200;
stepymin = 200;
stepymax = 200;
Hp = [1 0 0 0 ; 0 0 1 0 ]; %Y = Hp*X;
%% ----------- Measurement covariance noise ---------------- %%
cov_R = (10)^2; %m^2
std_R = 1; %m
cov_min = 0.3; %
cov_max = 1;
mask_R = [15 , 15]; % smoothing kernel filtering cov_map
%% ------------- State covariance noise -------------------- %%
cov_ry = (50000)^2; %m %200
cov_vy = (1)^2; %m/s %0.2
cov_rx = (50000)^2; %m
cov_vx = (1)^2; %m/s
cov_q = (5*10e-4)^2; % (m/s)^2
Q1 = [cov_ry 0 0 0 ; 0 cov_vy 0 0 ; 0 0 cov_rx 0; 0 0 0 cov_vx];
F = @(dt) [1 dt 0 0 ; 0 1 0 0 ; 0 0 1 dt ; 0 0 0 1 ];
Q = @(dt,cov_q) [dt.^3/3 dt.^2/2 0 0 ; dt.^2/2 dt 0 0 ; 0 0 dt.^3/3 dt.^2/2 ; 0 0 dt.^2/2 dt ].*cov_q;
%% -------------- Scale and Normalize map and Measurement covariance map -------------- %%
diff_Y = abs(Y_max - Y_min);
diff_X = abs(X_max - X_min);
pente_Y = diff_Y/(nR - 1);
pente_X = diff_X/(nC - 1);
vect_X = (X_min : pente_X : X_max);
vect_Y = (Y_min : pente_Y : Y_max);
X = zeros(d , length(vect_X)*length(vect_Y));
[x , y] = ndgrid(vect_X , vect_Y );
X([1 , 3] , :) = [y(:) , x(:)]';
hrx = diff_X/pas_x; %m
hry = diff_Y/pas_y; %m
h = [hry ; hrx ];
hxmin = -diff_X/stepxmin;
hxmax = diff_X/stepxmax;
hymin = -diff_Y/stepymin;
hymax = diff_Y/stepymax;
hmin = [hxmin ; hymin];
dh = [hxmax - hxmin ; hymax - hymin];
mini = min(map(:));
maxi = max(map(:));
map = Z_min + (Z_max - Z_min)*(map - mini)/(maxi - mini);
if (~cte_cov_map)
[fx , fy] = gradient(map);
norme = sqrt(fx.*fx + fy.*fy);
mintemp = min(norme((norme~=0)));
norme((norme==0)) = mintemp;
covnorma = (1/prod(mask_R))*conv2(1./(norme).^(0.5) , ones(mask_R) , 'same');
covnorma = (covnorma - min(covnorma(:)))/(max(covnorma(:)) - min(covnorma(:)));
covnorma = cov_min + covnorma*(cov_max - cov_min);
noise_covnorma = std_R*rand(nR , nC);
covmap = cov_R*covnorma + noise_covnorma;
else
covmap = cov_R*ones(nR , nC);
end
%% --------------------- Generate Reference Trajectory --------------------- %%
figure(1)
imagesc(vect_X , vect_Y , map)
colormap(colmap);
brighten(bright)
eval([ 'axis ' axes_orientation ]);
xlabel('X (m)' ,'Fontname' , font_name_xlabel , 'FontSize',font_size_xlabel , 'FontName' , font_name_text);
ylabel('Y (m)','Fontname' , font_name_ylabel , 'FontSize' , font_size_ylabel , 'FontName' , font_name_text);
title('Altimetry map', 'fontname' , font_name_title , 'fontsize' , font_size_title)
drawnow
Xpoints = getline(gcf)';
figure(1)
hold on
plot(Xpoints(1 , :) , Xpoints(2 , :) , 'r-+')
Y_ref = generate_trajectory(Xpoints , param);
Y_ref = Y_ref([1 , 2] , :);
pos_start = Y_ref(: , 1);
pos_finish = Y_ref(: , end);
K = size(Y_ref , 2);
plot(Y_ref(1 , :) , Y_ref(2 , :) , 'k');
hold off
title(sprintf('Altimetry map, K = %d', K) , 'fontname' , font_name_title , 'fontsize' , font_size_title)
drawnow
%% --------------------- Retrieve Law commands from Y_ref -------------------------- %%
[dt_ref , cap_ref] = dt_cap(Y_ref , v , 1);
[X1 , U_ref] = commande(Y_ref , v , dt_ref , cap_ref);
[F_ref , Q_ref] = modelFQ(F , Q , dt_ref , cov_q , d);
t_ref = cumsum([0 , dt_ref]);
%% --------------------- Run navigation Particle Filter -------------------------- %%
X_pf = zeros(d , K , nb_ite);
X_traj = zeros(d , K , nb_ite);
Z_traj = zeros(nb_ite , K);
nb_redist = zeros(1 , nb_ite);
for i = 1 : nb_ite
disp(sprintf('nb_ite = %d/%d' , i , nb_ite));
X_traj(: , : , i) = generate_traj(X1 , Q1 , F_ref , U_ref , Q_ref);
Y_traj = Hp*X_traj(: , : , i);
[Z_traj(i , :) , Z_nav] = nav_measure(Y_traj , map , covmap , Y_min , Y_max , X_min , X_max);
X_pf(: , : , i) = pf_nav(Z_traj(i , :) , X1 , Q1 , dt_ref , map , covmap , Hp , F_ref , U_ref , Q_ref , N_part , N_threshold , Y_min , Y_max , X_min , X_max , verbose , Y_traj , Z_nav) ;
drawnow
end
%% -------------------------------- Empirical MSE -------------------------- %%
res = (sum((X_pf - X_traj).^2 , 3)/nb_ite);
RMSE = sqrt(sum(res));
%% --------------------------------- PCRB ---------------------------------- %%
if(nb_ite > 1)
[tr , PCRB , inv_J] = pcrb(map , covmap , X1 , Q1 , F_ref , U_ref , Q_ref , M , Y_min , Y_max , X_min , X_max , Hp , h);
trace_pcrb = sqrt(tr);
end
%% ----------- Display results -------% %
fig1 = figure(1);
[x , y] = ndellipse(X1 , Q1 , Hp , alpha);
set(gcf , 'renderer' , 'zbuffer' , 'doublebuffer' , 'on');
imagesc(vect_X , vect_Y , map);
colormap(colmap);
brighten(bright)
hold on
p2 = plot(pos_start(1) , pos_start(2) , 'm^' , 'markersize' , marker_size_plot , 'linewidth' , line_width);
p3 = plot(pos_finish(1) , pos_finish(2) , 'c^' , 'markersize' , marker_size_plot , 'linewidth' , line_width);
p4 = plot(squeeze(X_pf(1 , : , :)) , squeeze(X_pf(3 , : , :)) , '-+', 'markersize' , 4 , 'linewidth' , 1);
p5 = plot(squeeze(X_traj(1 , : , :)) , squeeze(X_traj(3 , : , :)) , '-o' , 'markersize' , 4 , 'linewidth' , 1);
p6 = plot(Y_ref(1 , :) , Y_ref(2 , :) , 'y' , 'linewidth' , 2)
plot(x , y , 'k' , 'linewidth' , 2 )
hold off
eval([ 'axis ' axes_orientation ]);
xlabel('X (m)' ,'Fontname' , font_name_xlabel , 'FontSize',font_size_xlabel);
ylabel('Y (m)','Fontname' , font_name_ylabel , 'FontSize' , font_size_ylabel);
h = legend([p2 , p3 , p4(1) , p5(1) ] , 'Start' , 'End' , 'Reference path' , 'Estimated path' );
set(h , 'fontsize' , font_size_legend , 'fontname' ,font_name_legend);
fig2 = figure(2);
plot(t_ref , Z_traj' , 'linewidth' , 2)
grid on
title('Altimetric measurements');
if(nb_ite > 1)
fig3 = figure(3);
semilogy(t_ref , RMSE , t_ref , trace_pcrb , 'linewidth' , 2)
xlabel('(m)' ,'Fontname' , font_name_xlabel , 'FontSize',font_size_xlabel);
ylabel('PCRB^{0.5} , Cov(x_k|Z_{1:K})^{0.5}' ,'Fontname' , font_name_ylabel , 'FontSize' , font_size_ylabel)
title(sprintf('N_{ite} = %d, N_{part} = %d' , nb_ite , N_part))
h = legend('Cov(x_k|Z_{1:K})^{0.5}' , 'PCRB^{0.5}')
set(h , 'fontsize' , font_size_legend , 'fontname' ,font_name_legend);
grid on
end
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -