positioning group commit

main.m

+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

+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

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

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