Skip to content

T
TCHSLAM
Commit 13959d59 authored by Sensing's avatar Sensing
Browse files
Options

delete some files

parent 16b7ee79
Show whitespace changes
Inline Side-by-side
Showing with 0 additions and 832 deletions
AP Clustering/apclusterSparse.m deleted 100644 → 0
View file @ 16b7ee79
%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p)
%
% APCLUSTER uses affinity propagation (Frey and Dueck, Science,
% 2007) to identify data clusters, using a set of real-valued
% pair-wise data point similarities as input. Each cluster is
% represented by a data point called a cluster center, and the
% method searches for clusters so as to maximize a fitness
% function called net similarity. The method is iterative and
% stops after maxits iterations (default of 500 - see below for
% how to change this value) or when the cluster centers stay
% constant for convits iterations (default of 50). The command
% apcluster(s,p,'plot') can be used to plot the net similarity
% during operation of the algorithm.
%
% For N data points, there may be as many as N^2-N pair-wise
% similarities (note that the similarity of data point i to k
% need not be equal to the similarity of data point k to i).
% These may be passed to APCLUSTER in an NxN matrix s, where
% s(i,k) is the similarity of point i to point k. In fact, only
% a smaller number of relevant similarities are needed for
% APCLUSTER to work. If only M similarity values are known,
% where M < N^2-N, they can be passed to APCLUSTER in an Mx3
% matrix s, where each row of s contains a pair of data point
% indices and a corresponding similarity value: s(j,3) is the
% similarity of data point s(j,1) to data point s(j,2).
%
% APCLUSTER automatically determines the number of clusters,
% based on the input p, which is an Nx1 matrix of real numbers
% called preferences. p(i) indicates the preference that data
% point i be chosen as a cluster center. A good choice is to
% set all preference values to the median of the similarity
% values. The number of identified clusters can be increased or
% decreased by changing this value accordingly. If p is a
% scalar, APCLUSTER assumes all preferences are equal to p.
%
% The fitness function (net similarity) used to search for
% solutions equals the sum of the preferences of the the data
% centers plus the sum of the similarities of the other data
% points to their data centers.
%
% The identified cluster centers and the assignments of other
% data points to these centers are returned in idx. idx(j) is
% the index of the data point that is the cluster center for
% data point j. If idx(j) equals j, then point j is itself a
% cluster center. The sum of the similarities of the data
% points to their cluster centers is returned in dpsim, the
% sum of the preferences of the identified cluster centers is
% returned in expref and the net similarity (sum of the data
% point similarities and preferences) is returned in netsim.
%
% EXAMPLE
%
% N=100; x=rand(N,2); % Create N, 2-D data points
% M=N*N-N; s=zeros(M,3); % Make ALL N^2-N similarities
% j=1;
% for i=1:N
% for k=[1:i-1,i+1:N]
% s(j,1)=i; s(j,2)=k; s(j,3)=-sum((x(i,:)-x(k,:)).^2);
% j=j+1;
% end;
% end;
% p=median(s(:,3)); % Set preference to median similarity
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'plot');
% fprintf('Number of clusters: %d\n',length(unique(idx)));
% fprintf('Fitness (net similarity): %f\n',netsim);
% figure; % Make a figures showing the data and the clusters
% for i=unique(idx)'
% ii=find(idx==i); h=plot(x(ii,1),x(ii,2),'o'); hold on;
% col=rand(1,3); set(h,'Color',col,'MarkerFaceColor',col);
% xi1=x(i,1)*ones(size(ii)); xi2=x(i,2)*ones(size(ii));
% line([x(ii,1),xi1]',[x(ii,2),xi2]','Color',col);
% end;
% axis equal tight;
%
% PARAMETERS
%
% [idx,netsim,dpsim,expref]=apclusterSparse(s,p,'NAME',VALUE,...)
%
% The following parameters can be set by providing name-value
% pairs, eg, apcluster(s,p,'maxits',1000):
%
% Parameter Value
% 'maxits' Any positive integer. This specifies the
% maximum number of iterations performed by
% affinity propagation. Default: 1000.
% 'convits' Any positive integer. APCLUSTER decides that
% the algorithm has converged if the estimated
% cluster centers stay fixed for convits
% iterations. Increase this value to apply a
% more stringent convergence test. Default: 100.
% 'dampfact' A real number that is less than 1 and
% greater than or equal to 0.5. This sets the
% damping level of the message-passing method,
% where values close to 1 correspond to heavy
% damping which may be needed if oscillations
% occur. Default: .9
% 'plot' No value needed. This creates a figure that
% plots the net similarity after each iteration
% of the method. If the net similarity fails to
% converge, consider increasing the values of
% dampfact and maxits.
% 'details' No value needed. This causes idx, netsim,
% dpsim and expref to be stored after each
% iteration.
% 'nonoise' No value needed. Degenerate input similarities
% (eg, where the similarity of i to k equals the
% similarity of k to i) can prevent convergence.
% To avoid this, APCLUSTER adds a small amount
% of noise to the input similarities. This flag
% turns off the addition of noise.
%
% Copyright (c) Brendan J. Frey and Delbert Dueck (2006). This
% software may be freely used and distributed for
% non-commercial purposes.
function [idx,netsim,dpsim,expref]=apclusterSparse(s,p,varargin);
% Handle arguments to function
if nargin<2 error('Too few input arguments');
else
maxits=1000; convits=100; lam=0.9; plt=0; details=0; nonoise=0;
i=1;
while i<=length(varargin)
if strcmp(varargin{i},'plot')
plt=1; i=i+1;
elseif strcmp(varargin{i},'details')
details=1; i=i+1;
elseif strcmp(varargin{i},'nonoise')
nonoise=1; i=i+1;
elseif strcmp(varargin{i},'maxits')
maxits=varargin{i+1};
i=i+2;
if maxits<=0 error('maxits must be a positive integer'); end;
elseif strcmp(varargin{i},'convits')
convits=varargin{i+1};
i=i+2;
if convits<=0 error('convits must be a positive integer'); end;
elseif strcmp(varargin{i},'dampfact')
lam=varargin{i+1};
i=i+2;
if (lam<0.5)||(lam>=1)
error('dampfact must be >= 0.5 and < 1');
end;
else i=i+1;
end;
end;
end;
if lam>0.9
fprintf('\n*** Warning: Large damping factor in use. Turn on plotting\n');
fprintf(' to monitor the net similarity. The algorithm will\n');
fprintf(' change decisions slowly, so consider using a larger value\n');
fprintf(' of convits.\n\n');
end;
% Check that standard arguments are consistent in size
if length(size(s))~=2 error('s should be a 2D matrix');
elseif length(size(p))>2 error('p should be a vector or a scalar');
elseif size(s,2)==3
tmp=max(max(s(:,1)),max(s(:,2)));
if length(p)==1 N=tmp; else N=length(p); end;
if tmp>N
error('data point index exceeds number of data points');
elseif min(min(s(:,1)),min(s(:,2)))<=0
error('data point indices must be >= 1');
end;
elseif size(s,1)==size(s,2)
N=size(s,1);
if (length(p)~=N)&&(length(p)~=1)
error('p should be scalar or a vector of size N');
end;
else error('s must have 3 columns or be square'); end;
% Make vector of preferences
if length(p)==1 p=p*ones(N,1); end;
% Append self-similarities (preferences) to s-matrix
tmps=[repmat([1:N]',[1,2]),p]; s=[s;tmps];
M=size(s,1);
% In case user did not remove degeneracies from the input similarities,
% avoid degenerate solutions by adding a small amount of noise to the
% input similarities
if ~nonoise
rns=randn('state'); randn('state',0);
s(:,3)=s(:,3)+(eps*s(:,3)+realmin*100).*rand(M,1);
randn('state',rns);
end;
% Construct indices of neighbors
ind1e=zeros(N,1);
for j=1:M k=s(j,1); ind1e(k)=ind1e(k)+1; end;
ind1e=cumsum(ind1e); ind1s=[1;ind1e(1:end-1)+1]; ind1=zeros(M,1);
for j=1:M k=s(j,1); ind1(ind1s(k))=j; ind1s(k)=ind1s(k)+1; end;
ind1s=[1;ind1e(1:end-1)+1];
ind2e=zeros(N,1);
for j=1:M k=s(j,2); ind2e(k)=ind2e(k)+1; end;
ind2e=cumsum(ind2e); ind2s=[1;ind2e(1:end-1)+1]; ind2=zeros(M,1);
for j=1:M k=s(j,2); ind2(ind2s(k))=j; ind2s(k)=ind2s(k)+1; end;
ind2s=[1;ind2e(1:end-1)+1];
% Allocate space for messages, etc
A=zeros(M,1); R=zeros(M,1); t=1;
if plt netsim=zeros(1,maxits+1); end;
if details
idx=zeros(N,maxits+1);
netsim=zeros(1,maxits+1);
dpsim=zeros(1,maxits+1);
expref=zeros(1,maxits+1);
end;
% Execute parallel affinity propagation updates
e=zeros(N,convits); dn=0; i=0;
while ~dn
i=i+1;
% Compute responsibilities
for j=1:N
ss=s(ind1(ind1s(j):ind1e(j)),3);
as=A(ind1(ind1s(j):ind1e(j)))+ss;
[Y,I]=max(as); as(I)=-realmax; [Y2,I2]=max(as);
r=ss-Y; r(I)=ss(I)-Y2;
R(ind1(ind1s(j):ind1e(j)))=(1-lam)*r+ ...
lam*R(ind1(ind1s(j):ind1e(j)));
end;
% Compute availabilities
for j=1:N
rp=R(ind2(ind2s(j):ind2e(j)));
rp(1:end-1)=max(rp(1:end-1),0);
a=sum(rp)-rp;
a(1:end-1)=min(a(1:end-1),0);
A(ind2(ind2s(j):ind2e(j)))=(1-lam)*a+ ...
lam*A(ind2(ind2s(j):ind2e(j)));
end;
% Check for convergence
E=((A(M-N+1:M)+R(M-N+1:M))>0); e(:,mod(i-1,convits)+1)=E; K=sum(E);
if i>=convits || i>=maxits
se=sum(e,2);
unconverged=(sum((se==convits)+(se==0))~=N);
if (~unconverged&&(K>0))||(i==maxits) dn=1; end;
end;
% Handle plotting and storage of details, if requested
if plt||details
if K==0
tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
else
tmpidx=zeros(N,1); tmpdpsim=0;
tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
discon=0;
for j=find(E==0)'
ss=s(ind1(ind1s(j):ind1e(j)),3);
ii=s(ind1(ind1s(j):ind1e(j)),2);
ee=find(E(ii));
if length(ee)==0 discon=1;
else
[smx imx]=max(ss(ee));
tmpidx(j)=ii(ee(imx));
tmpdpsim=tmpdpsim+smx;
end;
end;
if discon
tmpnetsim=nan; tmpdpsim=nan; tmpexpref=nan; tmpidx=nan;
else tmpnetsim=tmpdpsim+tmpexpref;
end;
end;
end;
if details
netsim(i)=tmpnetsim; dpsim(i)=tmpdpsim; expref(i)=tmpexpref;
idx(:,i)=tmpidx;
end;
if plt
netsim(i)=tmpnetsim;
figure(234);
tmp=1:i; tmpi=find(~isnan(netsim(1:i)));
plot(tmp(tmpi),netsim(tmpi),'r-');
xlabel('# Iterations');
ylabel('Net similarity of quantized intermediate solution');
drawnow;
end;
end;
% Identify exemplars
E=((A(M-N+1:M)+R(M-N+1:M))>0); K=sum(E);
if K>0
tmpidx=zeros(N,1); tmpidx(find(E))=find(E); % Identify clusters
for j=find(E==0)'
ss=s(ind1(ind1s(j):ind1e(j)),3);
ii=s(ind1(ind1s(j):ind1e(j)),2);
ee=find(E(ii));
[smx imx]=max(ss(ee));
tmpidx(j)=ii(ee(imx));
end;
EE=zeros(N,1);
for j=find(E)'
jj=find(tmpidx==j); mx=-Inf;
ns=zeros(N,1); msk=zeros(N,1);
for m=jj'
mm=s(ind1(ind1s(m):ind1e(m)),2);
msk(mm)=msk(mm)+1;
ns(mm)=ns(mm)+s(ind1(ind1s(m):ind1e(m)),3);
end;
ii=jj(find(msk(jj)==length(jj)));
[smx imx]=max(ns(ii));
EE(ii(imx))=1;
end;
E=EE;
tmpidx=zeros(N,1); tmpdpsim=0;
tmpidx(find(E))=find(E); tmpexpref=sum(p(find(E)));
for j=find(E==0)'
ss=s(ind1(ind1s(j):ind1e(j)),3);
ii=s(ind1(ind1s(j):ind1e(j)),2);
ee=find(E(ii));
[smx imx]=max(ss(ee));
tmpidx(j)=ii(ee(imx));
tmpdpsim=tmpdpsim+smx;
end;
tmpnetsim=tmpdpsim+tmpexpref;
else
tmpidx=nan*ones(N,1); tmpnetsim=nan; tmpexpref=nan;
end;
if details
netsim(i+1)=tmpnetsim; netsim=netsim(1:i+1);
dpsim(i+1)=tmpnetsim-tmpexpref; dpsim=dpsim(1:i+1);
expref(i+1)=tmpexpref; expref=expref(1:i+1);
idx(:,i+1)=tmpidx; idx=idx(:,1:i+1);
else
netsim=tmpnetsim; dpsim=tmpnetsim-tmpexpref;
expref=tmpexpref; idx=tmpidx;
end;
if plt||details
fprintf('\nNumber of identified clusters: %d\n',K);
fprintf('Fitness (net similarity): %f\n',tmpnetsim);
fprintf(' Similarities of data points to exemplars: %f\n',dpsim(end));
fprintf(' Preferences of selected exemplars: %f\n',tmpexpref);
fprintf('Number of iterations: %d\n\n',i);
end;
if unconverged
fprintf('\n*** Warning: Algorithm did not converge. The similarities\n');
fprintf(' may contain degeneracies - add noise to the similarities\n');
fprintf(' to remove degeneracies. To monitor the net similarity,\n');
fprintf(' activate plotting. Also, consider increasing maxits and\n');
fprintf(' if necessary dampfact.\n\n');
end;
\ No newline at end of file
main.m deleted 100644 → 0
View file @ 16b7ee79
ITER = 100000;
for i = 10001:ITER
% time = datetime;
% save(['time',num2str(i),'.mat'],'time');
softmodem(i);
end
\ No newline at end of file
softmodem.m deleted 100644 → 0
View file @ 16b7ee79
function [] = softmodem(i)
save('i.mat','i');
clc;
clear all;
warning off;
global M;
global filename;
global VSP;
global VSS;
global VSB;
global VSA;
i = importdata('i.mat');
T = 200;
step = 2;
M_idx = 6;
data_file = ['all_data',num2str(i)];
mkdir(data_file);
while M_idx <= 6
i = importdata('i.mat');
% if i == 1 && M == 1
% M = M + 3;
% end
% init_const_para(M);
i = importdata('i.mat');
M_vec = importdata('M_vec.mat');
M = M_vec(M_idx);
fprintf('[ NUMBER ]the number of vehicle is %d\n',M);
filename = ['all_data',num2str(i),'/data_vehicle_number=',num2str(M)];
new_folder = filename;
mkdir(new_folder);
fprintf('[ M O D E ]vehicle number is %d, cluster\n',M);
top_file;
% save('M\M.mat','M');
% clear all;
% global M;
% M = importdata('M\M.mat');
% init_const_para(M);
% filename = ['all_data',num2str(i),'/data_without_cluster_vehicle_number=',num2str(M)];
% new_folder = filename;
% mkdir(new_folder);
% fprintf('[ M O D E ]vehicle number is %d, no-cluster\n',M);
% if M == 1
% copyfile('all_data\data_vehicle_number=1\*' ,'all_data\data_without_cluster_vehicle_number=1');
% else
% top_file_without_cluster;
% end
% % data_p(M,T,step);
save('M\M_idx.mat','M_idx');
clear all;
global M;
M_idx = importdata('M\M_idx.mat');
M_vec = importdata('M_vec.mat');
M = M_vec(M_idx);
M_idx = M_idx + 1;
end
end
\ No newline at end of file
top_file.m deleted 100644 → 0
View file @ 16b7ee79
% clear;
% clear state;
% clear ob;
% clear PF;
% clear obj;
% clear cluster;
%%%用观测去采样粒子！！！------learning approach
global state;
global ob;
global PF;
global obj;
global cluster;
global path_track;
global VSP;
global VSS;
global VSB;
global VSA;
global M;
global filename;
global reflector;
global vehicle_vec;
global recorder;
reflector.N_ref = 0;
PI = 3.1415926;
T_RECLUSTER = 10;
t_del = 15;
if M == 1
t_del = 100;
end
%% remained to be done
%%%1. 要与没有聚类的结果进行对比%%%注释掉cluster_update，重新跑一遍即可
%%%3. T_duration和加速度，加速度的方差需要调整
len_road=150;
len_veh_trace=100;
lane_wedith = 4;
len_ref_x=12;%15;
dis_ref_x=6;%7.5;
y_line=35;%40;%35;
building_height=15;%20;
bs_height=8;
theta_building.theta1 = PI/6;
theta_building.theta2 = PI/9;
theta_building.theta3 = PI/12;
% theta1_building=PI/4;
% theta2_building=PI/6;
% theta3_building=PI/12;
veh_speed = 10;
veh_acce_speed_straight = 9.8*0.5;
veh_angle_acce_speed = 0.8;
ref_step = 0.5;
edge_limit = 0.5;
limit_N = 200;
limit_percentage = 0.1;
batch_limit = 10;
lr = 5e-5;
ITER = 100;
% limit_dist = 8;
svt_combine_limit = 10;
del_limit = 20;
particle_limit = 120;
PF_step = 10;
N_batch = 1;
overlap = 1;
p_min_veh = 4;%exp(-4.25^2);
p_min_svt = 7;%exp(-7^2);
if M == 1
p_min_veh = 10;%exp(-4.25^2);
p_min_svt = 10;%exp(-7^2);
end
%% vehicle parameters
derta_d_super= 0.1*pow2(-1*(M>1));%0.2*pow2(-floor(0.34*log2(M)));%0.1,0.25**********************************vehicle
L_d_super = 6;%40*******************************************vehicle
k=6;%%%%2*pi*derta_d_super/k
%% svt parameters
sigma_vh.rvh = 0.5;%*********************************************SVT
derta_d_svt = 0.125*power(1.25,-1*(M>8));%******************************************SVT
L_d_svt = 2;%**********************************************SVT
derta_d_svt_upd = 0.15;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%svt_upd
derta_theta_upd = pi/4;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%svt_upd
L_d_upd = 4;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%svt_upd
%% const para
%%%% max iteration of time
T = 300;
T_duration = 0.1;%%0.5
%%%% number of vehicle
% M = 5;
% %%% VSP: vehicle_start_position---> M*2
% VSP = [ 0,0;8,5;16,-5;24,5;32,5 ];
% %%% VSS: vehicle start speed---> M*2
% VSS = [ 17,0;16,0;15,0;14,0;13,0 ];
% %%% VSB:vehicle start bias----> M*1
% VSB = [ 0;0;0;0;0 ];
% VSA = [ 0,0;0,0;0,0;0,0;0,0 ];
%%% the minimum weight that two reflector is combineed
limit_combine = 0.6;
%%%% the weight of the direction of the estimated reflector points
%%% hight for autenna in vehicle
H_v = 1;
%%% hight for building
H_b = 10;
%%% hight for base sta,tion
% H_bs = 15;
%%% the number of objects
% obj_N = 400;
%%% std for observation of TOA and AOA
sigma_d = 2.62;%%%3************************************************TOA
sigma_theta = 0.036;%%%0.01*****************************************AOA
sigma_theta_init = 0.01;
sigma_a = 0;%2*0.001*9.8/3;%%2
sigma_a_vec = 0;%0.5/(180*3)*pi;%*********************************************a
sigma_v = 0.1;
sigma_v_vec = 0.1*T_duration;%************************************************v
sigma_center = 4;
sigma_a_init = 1;
sigma_vh.dvh = 0.03;
sigma_gps = 3;
LAMDA = (sigma_d/sigma_theta)^2;
%%% block size for distance(for Xvt_ja)
derta_d = 0.1;%%0.5
%%%%the distance resolution of the reflector line
derta_d_reflector = 0.1;
beta = 0.95;
%%% block size for distance(for Xu_j)
%%% block size for and angle(for Xvt_j)
derta_theta = 10*pi/180;
%%% block size for speed(for Xu_j)
derta_v_super= 0.2;
%%% block size for bias(for Xu_j
derta_b_super= 5e-12;
%%% number of particle size in distance domain(for Xu_j)
L_d = 8;%8
L_d_cluster = 10;
%%% number of particle size in speed domain(for Xu_j)
L_v = 7;
%%% number of particle size in bias domain(for Xu_j)
L_b = 1;
%%% the number of particles for super PF
N_beta = 6;%6
N_beta_super = 18;%36********************************************vehicle
N_beta_cluster= 9;
N_super = L_d * L_v * L_b * N_beta * N_beta;
%%% K times of AOA estimation std to decide the number of particles(for
%%% Xvt_ja)
K = 2;%3
%%% time duration for one time slot
%%% the max number of particles(for Xvt_ja) in one block
N_max = 100;
%%% sigma_vt : the std of the noise added to the particles of virtual
%%% transmitters, with
%%%% sigma_vt.Rvt : the std for position
%%%% sigma_vt.Dvt : the std for addentional distance
% sigma_vt.Rvt = 3;
% sigma_vt.Dvt = 2;
sigma_u = sigma_v*T_duration*sqrt(2/pi);
step = 2;
rita = 6;
gama = 2;
N_select = ceil((gama/10)*L_d_super*N_beta_super);
%%% init the map
% init_map(T,T_duration,sigma_a_init,sigma_center,M,obj_N,VSP,VSS,VSA);
% filename = 'data';
% obj = importdata('obj.mat');
%%% generate the objects
%%%end
%% dynamic input
%%% veh_para: the parameters of vehicle including
%%% vehicle[0...M].MultiPath[0...N].d
%%% vehicle[0...M].Multipath[0...N].theta
%%% vehicle[0...M].Multipath[0...N].id //ascending order with the id
%%% u is a motion with
%%%% u.a accelerate speed
%%%% u.b bias
%%% end
%% main loop for estimation
%%% initialize the bias,speed and positions for vehicle
MAP_generation(M,len_road,len_veh_trace,len_ref_x,dis_ref_x,y_line,building_height,bs_height,theta_building,T_duration,veh_speed,veh_acce_speed_straight,veh_angle_acce_speed,lane_wedith,T)
obj_N = length(obj);
arrived = 1:1:M;
arrived_pre = [];
%%% initialize the position, speed and bias for state 1.
for t = 1: T
fprintf('\nvehicle = %d, cluster\nFor time slot T = %d*%.1f(second)\n',M,t,T_duration);
initialize(t,arrived,sigma_gps,sigma_v,sigma_v_vec,sigma_a,sigma_a_vec);%%% for new vehicle
[veh_para] = generate_para(t,M,obj_N,T_duration,sigma_d,sigma_theta,sigma_a,sigma_v);%%% the observation for state 1
%%% generate the parameters for multipath and the motion information.
%%% update the observation
% para_update(t,M,veh_para,sigma_d,sigma_theta);%start from time 1
%%% give the multipath parameter to observation.
motion_update(t,M,T_duration,sigma_a,sigma_a_vec,sigma_v,sigma_v_vec,arrived);
% if ~isempty(arrived)
% %%% init the particles of super_PF
% veh_PF_init(t,M,arrived);
% % init_super_PF(M,N_super,derta_d_super,derta_v_super,derta_b_super,L_d,L_v,L_b,N_beta); % for time 1 only
% %%% generate the super particles in blocks
% end
veh_PF_update(t,M,T_duration,L_d_super,derta_d_super,k,arrived,particle_limit);
%%% update the super_PF using the motion information u.
VTCI_update_del(t,M,derta_d_svt,derta_theta,K,sigma_theta_init,beta,sigma_vh,arrived);
% VTCH_estimate(t,M,sigma_d,sigma_theta,rita,derta_d,L_d,N_beta,arrived);
% vehicle_state_estimation(t,M,gama,arrived);
isChanged = VTCI_update_add(t,M,L_d_upd,derta_d_svt,derta_theta_upd,particle_limit);
% svt_cluster_judge(t,M,svt_combine_limit,particle_limit,del_limit,arrived);
svt_cluster_judge(t,M,svt_combine_limit,particle_limit,del_limit,T_RECLUSTER,t_del,L_d_upd,derta_d_svt,derta_theta_upd)
Analysis(t);
% if ~isempty(arrived)
% svt_re_drawing(t,L_d_upd,derta_d_svt_upd,derta_theta_upd,particle_limit);
% end
Coupled_PF(t,M,sigma_d,sigma_theta,rita,arrived,N_batch,overlap,sigma_u,gama,p_min_veh,p_min_svt,sigma_vh);
% state(t).vehicle(1).Xu.Ru
Analysis(t);
% cluster_update(t,M,N_beta,derta_d,particle_limit,limit_dist,sigma_vh,derta_d_reflector,limit_combine,ref_step,edge_limit,limit_N,limit_percentage,batch_limit,lr,LAMDA,ITER,sigma_d);
% reflector_building(t,sigma_d,sigma_vh,limit_percentage,limit_N,derta_d_reflector,limit_combine,step,edge_limit,batch_limit,lr,LAMDA,ITER);
% svt_re_drawing(t,L_d_upd,derta_d_svt_upd,derta_theta_upd,particle_limit);
update_path_track();
update_state(t,M,T_duration);
line_detection(t,sigma_d);
arrived_pre = arrived;
arrived = [];
% arrived = vehicle_arrived(t,M,veh_speed,len_road,len_veh_trace,T_duration);
if mod(t,step)==1 || t == T
clean_data(t,T,step,filename);
%%% clean the
end
fprintf('[REFLECTOR] THE NUMBER OF REFLECTOR IS %d...\n',reflector.N_ref);
end
% reflector_estimate(T);
obj_name = [filename,'\obj'];
save(obj_name,'obj');
ob_name = [filename,'\ob'];
save(ob_name,'ob');
recorder_name = [filename,'\recorder'];
save(recorder_name,'recorder');
% show_result(T_duration,M,T);
%%%end
%%%1. batch 应该shaffule一下
%%%2. 应该更新一下svt的位置
%%%3. 应该更多得体现随机性以提高精度
top_file_without_cluster.m deleted 100644 → 0
View file @ 16b7ee79
% clear;
% clear state;
% clear ob;
% clear PF;
% clear obj;
% clear cluster;
global state;
global ob;
global PF;
global obj;
global cluster;
global VSP;
global VSS;
global VSB;
global VSA;
global M;
global filename;
%% remained to be done
%%%1. 要与没有聚类的结果进行对比%%%注释掉cluster_update，重新跑一遍即可
%%%3. T_duration和加速度，加速度的方差需要调整
%% const para
%%%% max iteration of time
T = 100;
% %%%% number of vehicle
% M = 5;
% %%% VSP: vehicle_start_position---> M*2
% VSP = [ 0,0;8,5;16,-5;24,5;32,5 ];
% %%% VSS: vehicle start speed---> M*2
% VSS = [ 17,0;16,0;15,0;14,0;13,0 ];
% %%% VSB:vehicle start bias----> M*1
% VSB = [ 0;0;0;0;0 ];
% VSA = [ 0,0;0,0;0,0;0,0;0,0 ];
%%% hight for autenna in vehicle
H_v = 1;
%%% hight for building
H_b = 10;
%%% hight for base station
H_bs = 15;
%%% the number of objects
obj_N = 400;
%%% std for observation of TOA and AOA
sigma_d = 2;%%%1
sigma_theta = 0.01;
sigma_theta_init = 0.01;
sigma_a = 0.5;%%2
sigma_v = 0.1;
sigma_center = 4;
sigma_a_init = 1;
sigma_vh.rvh = 0.2;
sigma_vh.dvh = 0;
%%% block size for distance(for Xvt_ja)
derta_d = 0.5;%%0.1
beta = 0.95;
%%% block size for distance(for Xu_j)
derta_d_super= 0.1;
%%% block size for and angle(for Xvt_j)
derta_theta = 0.01;
%%% block size for speed(for Xu_j)
derta_v_super= 0.2;
%%% block size for bias(for Xu_j)
derta_b_super= 5e-12;
%%% number of particle size in distance domain(for Xu_j)
L_d = 8;
L_d_super = 40;
L_d_cluster = 10;
%%% number of particle size in speed domain(for Xu_j)
L_v = 7;
%%% number of particle size in bias domain(for Xu_j)
L_b = 1;
%%% the number of particles for super PF
N_beta = 6;%18
N_beta_super = 36;
N_beta_cluster= 9;
N_super = L_d * L_v * L_b * N_beta * N_beta;
%%% K times of AOA estimation std to decide the number of particles(for
%%% Xvt_ja)
K = 3;%3
%%% time duration for one time slot
T_duration = 0.2;%%0.5
%%% the max number of particles(for Xvt_ja) in one block
N_max = 100;
%%% sigma_vt : the std of the noise added to the particles of virtual
%%% transmitters, with
%%%% sigma_vt.Rvt : the std for position
%%%% sigma_vt.Dvt : the std for addentional distance
% sigma_vt.Rvt = 3;
% sigma_vt.Dvt = 2;
sigma_u = 1;
step = 2;
rita =2;
%%% init the map
init_map(T,T_duration,sigma_a_init,sigma_center,M,obj_N,VSP,VSS,VSA);
% filename = 'data_without_cluster';
% obj = importdata('obj.mat');
%%% generate the objects
%%%end
%% dynamic input
%%% veh_para: the parameters of vehicle including
%%% vehicle[0...M].MultiPath[0...N].d
%%% vehicle[0...M].Multipath[0...N].theta
%%% vehicle[0...M].Multipath[0...N].id //ascending order with the id
%%% u is a motion with
%%%% u.a accelerate speed
%%%% u.b bias
%%% end
%% main loop for estimation
%%% initialize the bias,speed and positions for vehicle
initialize( T,M,VSP,VSS,VSB);%%% for state 1
%%% initialize the position, speed and bias for state 1.
for t = 1: T
fprintf('\nvehicle = %d, no-cluster\nFor time slot T = %d*%.1f(second)\n',M,t,T_duration);
[veh_para] = generate_para(t,M,obj_N,T_duration,sigma_d,sigma_theta,sigma_a,sigma_v);%%% the observation for state 1
%%% generate the parameters for multipath and the motion information.
%%% update the observation
% para_update(t,M,veh_para,sigma_d,sigma_theta);%start from time 1
%%% give the multipath parameter to observation.
if t == 1
%%% init the particles of super_PF
veh_PF_init(M);
% init_super_PF(M,N_super,derta_d_super,derta_v_super,derta_b_super,L_d,L_v,L_b,N_beta); % for time 1 only
%%% generate the super particles in blocks
else
veh_PF_update(t,M,T_duration,L_d_super,derta_d_super,N_beta_super,sigma_u);
%%% update the super_PF using the motion information u.
end
isChanged = VTCI_update_del(t,M,derta_d,derta_theta,K,sigma_theta_init,beta,sigma_vh);
% if t>1
% cluster(t).VH(1).N_VH
% end
VTCH_estimate(t,M,sigma_d,sigma_theta,rita,derta_d,L_d,N_beta);
vehicle_state_estimation(t,M);
isChanged = VTCI_update_add(t,M,derta_d,derta_theta,K,sigma_theta,beta);
motion_update(t,M,T_duration,sigma_a,sigma_v);
% cluster_update(t,M,N_beta,derta_d,L_d);
%%% RB-particle_filter
% RBPF(t,M,T_duration,derta_d,derta_theta,K,u,N_max,sigma_vt,beta);
%%% MMSE to calculate the final state of vehicle
% mmse(t,M);
% noise_PF(t,M,sigma_vt,sigma_u);
update_state(t,M,T_duration);
if mod(t,step)==1 || t == T
clean_data(t,step,filename);
%%% clean the
end
end
obj_name = [filename,'\obj'];
save(obj_name,'obj');
ob_name = [filename,'\ob'];
save(ob_name,'ob');
% show_result(T_duration,M,T);
%%%end
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment