179 lines
4.8 KiB
Python
179 lines
4.8 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Mon Oct 3 07:59:40 2016
|
|
|
|
@author: rlabbe
|
|
"""
|
|
|
|
from filterpy.kalman import UnscentedKalmanFilter as UKF, MerweScaledSigmaPoints
|
|
from math import tan, sin, cos, sqrt, atan2
|
|
import numpy as np
|
|
from numpy.random import randn
|
|
import matplotlib.pyplot as plt
|
|
from filterpy.stats import plot_covariance_ellipse
|
|
|
|
|
|
|
|
def move(x, u, dt, wheelbase):
|
|
hdg = x[2]
|
|
vel = u[0]
|
|
steering_angle = u[1]
|
|
dist = vel * dt
|
|
|
|
if abs(steering_angle) > 0.001: # is robot turning?
|
|
beta = (dist / wheelbase) * tan(steering_angle)
|
|
r = wheelbase / tan(steering_angle) # radius
|
|
|
|
sinh, sinhb = sin(hdg), sin(hdg + beta)
|
|
cosh, coshb = cos(hdg), cos(hdg + beta)
|
|
return x + np.array([-r*sinh + r*sinhb,
|
|
r*cosh - r*coshb, beta])
|
|
else: # moving in straight line
|
|
return x + np.array([dist*cos(hdg), dist*sin(hdg), 0])
|
|
|
|
|
|
def fx(x, dt, u):
|
|
return move(x, u, dt, wheelbase)
|
|
|
|
|
|
def normalize_angle(x):
|
|
x = x % (2 * np.pi) # force in range [0, 2 pi)
|
|
if x > np.pi: # move to [-pi, pi)
|
|
x -= 2 * np.pi
|
|
return x
|
|
|
|
|
|
def residual_h(a, b):
|
|
y = a - b
|
|
# data in format [dist_1, bearing_1, dist_2, bearing_2,...]
|
|
for i in range(0, len(y), 2):
|
|
y[i + 1] = normalize_angle(y[i + 1])
|
|
return y
|
|
|
|
|
|
def residual_x(a, b):
|
|
y = a - b
|
|
y[2] = normalize_angle(y[2])
|
|
return y
|
|
|
|
|
|
def aa(x, y):
|
|
if y is not None:
|
|
return x % y
|
|
else:
|
|
return x
|
|
|
|
def bb(x,y):
|
|
try:
|
|
return x % y
|
|
except:
|
|
return x
|
|
|
|
|
|
|
|
def Hx(x, landmarks):
|
|
""" takes a state variable and returns the measurement
|
|
that would correspond to that state. """
|
|
hx = []
|
|
for lmark in landmarks:
|
|
px, py = lmark
|
|
dist = sqrt((px - x[0])**2 + (py - x[1])**2)
|
|
angle = atan2(py - x[1], px - x[0])
|
|
hx.extend([dist, normalize_angle(angle - x[2])])
|
|
return np.array(hx)
|
|
|
|
|
|
def state_mean(sigmas, Wm):
|
|
x = np.zeros(3)
|
|
|
|
sum_sin = np.sum(np.dot(np.sin(sigmas[:, 2]), Wm))
|
|
sum_cos = np.sum(np.dot(np.cos(sigmas[:, 2]), Wm))
|
|
x[0] = np.sum(np.dot(sigmas[:, 0], Wm))
|
|
x[1] = np.sum(np.dot(sigmas[:, 1], Wm))
|
|
x[2] = atan2(sum_sin, sum_cos)
|
|
return x
|
|
|
|
|
|
def z_mean(sigmas, Wm):
|
|
z_count = sigmas.shape[1]
|
|
x = np.zeros(z_count)
|
|
|
|
for z in range(0, z_count, 2):
|
|
sum_sin = np.sum(np.dot(np.sin(sigmas[:, z+1]), Wm))
|
|
sum_cos = np.sum(np.dot(np.cos(sigmas[:, z+1]), Wm))
|
|
|
|
x[z] = np.sum(np.dot(sigmas[:,z], Wm))
|
|
x[z+1] = atan2(sum_sin, sum_cos)
|
|
return x
|
|
|
|
|
|
dt = 1.0
|
|
wheelbase = 0.5
|
|
|
|
def run_localization(
|
|
cmds, landmarks, sigma_vel, sigma_steer, sigma_range,
|
|
sigma_bearing, ellipse_step=1, step=10):
|
|
|
|
plt.figure()
|
|
points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0,
|
|
subtract=residual_x)
|
|
ukf = UKF(dim_x=3, dim_z=2*len(landmarks), 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 = np.array([2, 6, .3])
|
|
ukf.P = np.diag([.1, .1, .05])
|
|
ukf.R = np.diag([sigma_range**2,
|
|
sigma_bearing**2]*len(landmarks))
|
|
ukf.Q = np.eye(3)*0.0001
|
|
|
|
sim_pos = ukf.x.copy()
|
|
|
|
# plot landmarks
|
|
if len(landmarks) > 0:
|
|
plt.scatter(landmarks[:, 0], landmarks[:, 1],
|
|
marker='s', s=60)
|
|
|
|
track = []
|
|
for i, u in enumerate(cmds):
|
|
sim_pos = move(sim_pos, u, dt/step, wheelbase)
|
|
track.append(sim_pos)
|
|
|
|
if i % step == 0:
|
|
ukf.predict(fx_args=u)
|
|
|
|
if i % ellipse_step == 0:
|
|
plot_covariance_ellipse(
|
|
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
|
|
facecolor='k', alpha=0.3)
|
|
|
|
x, y = sim_pos[0], sim_pos[1]
|
|
z = []
|
|
for lmark in landmarks:
|
|
dx, dy = lmark[0] - x, lmark[1] - y
|
|
d = sqrt(dx**2 + dy**2) + randn()*sigma_range
|
|
bearing = atan2(lmark[1] - y, lmark[0] - x)
|
|
a = (normalize_angle(bearing - sim_pos[2] +
|
|
randn()*sigma_bearing))
|
|
z.extend([d, a])
|
|
ukf.update(z, hx_args=(landmarks,))
|
|
|
|
if i % ellipse_step == 0:
|
|
plot_covariance_ellipse(
|
|
(ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
|
|
facecolor='g', alpha=0.8)
|
|
track = np.array(track)
|
|
plt.plot(track[:, 0], track[:,1], color='k', lw=2)
|
|
plt.axis('equal')
|
|
plt.title("UKF Robot localization")
|
|
plt.show()
|
|
return ukf
|
|
|
|
|
|
landmarks = np.array([[5, 10], [10, 5], [15, 15]])
|
|
cmds = [np.array([1.1, .01])] * 200
|
|
ukf = run_localization(
|
|
cmds, landmarks, sigma_vel=0.1, sigma_steer=np.radians(1),
|
|
sigma_range=0.3, sigma_bearing=0.1)
|
|
print('Final P:', ukf.P.diagonal()) |