From 4adfc8ca501526dfaf98a27e255613f505843243 Mon Sep 17 00:00:00 2001 From: Roger Labbe Date: Sat, 13 Jun 2015 13:22:43 -0700 Subject: [PATCH] Changes in test code for ukf - not for book. --- experiments/ukfloc.py | 105 +++++++++++---------- experiments/ukfloc2.py | 201 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 252 insertions(+), 54 deletions(-) create mode 100644 experiments/ukfloc2.py diff --git a/experiments/ukfloc.py b/experiments/ukfloc.py index f417b15..682d1c7 100644 --- a/experiments/ukfloc.py +++ b/experiments/ukfloc.py @@ -7,24 +7,24 @@ Created on Mon Jun 1 18:13:23 2015 from filterpy.common import plot_covariance_ellipse from filterpy.kalman import UnscentedKalmanFilter as UKF -from filterpy.kalman import MerweScaledSigmaPoints -from math import tan, sin, cos, sqrt, atan2 +from filterpy.kalman import MerweScaledSigmaPoints, JulierSigmaPoints +from math import tan, sin, cos, sqrt, atan2, radians, sqrt import matplotlib.pyplot as plt from numpy import array import numpy as np -from numpy.random import randn +from numpy.random import randn, seed def normalize_angle(x): - if x > np.pi: - x -= 2*np.pi - if x < -np.pi: - x = 2*np.pi + 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 + +def residual_h(aa, bb): + y = aa - bb y[1] = normalize_angle(y[1]) return y @@ -42,21 +42,18 @@ def move(x, u, dt, wheelbase): dist = v*dt - if abs(steering_angle) < 0.0001: - # approximate straight line with huge radius - r = 1.e30 - steering_angle = 1.e-5 - b = dist / wheelbase * tan(steering_angle) - r = wheelbase / tan(steering_angle) # radius + if abs(steering_angle) > 0.001: + b = dist / wheelbase * tan(steering_angle) + r = wheelbase / tan(steering_angle) # radius + sinh = sin(h) + sinhb = sin(h + b) + cosh = cos(h) + coshb = cos(h + b) - sinh = sin(h) - sinhb = sin(h + b) - cosh = cos(h) - coshb = cos(h + b) - - return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b]) - + return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b]) + else: + return x + array([dist*cos(h), dist*sin(h), 0]) def state_mean(sigmas, Wm): @@ -89,16 +86,14 @@ def z_mean(sigmas, Wm): sigma_r = .3 -sigma_h = .1#np.radians(1) -sigma_steer = np.radians(.01) +sigma_h = .01#radians(.5)#np.radians(1) +#sigma_steer = radians(10) dt = 0.1 wheelbase = 0.5 -m = array([[5, 10], - [10, 5], - [15, 15], - [20, 5], - [0, 30], [50, 30], [40, 10]]) +m = array([[5., 10], [10, 5], [15, 15], [20, 5], [0, 30], [50, 30], [40, 10]]) +#m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]]) +m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]]) def fx(x, dt, u): @@ -109,29 +104,29 @@ def Hx(x, landmark): """ takes a state variable and returns the measurement that would correspond to that state. """ - px = landmark[0] - py = landmark[1] - dist = np.sqrt((px - x[0])**2 + (py - x[1])**2) + px, py = landmark + dist = sqrt((px - x[0])**2 + (py - x[1])**2) + angle = atan2(py - x[1], px - x[0]) + return array([dist, normalize_angle(angle - x[2])]) - Hx = array([dist, atan2(py - x[1], px - x[0]) - x[2]]) - return Hx points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0) +#points = JulierSigmaPoints(n=3, kappa=3) 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.eye(3)*0.001 +ukf.Q = None#np.eye(3)*.00000 -u = array([1.1, .0000001]) +u = array([1.1, 0.]) xp = ukf.x.copy() -plt.figure() +plt.cla() plt.scatter(m[:, 0], m[:, 1]) cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] @@ -145,12 +140,13 @@ 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([cmds[-1]]*150) -cmds.extend([[v, a] for a in np.linspace(0, -np.radians(1), 25)]) +#cmds.extend([[v, a] for a in np.linspace(0, -np.radians(1), 25)]) +seed(12) cmds = np.array(cmds) cindex = 0 @@ -163,24 +159,31 @@ while cindex < len(cmds): 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.18) + if cindex % 20 == 30: + plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=18, + facecolor='b', alpha=0.58) + #print(cindex, ukf.P.diagonal()) + print(xp) 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 + + d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r + bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) + a = normalize_angle(bearing - xp[2] + randn()*sigma_h) z = np.array([d, a]) + if cindex % 20 == 0: + plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], [lmark[1], lmark[1]-d*sin(a+xp[2])], color='r') + ukf.update(z, hx_args=(lmark,)) + print(ukf.P) + print() 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) + plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=15, + facecolor='g', alpha=0.99) cindex += 1 @@ -191,9 +194,3 @@ plt.axis('equal') plt.title("UKF Robot localization") plt.show() print(ukf.P.diagonal()) - - - - - - diff --git a/experiments/ukfloc2.py b/experiments/ukfloc2.py new file mode 100644 index 0000000..77a754f --- /dev/null +++ b/experiments/ukfloc2.py @@ -0,0 +1,201 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Jun 1 18:13:23 2015 + +@author: rlabbe +""" + +from filterpy.common import plot_covariance_ellipse +from filterpy.kalman import UnscentedKalmanFilter as UKF +from filterpy.kalman import MerweScaledSigmaPoints +from math import tan, sin, cos, sqrt, atan2, radians +import matplotlib.pyplot as plt +from numpy import array +import numpy as np +from numpy.random import randn, seed + + + +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(aa, bb): + y = aa - bb + 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 move(x, u, dt, wheelbase): + h = x[2] + v = u[0] + steering_angle = u[1] + + dist = v*dt + + if abs(steering_angle) > 0.001: + b = dist / wheelbase * tan(steering_angle) + r = wheelbase / tan(steering_angle) # radius + + sinh = sin(h) + sinhb = sin(h + b) + cosh = cos(h) + coshb = cos(h + b) + + return x + array([-r*sinh + r*sinhb, r*cosh - r*coshb, b]) + else: + return x + array([dist*cos(h), dist*sin(h), 0]) + + +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 + + +def fx(x, dt, u): + return move(x, u, dt, wheelbase) + + +def Hx(x, landmark): + """ takes a state variable and returns the measurement that would + correspond to that state. + """ + + hx = [] + for lmark in landmark: + 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) + + +m = array([[5., 10], [10, 5], [15, 15], [20., 16], [0, 30], [50, 30], [40, 10]]) +#m = array([[5, 10], [10, 5], [15, 15], [20, 5],[5,5], [8, 8.4]])#, [0, 30], [50, 30], [40, 10]]) +#m = array([[5, 10], [10, 5]])#, [0, 30], [50, 30], [40, 10]]) +#m = array([[5., 10], [10, 5]]) +#m = array([[5., 10], [10, 5]]) + + +sigma_r = .3 +sigma_h = .1#radians(.5)#np.radians(1) +#sigma_steer = radians(10) +dt = 0.1 +wheelbase = 0.5 + +points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2, kappa=0, subtract=residual_x) +#points = JulierSigmaPoints(n=3, kappa=3) +ukf= UKF(dim_x=3, dim_z=2*len(m), 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, .05]) +ukf.R = np.diag([sigma_r**2, sigma_h**2]* len(m)) +ukf.Q =np.eye(3)*.00001 + + +u = array([1.1, 0.]) + +xp = ukf.x.copy() + + +plt.cla() +plt.scatter(m[:, 0], m[:, 1]) + +cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)] +cmds.extend([cmds[-1]]*50) + +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]]*150) + + +seed(12) +cmds = np.array(cmds) + +cindex = 0 +u = cmds[0] + +track = [] + +std = 16 +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=std, + facecolor='b', alpha=0.58) + + #print(cindex, ukf.P.diagonal()) + #print(ukf.P.diagonal()) + z = [] + for lmark in m: + d = sqrt((lmark[0] - xp[0])**2 + (lmark[1] - xp[1])**2) + randn()*sigma_r + bearing = atan2(lmark[1] - xp[1], lmark[0] - xp[0]) + a = normalize_angle(bearing - xp[2] + randn()*sigma_h) + z.extend([d, a]) + + #if cindex % 20 == 0: + # plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], [lmark[1], lmark[1]-d*sin(a+xp[2])], color='r') + + if cindex == 1197: + plt.plot([lmark[0], lmark[0] - d2*cos(a2+xp[2])], + [lmark[1], lmark[1] - d2*sin(a2+xp[2])], color='r') + plt.plot([lmark[0], lmark[0] - d*cos(a+xp[2])], + [lmark[1], lmark[1] - d*sin(a+xp[2])], color='k') + + ukf.update(np.array(z), hx_args=(m,)) + + if cindex % 20 == 0: + plot_covariance_ellipse((ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=std, + facecolor='g', alpha=0.5) + cindex += 1 + + +track = np.array(track) +plt.plot(track[:, 0], track[:,1], color='k') + +plt.axis('equal') +plt.title("UKF Robot localization") +plt.show()