-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathget_Observation.m
109 lines (95 loc) · 3.53 KB
/
get_Observation.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
% lisheng 11/27/17
function obs = get_Observation(subject_car_idx, Cars, Params)
% convert theta from -pi~pi to 0~2*pi
for i = 1:length(Cars)
if Cars(i).theta < 0 && Cars(i).theta >= - pi
Cars(i).theta = Cars(i).theta + 2*pi;
end
end
subject_car = Cars(subject_car_idx);
all_idces = 1:length(Cars);
other_cars_idces = find(all_idces ~= subject_car_idx);
% make a copy of other_cars
other_cars = Cars(other_cars_idces);
num_other_cars = length(Cars) - 1;
dthetas = zeros(num_other_cars, 1);
other_cars_in_same_lane_idces = [];
other_cars_in_target_lane_idces = [];
for i = 1: num_other_cars
dthetas(i) = other_cars(i).theta - subject_car.theta;
if dthetas(i) > pi
dthetas(i) = dthetas(i) - 2*pi;
elseif dthetas(i) < - pi
dthetas(i) = dthetas(i) + 2*pi;
end
if other_cars(i).lane_id == subject_car.lane_id
other_cars_in_same_lane_idces = [other_cars_in_same_lane_idces; i];
else
other_cars_in_target_lane_idces = [other_cars_in_target_lane_idces; i];
end
end
% find the three surrounding cars around subject_car
%% 1 - front center
dtheta_same_lane = dthetas(other_cars_in_same_lane_idces);
possible_fc_cars_idces = find(dtheta_same_lane > 0);
fc_dtheta = min(dtheta_same_lane(possible_fc_cars_idces));
if ~isempty(fc_dtheta)
fc_car_idx = find(dthetas == fc_dtheta);
fc_car = other_cars(fc_car_idx);
fc_v = fc_car.vx - subject_car.vx;
fc_d = fc_dtheta * Params.road_radius;
elseif ~isempty(find(dtheta_same_lane < 0, 1))
possible_fc_cars_idces = find(dtheta_same_lane < 0);
fc_dtheta = min(dtheta_same_lane(possible_fc_cars_idces));
fc_car_idx = find(dthetas == fc_dtheta);
fc_car = other_cars(fc_car_idx);
fc_v = fc_car.vx - subject_car.vx;
fc_d = Params.road_length + fc_dtheta * Params.road_radius;
else
fc_v = 0;
fc_d = Params.road_length;
end
%% 2 - front target
dtheta_target_lane = dthetas(other_cars_in_target_lane_idces);
possible_ft_cars_idces = find(dtheta_target_lane > 0);
ft_dtheta = min(dtheta_target_lane(possible_ft_cars_idces));
if ~isempty(ft_dtheta)
ft_car_idx = find(dthetas == ft_dtheta);
ft_car = other_cars(ft_car_idx);
ft_v = ft_car.vx - subject_car.vx;
ft_d = ft_dtheta * Params.road_radius;
elseif ~isempty(find(dtheta_target_lane < 0, 1))
possible_ft_cars_idces = find(dtheta_target_lane < 0);
ft_dtheta = min(dtheta_target_lane(possible_ft_cars_idces));
ft_car_idx = find(dthetas == ft_dtheta);
ft_car = other_cars(ft_car_idx);
ft_v = ft_car.vx - subject_car.vx;
ft_d = Params.road_length + ft_dtheta * Params.road_radius;
else
ft_v = 0;
ft_d = Params.road_length;
end
%% 3 - rear target
dtheta_target_lane = dthetas(other_cars_in_target_lane_idces);
possible_rt_cars_idces = find(dtheta_target_lane < 0);
rt_dtheta = max(dtheta_target_lane(possible_rt_cars_idces));
if ~isempty(rt_dtheta)
rt_car_idx = find(dthetas == rt_dtheta);
rt_car = other_cars(rt_car_idx);
rt_v = rt_car.vx - subject_car.vx;
rt_d = rt_dtheta * Params.road_radius;
elseif ~isempty(find(dtheta_target_lane > 0, 1))
possible_rt_cars_idces = find(dtheta_target_lane > 0);
rt_dtheta = max(dtheta_target_lane(possible_rt_cars_idces));
rt_car_idx = find(dthetas == rt_dtheta);
rt_car = other_cars(rt_car_idx);
rt_v = rt_car.vx - subject_car.vx;
rt_d = - (Params.road_length - rt_dtheta * Params.road_radius);
else
rt_v = 0;
rt_d = - Params.road_length;
end
%% construct obs
lane_id = subject_car.lane_id;
obs = Construct_Observation(fc_d, fc_v, ft_d, ft_v, rt_d, rt_v, lane_id);
return