Kalman-and-Bayesian-Filters.../experiments/slam.py
2016-11-02 08:38:18 -07:00

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())