Skip to content

Commit de1cc46

Browse files
drewtu2drewtu2
committed
2 parents a96073e + 9f739c1 commit de1cc46

File tree

20 files changed

+1640
-468
lines changed

20 files changed

+1640
-468
lines changed

‎.github/FUNDING.yml‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
# These are supported funding model platforms
2+
patreon: myenigma
3+
custom: https://www.paypal.me/myenigmapay/

‎.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
*.csv
22
*.gif
3+
*.g2o
34

45
# Byte-compiled / optimized / DLL files
56
__pycache__/

‎Localization/extended_kalman_filter/extended_kalman_filter.py‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -6,17 +6,22 @@
66
77
"""
88

9-
import numpy as np
109
import math
10+
import numpy as np
1111
import matplotlib.pyplot as plt
1212

13-
# Estimation parameter of EKF
14-
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
13+
# Covariance for EKF simulation
14+
Q = np.diag([
15+
0.1, # variance of location on x-axis
16+
0.1, # variance of location on y-axis
17+
np.deg2rad(1.0), # variance of yaw angle
18+
1.0 # variance of velocity
19+
])**2 # predict state covariance
1520
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
1621

1722
# Simulation parameter
18-
Qsim = np.diag([1.0, np.deg2rad(30.0)])**2
19-
Rsim = np.diag([0.5, 0.5])**2
23+
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
24+
GPS_NOISE = np.diag([0.5, 0.5])**2
2025

2126
DT = 0.1 # time tick [s]
2227
SIM_TIME = 50.0 # simulation time [s]
@@ -27,7 +32,7 @@
2732
def calc_input():
2833
v = 1.0 # [m/s]
2934
yawrate = 0.1 # [rad/s]
30-
u = np.array([[v, yawrate]]).T
35+
u = np.array([[v], [yawrate]])
3136
return u
3237

3338

@@ -36,14 +41,10 @@ def observation(xTrue, xd, u):
3641
xTrue = motion_model(xTrue, u)
3742

3843
# add noise to gps x-y
39-
zx = xTrue[0, 0] + np.random.randn() * Rsim[0, 0]
40-
zy = xTrue[1, 0] + np.random.randn() * Rsim[1, 1]
41-
z = np.array([[zx, zy]]).T
44+
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
4245

4346
# add noise to input
44-
ud1 = u[0, 0] + np.random.randn() * Qsim[0, 0]
45-
ud2 = u[1, 0] + np.random.randn() * Qsim[1, 1]
46-
ud = np.array([[ud1, ud2]]).T
47+
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
4748

4849
xd = motion_model(xd, ud)
4950

@@ -62,19 +63,18 @@ def motion_model(x, u):
6263
[0.0, DT],
6364
[1.0, 0.0]])
6465

65-
x = F@x + B@u
66+
x = F @ x + B @ u
6667

6768
return x
6869

6970

7071
def observation_model(x):
71-
# Observation Model
7272
H = np.array([
7373
[1, 0, 0, 0],
7474
[0, 1, 0, 0]
7575
])
7676

77-
z = H@x
77+
z = H @ x
7878

7979
return z
8080

‎Localization/unscented_kalman_filter/unscented_kalman_filter.py‎

Lines changed: 18 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -11,13 +11,18 @@
1111
import math
1212
import matplotlib.pyplot as plt
1313

14-
# Estimation parameter of UKF
15-
Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
16-
R = np.diag([1.0, np.deg2rad(40.0)])**2
14+
# Covariance for UKF simulation
15+
Q = np.diag([
16+
0.1, # variance of location on x-axis
17+
0.1, # variance of location on y-axis
18+
np.deg2rad(1.0), # variance of yaw angle
19+
1.0 # variance of velocity
20+
])**2 # predict state covariance
21+
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
1722

1823
# Simulation parameter
19-
Qsim = np.diag([0.5, 0.5])**2
20-
Rsim = np.diag([1.0, np.deg2rad(30.0)])**2
24+
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
25+
GPS_NOISE = np.diag([0.5, 0.5])**2
2126

2227
DT = 0.1 # time tick [s]
2328
SIM_TIME = 50.0 # simulation time [s]
@@ -42,14 +47,10 @@ def observation(xTrue, xd, u):
4247
xTrue = motion_model(xTrue, u)
4348

4449
# add noise to gps x-y
45-
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
46-
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
47-
z = np.array([[zx, zy]]).T
50+
z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)
4851

4952
# add noise to input
50-
ud1 = u[0] + np.random.randn() * Rsim[0, 0]
51-
ud2 = u[1] + np.random.randn() * Rsim[1, 1]
52-
ud = np.array([ud1, ud2])
53+
ud = u + INPUT_NOISE @ np.random.randn(2, 1)
5354

5455
xd = motion_model(xd, ud)
5556

@@ -100,15 +101,19 @@ def generate_sigma_points(xEst, PEst, gamma):
100101

101102

102103
def predict_sigma_motion(sigma, u):
103-
# Sigma Points prediction with motion model
104+
"""
105+
Sigma Points prediction with motion model
106+
"""
104107
for i in range(sigma.shape[1]):
105108
sigma[:, i:i + 1] = motion_model(sigma[:, i:i + 1], u)
106109

107110
return sigma
108111

109112

110113
def predict_sigma_observation(sigma):
111-
# Sigma Points prediction with observation model
114+
"""
115+
Sigma Points prediction with observation model
116+
"""
112117
for i in range(sigma.shape[1]):
113118
sigma[0:2, i] = observation_model(sigma[:, i])
114119

‎Mapping/kmeans_clustering/kmeans_clustering.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ def update_clusters(clusters):
7373
mind = min(dlist)
7474
min_id = dlist.index(mind)
7575
clusters.labels[ip] = min_id
76-
cost += min_id
76+
cost += mind
7777

7878
return clusters, cost
7979

‎Mapping/lidar_to_grid_map/animation.gif‎

279 KB
Loading

‎Mapping/lidar_to_grid_map/grid_map_example.png‎

372 KB
Loading

‎Mapping/lidar_to_grid_map/lidar01.csv‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
0.008450416037156572,0.5335
2+
0.046902201120156306,0.5345
3+
0.08508127850753233,0.537
4+
0.1979822644959155,0.2605
5+
0.21189035697274505,0.2625
6+
0.2587960806200922,0.26475
7+
0.3043382657893199,0.2675
8+
0.34660795861105775,0.27075
9+
0.43632879047139106,0.59
10+
0.4739624524675188,0.60025
11+
0.5137777760286397,0.611
12+
0.5492297764597742,0.6265
13+
0.5895905154121426,0.64
14+
0.6253152235389017,0.6565
15+
0.6645851317087743,0.676
16+
0.6997644244442851,0.6975
17+
0.7785769484796541,0.3345
18+
0.7772134100015329,0.74575
19+
0.8652979956881222,0.3315
20+
0.8996591653367609,0.31775
21+
0.9397471965935056,0.31275
22+
0.9847439663714841,0.31125
23+
1.0283771976713423,0.31325
24+
1.0641019057981014,0.31975
25+
1.1009174447073562,0.3335
26+
1.2012738766970301,0.92275
27+
1.2397256617800307,0.95325
28+
1.2779047391674068,0.9865
29+
1.316629231946031,1.01775
30+
1.3561718478115274,1.011
31+
1.3948963405901518,1.0055
32+
1.4330754179775278,1.00225
33+
1.4731634492342724,0.99975
34+
1.5113425266216485,0.9975
35+
1.5517032655740168,1.001
36+
1.5896096352657691,1.00275
37+
1.6288795434356418,1.008
38+
1.6684221593011381,1.0135
39+
1.7066012366885142,1.022
40+
1.7453257294671385,1.02875
41+
1.7862318838107551,0.9935
42+
1.8257744996762515,1.0025
43+
1.8661352386286207,0.96075
44+
1.9045870237116205,0.92125
45+
1.9465840088377355,0.8855
46+
1.986944747790103,0.85725
47+
2.025669240568728,0.832
48+
2.065757271825472,0.80675
49+
2.1066634261690886,0.78875
50+
2.1464787497302105,0.7705
51+
2.1865667809869542,0.75625
52+
2.2261093968524506,0.74475
53+
2.2683790896741876,0.68275
54+
2.3090125363221823,0.6375
55+
2.3510095214482956,0.59925
56+
2.3916429680962885,0.5665
57+
2.4330945378311526,0.538
58+
2.4783640153047557,0.50825
59+
2.5203610004308707,0.4875
60+
2.562903400948233,0.46825
61+
2.599173524466238,0.45
62+
2.642806755766097,0.4355
63+
2.685076448587836,0.42275
64+
2.722437402888339,0.4125
65+
2.766888757275069,0.40125
66+
2.8007045115324587,0.39525
67+
2.841883373571701,0.385
68+
2.8819714048284446,0.3805
69+
2.922332143780814,0.38575
70+
2.9637837135156797,0.38425
71+
3.0005992524249336,0.36575
72+
3.0401418682904318,0.3765
73+
3.079957191851552,0.3915
74+
3.115409192282687,0.408
75+
3.154679100452558,0.4265
76+
3.184949654666836,0.447
77+
3.2242195628367085,0.4715
78+
3.2574899017028507,0.49875
79+
3.2959416867858504,0.52875
80+
3.3292120256519926,0.564
81+
3.3665729799524957,0.6055
82+
3.4031158111661277,0.6515
83+
3.438022396206014,0.70675
84+
3.4756560582021407,0.771
85+
3.513562427893893,0.77075
86+
3.5522869206725183,0.7785
87+
3.621827383056667,0.79575
88+
3.65918833735717,0.8045
89+
3.697367414744546,0.81725
90+
3.7377281536969154,0.83325
91+
3.775634523388667,0.8415
92+
3.8135408930804187,0.85575
93+
3.8522653858590425,0.87325
94+
3.8898990478551703,0.88725
95+
3.9299870791119154,0.906
96+
3.9665299103255465,0.9265
97+
4.006072526191043,0.94575
98+
4.043978895882795,0.97175
99+
4.081885265574547,1.02075
100+
4.1206097583531704,1.046
101+
4.1587888357405465,1.07025
102+
4.196422497736674,1.097
103+
4.234874282819675,1.12575
104+
4.286688744988257,0.73475
105+
4.324322406984384,0.72225
106+
4.364410438241129,0.731
107+
4.405862007975994,0.7405
108+
4.44267754688525,0.749
109+
4.484129116620115,0.76025
110+
4.522853609398739,0.76825
111+
4.560759979090491,0.77125
112+
4.5989390564778665,0.77725
113+
4.640117918517108,0.782
114+
4.679115118991357,0.78425
115+
4.717294196378733,0.789
116+
4.757109519939853,0.78825
117+
4.796652135805349,0.7855
118+
4.8337403824102285,0.786
119+
4.871646752101981,0.78275
120+
4.9109166602718535,0.7785
121+
4.950186568441726,0.7635
122+
4.990274599698471,0.74725
123+
5.028180969390222,0.737
124+
5.0677235852557185,0.72575
125+
5.109720570381833,0.71525
126+
5.149263186247329,0.70625
127+
5.1863514328522085,0.69975
128+
5.230530079543315,0.693
129+
5.269799987713188,0.68925
130+
5.307979065100563,0.68425
131+
5.347248973270435,0.68275
132+
5.386518881440308,0.68075
133+
5.426606912697053,0.68175
134+
5.465604113171301,0.67825
135+
5.502419652080556,0.6835
136+
5.543871221815422,0.6885
137+
5.580959468420302,0.67925
138+
5.624319992024535,0.6555
139+
5.660044700151294,0.639
140+
5.700950854494911,0.623
141+
5.740220762664784,0.6075
142+
5.783581286269018,0.59475
143+
5.820124117482649,0.58475
144+
5.861848394913139,0.57325
145+
5.899209349213642,0.565
146+
5.938751965079138,0.55525
147+
5.9782945809446355,0.55175
148+
6.017564489114507,0.546
149+
6.059288766544997,0.5405
150+
6.097467843932373,0.53825
151+
6.139464829058487,0.534
152+
6.176825783358991,0.5325
153+
6.215822983833238,0.53125
154+
6.252911230438118,0.53075

0 commit comments

Comments
 (0)