daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added robot steering example to UKF.

Browse Source
This commit is contained in:
Roger Labbe 2015-06-09 19:39:39 -07:00
parent 3c3a5e5bf6
commit aadc5abaf6
2 changed files with 192 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
09_Unscented_Kalman_Filter.ipynb
View File

File diff suppressed because one or more lines are too long

77
experiments/ukfloc.py
View File

@ -44,7 +44,8 @@ def move(x, u, dt, wheelbase):
if abs(steering_angle) < 0.0001:
# approximate straight line with huge radius
r = 1.e-30
r = 1.e30
steering_angle = 1.e-5
b = dist / wheelbase * tan(steering_angle)
r = wheelbase / tan(steering_angle) # radius
@ -90,13 +91,14 @@ def z_mean(sigmas, Wm):
sigma_r = .3
sigma_h = .1#np.radians(1)
sigma_steer = np.radians(.01)
dt = 1.0
dt = 0.1
wheelbase = 0.5
m = array([[5, 10],
[10, 5],
[15, 15],
[20, 5]])
[20, 5],
[0, 30], [50, 30], [40, 10]])
def fx(x, dt, u):
@ -114,47 +116,84 @@ def Hx(x, landmark):
Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]])
return Hx
points = MerweScaledSigmaPoints(n=3, alpha=1.e-3, beta=2, kappa=0)
points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0)
ukf= UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points,
x_mean_fn=state_mean, z_mean_fn=z_mean,
residual_x=residual_x, residual_z=residual_h)
ukf.x = array([2, 6, .3])
ukf.P = np.diag([.1, .1, .2])
ukf.R = np.diag([sigma_r**2, sigma_h**2])
ukf.Q = np.zeros((3,3))
ukf.Q = np.eye(3)*0.001
u = array([1.1, .01])
u = array([1.1, .0000001])
xp = ukf.x.copy()
plt.figure()
plt.scatter(m[:, 0], m[:, 1])
for i in range(200):
xp = move(xp, u, dt/10., wheelbase) # simulate robot
plt.plot(xp[0], xp[1], ',', color='g')
cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)
if i % 10 == 0:
ukf.predict(fx_args=u)
v = cmds[-1][0]
cmds.extend([[v, a] for a in np.linspace(0, np.radians(2), 15)])
cmds.extend([cmds[-1]]*100)
cmds.extend([[v, a] for a in np.linspace(np.radians(2), -np.radians(2), 15)])
cmds.extend([cmds[-1]]*200)
cmds.extend([[v, a] for a in np.linspace(-np.radians(2), 0, 15)])
cmds.extend([cmds[-1]]*50)
cmds.extend([[v, a] for a in np.linspace(0, -np.radians(1), 25)])
cmds = np.array(cmds)
cindex = 0
u = cmds[0]
track = []
while cindex < len(cmds):
u = cmds[cindex]
xp = move(xp, u, dt, wheelbase) # simulate robot
track.append(xp)
ukf.predict(fx_args=u)
if cindex % 20 == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=3,
facecolor='b', alpha=0.08)
facecolor='b', alpha=0.18)
for lmark in m:
d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r
a = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) - xp[2] + randn()*sigma_h
z = np.array([d, a])
for lmark in m:
d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r
a = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) - xp[2] + randn()*sigma_h
z = np.array([d, a])
ukf.update(z, hx_args=(lmark,))
ukf.update(z, hx_args=(lmark,))
if cindex % 20 == 0:
plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=3,
facecolor='g', alpha=0.4)
cindex += 1
#plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
track = np.array(track)
plt.plot(track[:, 0], track[:,1], color='k')
plt.axis('equal')
plt.title("UKF Robot localization")
plt.show()
print(ukf.P.diagonal())
print(ukf.P.diagonal())