Added EKF robot localization example.
Still needs a lot of explanation; mostly the implementation is there for now.
This commit is contained in:
Roger Labbe
parent
36d45b35bb
commit
54bce9d7a0
File diff suppressed because one or more lines are too long
@ -12,7 +12,7 @@ sys.path.insert(0,'./code') # allow us to import book_format
|
||||
|
||||
def test_filterpy_version():
|
||||
import filterpy
|
||||
min_version = [0,0,10]
|
||||
min_version = [0,0,18]
|
||||
v = filterpy.__version__
|
||||
tokens = v.split('.')
|
||||
for i,v in enumerate(tokens):
|
||||
@ -22,7 +22,8 @@ def test_filterpy_version():
|
||||
i = len(tokens) - 1
|
||||
if min_version[i] > int(tokens[i]):
|
||||
raise Exception("Minimum FilterPy version supported is {}.{}.{}.\n"
|
||||
"Please install a more recent version." .format(
|
||||
"Please install a more recent version.\n"
|
||||
" ex: pip install filterpy --upgrade".format(
|
||||
*min_version))
|
||||
v = int(tokens[0]*1000)
|
||||
|
||||
|
||||
@ -38,6 +38,36 @@ def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.02):
|
||||
return f1
|
||||
|
||||
|
||||
def plot_bicycle():
|
||||
plt.clf()
|
||||
plt.axes()
|
||||
ax = plt.gca()
|
||||
#ax.add_patch(plt.Rectangle((10,0), 10, 20, fill=False, ec='k')) #car
|
||||
ax.add_patch(plt.Rectangle((21,1), .75, 2, fill=False, ec='k')) #wheel
|
||||
ax.add_patch(plt.Rectangle((21.33,10), .75, 2, fill=False, ec='k', angle=20)) #wheel
|
||||
ax.add_patch(plt.Rectangle((21.,4.), .75, 2, fill=True, ec='k', angle=5, ls='dashdot', alpha=0.3)) #wheel
|
||||
|
||||
plt.arrow(0, 2, 20.5, 0, fc='k', ec='k', head_width=0.5, head_length=0.5)
|
||||
plt.arrow(0, 2, 20.4, 3, fc='k', ec='k', head_width=0.5, head_length=0.5)
|
||||
plt.arrow(21.375, 2., 0, 8.5, fc='k', ec='k', head_width=0.5, head_length=0.5)
|
||||
plt.arrow(23, 2, 0, 2.5, fc='k', ec='k', head_width=0.5, head_length=0.5)
|
||||
|
||||
#ax.add_patch(plt.Rectangle((10,0), 10, 20, fill=False, ec='k'))
|
||||
plt.text(11, 1.0, "R", fontsize=18)
|
||||
plt.text(8, 2.2, r"$\beta$", fontsize=18)
|
||||
plt.text(20.4, 13.5, r"$\alpha$", fontsize=18)
|
||||
plt.text(21.6, 7, "w (wheelbase)", fontsize=18)
|
||||
plt.text(0, 1, "C", fontsize=18)
|
||||
plt.text(24, 3, "d", fontsize=18)
|
||||
plt.plot([21.375, 21.25], [11, 14], color='k', lw=1)
|
||||
plt.plot([21.375, 20.2], [11, 14], color='k', lw=1)
|
||||
plt.axis('scaled')
|
||||
plt.xlim(0,25)
|
||||
plt.axis('off')
|
||||
plt.show()
|
||||
|
||||
#plot_bicycle()
|
||||
|
||||
class BaseballPath(object):
|
||||
def __init__(self, x0, y0, launch_angle_deg, velocity_ms, noise=(1.0,1.0)):
|
||||
""" Create baseball path object in 2D (y=height above ground)
|
||||
|
||||
@ -9,6 +9,7 @@ Created on Tue Apr 28 08:19:21 2015
|
||||
from math import *
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.patches import Polygon
|
||||
|
||||
wheelbase = 100 #inches
|
||||
|
||||
@ -21,17 +22,17 @@ pos = np.array([0., 0.]
|
||||
|
||||
for i in range(100):
|
||||
#if abs(steering_angle) > 1.e-8:
|
||||
dist = vel*t
|
||||
turn_radius = tan(steering_angle)
|
||||
radius = wheelbase / tan(steering_angle)
|
||||
|
||||
dist = vel*t
|
||||
arc_len = dist / (2*pi*radius)
|
||||
|
||||
turn_angle = 2*pi * arc_len
|
||||
|
||||
|
||||
cx = pos[0] - (sin(orientation) * radius)
|
||||
cy = pos[1] + (cos(orientation) * radius)
|
||||
cx = pos[0] - radius * sin(orientation)
|
||||
cy = pos[1] + radius * cos(orientation)
|
||||
|
||||
orientation = (orientation + turn_angle) % (2.0 * pi)
|
||||
pos[0] = cx + (sin(orientation) * radius)
|
||||
@ -41,3 +42,6 @@ for i in range(100):
|
||||
|
||||
plt.axis('equal')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
194
experiments/ekf4.py
Normal file
194
experiments/ekf4.py
Normal file
@ -0,0 +1,194 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sat May 30 13:24:01 2015
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Thu May 28 20:23:57 2015
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
|
||||
from math import cos, sin, sqrt, atan2, tan
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from numpy import array, dot
|
||||
from numpy.linalg import pinv
|
||||
from numpy.random import randn
|
||||
from filterpy.common import plot_covariance_ellipse
|
||||
from filterpy.kalman import ExtendedKalmanFilter as EKF
|
||||
from sympy import Matrix, symbols
|
||||
import sympy
|
||||
|
||||
def print_x(x):
|
||||
print(x[0, 0], x[1, 0], np.degrees(x[2, 0]))
|
||||
|
||||
|
||||
def normalize_angle(x, index):
|
||||
if x[index] > np.pi:
|
||||
x[index] -= 2*np.pi
|
||||
if x[index] < -np.pi:
|
||||
x[index] = 2*np.pi
|
||||
|
||||
def residual(a,b):
|
||||
y = a - b
|
||||
normalize_angle(y, 1)
|
||||
return y
|
||||
|
||||
|
||||
sigma_r = 0.1
|
||||
sigma_h = np.radians(1)
|
||||
sigma_steer = np.radians(1)
|
||||
|
||||
#only partway through. predict is using old control and movement code. computation of m uses
|
||||
#old u.
|
||||
|
||||
class RobotEKF(EKF):
|
||||
def __init__(self, dt, wheelbase):
|
||||
EKF.__init__(self, 3, 2, 2)
|
||||
self.dt = dt
|
||||
self.wheelbase = wheelbase
|
||||
|
||||
a, x, y, v, w, theta, time = symbols(
|
||||
'a, x, y, v, w, theta, t')
|
||||
|
||||
d = v*time
|
||||
beta = (d/w)*sympy.tan(a)
|
||||
r = w/sympy.tan(a)
|
||||
|
||||
|
||||
self.fxu = Matrix([[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)],
|
||||
[y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)],
|
||||
[theta+beta]])
|
||||
|
||||
self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
|
||||
self.V_j = self.fxu.jacobian(Matrix([v, a]))
|
||||
|
||||
self.subs = {x: 0, y: 0, v:0, a:0, time:dt, w:wheelbase, theta:0}
|
||||
self.x_x = x
|
||||
self.x_y = y
|
||||
self.v = v
|
||||
self.a = a
|
||||
self.theta = theta
|
||||
|
||||
|
||||
def predict(self, u=0):
|
||||
|
||||
|
||||
self.x = self.move(self.x, u, self.dt)
|
||||
|
||||
self.subs[self.theta] = self.x[2, 0]
|
||||
self.subs[self.v] = u[0]
|
||||
self.subs[self.a] = u[1]
|
||||
|
||||
|
||||
F = array(self.F_j.evalf(subs=self.subs)).astype(float)
|
||||
V = array(self.V_j.evalf(subs=self.subs)).astype(float)
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[0.1*u[0]**2, 0],
|
||||
[0, sigma_steer**2]])
|
||||
|
||||
self.P = dot(F, self.P).dot(F.T) + dot(V, M).dot(V.T)
|
||||
|
||||
|
||||
def move(self, x, u, dt):
|
||||
h = x[2, 0]
|
||||
v = u[0]
|
||||
steering_angle = u[1]
|
||||
|
||||
dist = v*dt
|
||||
|
||||
if abs(steering_angle) < 0.0001:
|
||||
# approximate straight line with huge radius
|
||||
r = 1.e-30
|
||||
b = dist / self.wheelbase * tan(steering_angle)
|
||||
r = self.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]])
|
||||
|
||||
|
||||
def H_of(x, p):
|
||||
""" compute Jacobian of H matrix where h(x) computes the range and
|
||||
bearing to a landmark for state x """
|
||||
|
||||
px = p[0]
|
||||
py = p[1]
|
||||
hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2
|
||||
dist = np.sqrt(hyp)
|
||||
|
||||
H = array(
|
||||
[[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0],
|
||||
[ (py - x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1]])
|
||||
return H
|
||||
|
||||
|
||||
def Hx(x, p):
|
||||
""" takes a state variable and returns the measurement that would
|
||||
correspond to that state.
|
||||
"""
|
||||
px = p[0]
|
||||
py = p[1]
|
||||
dist = np.sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2)
|
||||
|
||||
Hx = array([[dist],
|
||||
[atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]])
|
||||
return Hx
|
||||
|
||||
dt = 1.0
|
||||
ekf = RobotEKF(dt, wheelbase=0.5)
|
||||
|
||||
#np.random.seed(1234)
|
||||
|
||||
m = array([[5, 10],
|
||||
[10, 5],
|
||||
[15, 15]])
|
||||
|
||||
ekf.x = array([[2, 6, .3]]).T
|
||||
u = array([1.1, .01])
|
||||
ekf.P = np.diag([.1, .1, .1])
|
||||
ekf.R = np.diag([sigma_r**2, sigma_h**2])
|
||||
c = [0, 1, 2]
|
||||
|
||||
xp = ekf.x.copy()
|
||||
|
||||
plt.scatter(m[:, 0], m[:, 1])
|
||||
for i in range(150):
|
||||
xp = ekf.move(xp, u, dt/10.) # simulate robot
|
||||
plt.plot(xp[0], xp[1], ',', color='g')
|
||||
|
||||
if i % 10 == 0:
|
||||
|
||||
ekf.predict(u=u)
|
||||
|
||||
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
|
||||
facecolor='b', alpha=0.08)
|
||||
|
||||
for lmark in m:
|
||||
d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r
|
||||
a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h
|
||||
z = np.array([[d], [a]])
|
||||
|
||||
ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
|
||||
args=(lmark), hx_args=(lmark))
|
||||
|
||||
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
|
||||
facecolor='g', alpha=0.4)
|
||||
|
||||
#plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
|
||||
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
|
||||
290
experiments/ekfloc.py
Normal file
290
experiments/ekfloc.py
Normal file
@ -0,0 +1,290 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Sun May 24 08:39:36 2015
|
||||
|
||||
@author: Roger
|
||||
"""
|
||||
|
||||
#x = x x' y y' theta
|
||||
|
||||
from math import cos, sin, sqrt, atan2
|
||||
import numpy as np
|
||||
from numpy import array, dot
|
||||
from numpy.linalg import pinv
|
||||
|
||||
|
||||
def print_x(x):
|
||||
print(x[0, 0], x[1, 0], np.degrees(x[2, 0]))
|
||||
|
||||
|
||||
def control_update(x, u, dt):
|
||||
""" x is [x, y, hdg], u is [vel, omega] """
|
||||
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
|
||||
return x + np.array([[-r*sin(x[2]) + r*sin(x[2] + w*dt)],
|
||||
[ r*cos(x[2]) - r*cos(x[2] + w*dt)],
|
||||
[w*dt]])
|
||||
|
||||
|
||||
a1 = 0.001
|
||||
a2 = 0.001
|
||||
a3 = 0.001
|
||||
a4 = 0.001
|
||||
|
||||
sigma_r = 0.1
|
||||
sigma_h = a_error = np.radians(1)
|
||||
sigma_s = 0.00001
|
||||
|
||||
def normalize_angle(x, index):
|
||||
if x[index] > np.pi:
|
||||
x[index] -= 2*np.pi
|
||||
if x[index] < -np.pi:
|
||||
x[index] = 2*np.pi
|
||||
|
||||
|
||||
def ekfloc_predict(x, P, u, dt):
|
||||
|
||||
h = x[2]
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
sinh = sin(h)
|
||||
sinhwdt = sin(h + w*dt)
|
||||
cosh = cos(h)
|
||||
coshwdt = cos(h + w*dt)
|
||||
|
||||
G = array(
|
||||
[[1, 0, -r*cosh + r*coshwdt],
|
||||
[0, 1, -r*sinh + r*sinhwdt],
|
||||
[0, 0, 1]])
|
||||
|
||||
V = array(
|
||||
[[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w],
|
||||
[(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w],
|
||||
[0, dt]])
|
||||
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[a1*v**2 + a2*w**2, 0],
|
||||
[0, a3*v**2 + a4*w**2]])
|
||||
|
||||
|
||||
|
||||
x = x + array([[-r*sinh + r*sinhwdt],
|
||||
[r*cosh - r*coshwdt],
|
||||
[w*dt]])
|
||||
|
||||
P = dot(G, P).dot(G.T) + dot(V, M).dot(V.T)
|
||||
|
||||
return x, P
|
||||
|
||||
def ekfloc(x, P, u, zs, c, m, dt):
|
||||
|
||||
h = x[2]
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
sinh = sin(h)
|
||||
sinhwdt = sin(h + w*dt)
|
||||
cosh = cos(h)
|
||||
coshwdt = cos(h + w*dt)
|
||||
|
||||
F = array(
|
||||
[[1, 0, -r*cosh + r*coshwdt],
|
||||
[0, 1, -r*sinh + r*sinhwdt],
|
||||
[0, 0, 1]])
|
||||
|
||||
V = array(
|
||||
[[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w],
|
||||
[(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w],
|
||||
[0, dt]])
|
||||
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[a1*v**2 + a2*w**2, 0],
|
||||
[0, a3*v**2 + a4*w**2]])
|
||||
|
||||
|
||||
x = x + array([[-r*sinh + r*sinhwdt],
|
||||
[r*cosh - r*coshwdt],
|
||||
[w*dt]])
|
||||
|
||||
P = dot(F, P).dot(F.T) + dot(V, M).dot(V.T)
|
||||
|
||||
|
||||
R = np.diag([sigma_r**2, sigma_h**2, sigma_s**2])
|
||||
|
||||
for i, z in enumerate(zs):
|
||||
j = c[i]
|
||||
|
||||
q = (m[j][0] - x[0, 0])**2 + (m[j][1] - x[1, 0])**2
|
||||
|
||||
z_est = array([sqrt(q),
|
||||
atan2(m[j][1] - x[1, 0], m[j][0] - x[0, 0]) - x[2, 0],
|
||||
0])
|
||||
|
||||
|
||||
H = array(
|
||||
[[-(m[j, 0] - x[0, 0]) / sqrt(q), -(m[j, 1] - x[1, 0]) / sqrt(q), 0],
|
||||
[ (m[j, 1] - x[1, 0]) / q, -(m[j, 0] - x[0, 0]) / q, -1],
|
||||
[0, 0, 0]])
|
||||
|
||||
|
||||
|
||||
S = dot(H, P).dot(H.T) + R
|
||||
|
||||
#print('S', S)
|
||||
K = dot(P, H.T).dot(pinv(S))
|
||||
y = z - z_est
|
||||
normalize_angle(y, 1)
|
||||
y = array([y]).T
|
||||
#print('y', y)
|
||||
|
||||
x = x + dot(K, y)
|
||||
I = np.eye(P.shape[0])
|
||||
I_KH = I - dot(K, H)
|
||||
#print('i', I_KH)
|
||||
|
||||
P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T)
|
||||
|
||||
return x, P
|
||||
|
||||
|
||||
|
||||
def ekfloc2(x, P, u, zs, c, m, dt):
|
||||
|
||||
h = x[2]
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
sinh = sin(h)
|
||||
sinhwdt = sin(h + w*dt)
|
||||
cosh = cos(h)
|
||||
coshwdt = cos(h + w*dt)
|
||||
|
||||
F = array(
|
||||
[[1, 0, -r*cosh + r*coshwdt],
|
||||
[0, 1, -r*sinh + r*sinhwdt],
|
||||
[0, 0, 1]])
|
||||
|
||||
V = array(
|
||||
[[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w],
|
||||
[(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w],
|
||||
[0, dt]])
|
||||
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[a1*v**2 + a2*w**2, 0],
|
||||
[0, a3*v**2 + a4*w**2]])
|
||||
|
||||
|
||||
x = x + array([[-r*sinh + r*sinhwdt],
|
||||
[r*cosh - r*coshwdt],
|
||||
[w*dt]])
|
||||
|
||||
|
||||
P = dot(F, P).dot(F.T) + dot(V, M).dot(V.T)
|
||||
|
||||
|
||||
|
||||
R = np.diag([sigma_r**2, sigma_h**2])
|
||||
|
||||
for i, z in enumerate(zs):
|
||||
j = c[i]
|
||||
|
||||
q = (m[j][0] - x[0, 0])**2 + (m[j][1] - x[1, 0])**2
|
||||
|
||||
z_est = array([sqrt(q),
|
||||
atan2(m[j][1] - x[1, 0], m[j][0] - x[0, 0]) - x[2, 0]])
|
||||
|
||||
H = array(
|
||||
[[-(m[j, 0] - x[0, 0]) / sqrt(q), -(m[j, 1] - x[1, 0]) / sqrt(q), 0],
|
||||
[ (m[j, 1] - x[1, 0]) / q, -(m[j, 0] - x[0, 0]) / q, -1]])
|
||||
|
||||
|
||||
S = dot(H, P).dot(H.T) + R
|
||||
|
||||
#print('S', S)
|
||||
K = dot(P, H.T).dot(pinv(S))
|
||||
y = z - z_est
|
||||
normalize_angle(y, 1)
|
||||
y = array([y]).T
|
||||
#print('y', y)
|
||||
|
||||
x = x + dot(K, y)
|
||||
print('x', x)
|
||||
I = np.eye(P.shape[0])
|
||||
I_KH = I - dot(K, H)
|
||||
|
||||
P = dot(I_KH, P).dot(I_KH.T) + dot(K, R).dot(K.T)
|
||||
|
||||
return x, P
|
||||
|
||||
m = array([[5, 5],
|
||||
[7,6],
|
||||
[4, 8]])
|
||||
|
||||
x = array([[2, 6, .3]]).T
|
||||
u = array([.5, .01])
|
||||
P = np.diag([1., 1., 1.])
|
||||
c = [0, 1, 2]
|
||||
|
||||
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
from numpy.random import randn
|
||||
from filterpy.common import plot_covariance_ellipse
|
||||
from filterpy.kalman import KalmanFilter
|
||||
plt.figure()
|
||||
plt.plot(m[:, 0], m[:, 1], 'o')
|
||||
plt.plot(x[0], x[1], 'x', color='b', ms=20)
|
||||
|
||||
xp = x.copy()
|
||||
dt = 0.1
|
||||
np.random.seed(1234)
|
||||
|
||||
for i in range(1000):
|
||||
xp, _ = ekfloc_predict(xp, P, u, dt)
|
||||
plt.plot(xp[0], xp[1], 'x', color='g', ms=20)
|
||||
|
||||
if i % 10 == 0:
|
||||
zs = []
|
||||
|
||||
for lmark in m:
|
||||
d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r
|
||||
a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h
|
||||
zs.append(np.array([d, a]))
|
||||
|
||||
x, P = ekfloc2(x, P, u, zs, c, m, dt*10)
|
||||
|
||||
|
||||
if P[0,0] < 10000:
|
||||
plot_covariance_ellipse((x[0,0], x[1,0]), P[0:2, 0:2], std=2,
|
||||
facecolor='g', alpha=0.3)
|
||||
|
||||
plt.plot(x[0], x[1], 'x', color='r')
|
||||
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
191
experiments/ekfloc2.py
Normal file
191
experiments/ekfloc2.py
Normal file
@ -0,0 +1,191 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Mon May 25 18:18:54 2015
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
|
||||
from math import cos, sin, sqrt, atan2
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from numpy import array, dot
|
||||
from numpy.linalg import pinv
|
||||
from numpy.random import randn
|
||||
from filterpy.common import plot_covariance_ellipse
|
||||
from filterpy.kalman import ExtendedKalmanFilter as EKF
|
||||
|
||||
|
||||
def print_x(x):
|
||||
print(x[0, 0], x[1, 0], np.degrees(x[2, 0]))
|
||||
|
||||
|
||||
def normalize_angle(x, index):
|
||||
if x[index] > np.pi:
|
||||
x[index] -= 2*np.pi
|
||||
if x[index] < -np.pi:
|
||||
x[index] = 2*np.pi
|
||||
|
||||
def residual(a,b):
|
||||
y = a - b
|
||||
normalize_angle(y, 1)
|
||||
return y
|
||||
|
||||
|
||||
def control_update(x, u, dt):
|
||||
""" x is [x, y, hdg], u is [vel, omega] """
|
||||
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
|
||||
return x + np.array([[-r*sin(x[2]) + r*sin(x[2] + w*dt)],
|
||||
[ r*cos(x[2]) - r*cos(x[2] + w*dt)],
|
||||
[w*dt]])
|
||||
|
||||
a1 = 0.001
|
||||
a2 = 0.001
|
||||
a3 = 0.001
|
||||
a4 = 0.001
|
||||
|
||||
sigma_r = 0.1
|
||||
sigma_h = a_error = np.radians(1)
|
||||
sigma_s = 0.00001
|
||||
|
||||
def normalize_angle(x, index):
|
||||
if x[index] > np.pi:
|
||||
x[index] -= 2*np.pi
|
||||
if x[index] < -np.pi:
|
||||
x[index] = 2*np.pi
|
||||
|
||||
|
||||
|
||||
class RobotEKF(EKF):
|
||||
def __init__(self, dt):
|
||||
EKF.__init__(self, 3, 2, 2)
|
||||
self.dt = dt
|
||||
|
||||
def predict_x(self, u):
|
||||
self.x = self.move(self.x, u, self.dt)
|
||||
|
||||
|
||||
def move(self, x, u, dt):
|
||||
h = x[2, 0]
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
sinh = sin(h)
|
||||
sinhwdt = sin(h + w*dt)
|
||||
cosh = cos(h)
|
||||
coshwdt = cos(h + w*dt)
|
||||
|
||||
return x + array([[-r*sinh + r*sinhwdt],
|
||||
[r*cosh - r*coshwdt],
|
||||
[w*dt]])
|
||||
|
||||
|
||||
def H_of(x, landmark_pos):
|
||||
""" compute Jacobian of H matrix for state x """
|
||||
|
||||
mx = landmark_pos[0]
|
||||
my = landmark_pos[1]
|
||||
q = (mx - x[0, 0])**2 + (my - x[1, 0])**2
|
||||
|
||||
H = array(
|
||||
[[-(mx - x[0, 0]) / sqrt(q), -(my - x[1, 0]) / sqrt(q), 0],
|
||||
[ (my - x[1, 0]) / q, -(mx - x[0, 0]) / q, -1]])
|
||||
return H
|
||||
|
||||
|
||||
def Hx(x, landmark_pos):
|
||||
""" takes a state variable and returns the measurement that would
|
||||
correspond to that state.
|
||||
"""
|
||||
mx = landmark_pos[0]
|
||||
my = landmark_pos[1]
|
||||
q = (mx - x[0, 0])**2 + (my - x[1, 0])**2
|
||||
|
||||
Hx = array([[sqrt(q)],
|
||||
[atan2(my - x[1, 0], mx - x[0, 0]) - x[2, 0]]])
|
||||
return Hx
|
||||
|
||||
dt = 1.0
|
||||
ekf = RobotEKF(dt)
|
||||
|
||||
#np.random.seed(1234)
|
||||
|
||||
m = array([[5, 10],
|
||||
[10, 5],
|
||||
[15, 15]])
|
||||
|
||||
ekf.x = array([[2, 6, .3]]).T
|
||||
u = array([.5, .01])
|
||||
ekf.P = np.diag([1., 1., 1.])
|
||||
ekf.R = np.diag([sigma_r**2, sigma_h**2])
|
||||
c = [0, 1, 2]
|
||||
|
||||
xp = ekf.x.copy()
|
||||
|
||||
plt.scatter(m[:, 0], m[:, 1])
|
||||
for i in range(300):
|
||||
xp = ekf.move(xp, u, dt/10.) # simulate robot
|
||||
plt.plot(xp[0], xp[1], ',', color='g')
|
||||
|
||||
if i % 10 == 0:
|
||||
h = ekf.x[2, 0]
|
||||
v = u[0]
|
||||
w = u[1]
|
||||
|
||||
if w == 0:
|
||||
# approximate straight line with huge radius
|
||||
w = 1.e-30
|
||||
r = v/w # radius
|
||||
|
||||
sinh = sin(h)
|
||||
sinhwdt = sin(h + w*dt)
|
||||
cosh = cos(h)
|
||||
coshwdt = cos(h + w*dt)
|
||||
|
||||
ekf.F = array(
|
||||
[[1, 0, -r*cosh + r*coshwdt],
|
||||
[0, 1, -r*sinh + r*sinhwdt],
|
||||
[0, 0, 1]])
|
||||
|
||||
V = array(
|
||||
[[(-sinh + sinhwdt)/w, v*(sin(h)-sinhwdt)/(w**2) + v*coshwdt*dt/w],
|
||||
[(cosh - coshwdt)/w, -v*(cosh-coshwdt)/(w**2) + v*sinhwdt*dt/w],
|
||||
[0, dt]])
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[a1*v**2 + a2*w**2, 0],
|
||||
[0, a3*v**2 + a4*w**2]])
|
||||
|
||||
ekf.Q = dot(V, M).dot(V.T)
|
||||
|
||||
ekf.predict(u=u)
|
||||
|
||||
for lmark in m:
|
||||
d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r
|
||||
a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h
|
||||
z = np.array([[d], [a]])
|
||||
|
||||
ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
|
||||
args=(lmark), hx_args=(lmark))
|
||||
|
||||
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
|
||||
facecolor='g', alpha=0.3)
|
||||
|
||||
#plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
|
||||
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
|
||||
259
experiments/ekfloc3.py
Normal file
259
experiments/ekfloc3.py
Normal file
@ -0,0 +1,259 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Thu May 28 20:23:57 2015
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
|
||||
from math import cos, sin, sqrt, atan2, tan
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
from numpy import array, dot
|
||||
from numpy.linalg import pinv
|
||||
from numpy.random import randn
|
||||
from filterpy.common import plot_covariance_ellipse
|
||||
from filterpy.kalman import ExtendedKalmanFilter as EKF
|
||||
|
||||
|
||||
def print_x(x):
|
||||
print(x[0, 0], x[1, 0], np.degrees(x[2, 0]))
|
||||
|
||||
|
||||
def normalize_angle(x, index):
|
||||
if x[index] > np.pi:
|
||||
x[index] -= 2*np.pi
|
||||
if x[index] < -np.pi:
|
||||
x[index] = 2*np.pi
|
||||
|
||||
def residual(a,b):
|
||||
y = a - b
|
||||
normalize_angle(y, 1)
|
||||
return y
|
||||
|
||||
|
||||
sigma_r = 0.1
|
||||
sigma_h = np.radians(1)
|
||||
sigma_steer = np.radians(1)
|
||||
|
||||
#only partway through. predict is using old control and movement code. computation of m uses
|
||||
#old u.
|
||||
|
||||
class RobotEKF(EKF):
|
||||
def __init__(self, dt, wheelbase):
|
||||
EKF.__init__(self, 3, 2, 2)
|
||||
self.dt = dt
|
||||
self.wheelbase = wheelbase
|
||||
|
||||
def predict(self, u=0):
|
||||
|
||||
self.x = self.move(self.x, u, self.dt)
|
||||
|
||||
h = self.x[2, 0]
|
||||
v = u[0]
|
||||
steering_angle = u[1]
|
||||
|
||||
dist = v*self.dt
|
||||
|
||||
if abs(steering_angle) < 0.0001:
|
||||
# approximate straight line with huge radius
|
||||
r = 1.e-30
|
||||
b = dist / self.wheelbase * tan(steering_angle)
|
||||
r = self.wheelbase / tan(steering_angle) # radius
|
||||
|
||||
|
||||
sinh = sin(h)
|
||||
sinhb = sin(h + b)
|
||||
cosh = cos(h)
|
||||
coshb = cos(h + b)
|
||||
|
||||
F = array([[1., 0., -r*cosh + r*coshb],
|
||||
[0., 1., -r*sinh + r*sinhb],
|
||||
[0., 0., 1.]])
|
||||
|
||||
w = self.wheelbase
|
||||
|
||||
F = array([[1., 0., (-w*cosh + w*coshb)/tan(steering_angle)],
|
||||
[0., 1., (-w*sinh + w*sinhb)/tan(steering_angle)],
|
||||
[0., 0., 1.]])
|
||||
|
||||
print('F', F)
|
||||
|
||||
V = array(
|
||||
[[-r*sinh + r*sinhb, 0],
|
||||
[r*cosh + r*coshb, 0],
|
||||
[0, 0]])
|
||||
|
||||
|
||||
t2 = tan(steering_angle)**2
|
||||
V = array([[0, w*sinh*(-t2-1)/t2 + w*sinhb*(-t2-1)/t2],
|
||||
[0, w*cosh*(-t2-1)/t2 - w*coshb*(-t2-1)/t2],
|
||||
[0,0]])
|
||||
|
||||
|
||||
t2 = tan(steering_angle)**2
|
||||
|
||||
a = steering_angle
|
||||
d = v*dt
|
||||
it = dt*v*tan(a)/w + h
|
||||
|
||||
|
||||
|
||||
V[0,0] = dt*cos(d/w*tan(a) + h)
|
||||
V[0,1] = (dt*v*(t2+1)*cos(it)/tan(a) -
|
||||
w*sinh*(-t2-1)/t2 +
|
||||
w*(-t2-1)*sin(it)/t2)
|
||||
|
||||
|
||||
print(dt*v*(t2+1)*cos(it)/tan(a))
|
||||
print(w*sinh*(-t2-1)/t2)
|
||||
print(w*(-t2-1)*sin(it)/t2)
|
||||
|
||||
|
||||
|
||||
V[1,0] = dt*sin(it)
|
||||
|
||||
V[1,1] = (d*(t2+1)*sin(it)/tan(a) + w*cosh/t2*(-t2-1) -
|
||||
w*(-t2-1)*cos(it)/t2)
|
||||
|
||||
V[2,0] = dt/w*tan(a)
|
||||
V[2,1] = d/w*(t2+1)
|
||||
|
||||
theta = h
|
||||
|
||||
v01 = (dt*v*(tan(a)**2 + 1)*cos(dt*v*tan(a)/w + theta)/tan(a) -
|
||||
w*(-tan(a)**2 - 1)*sin(theta)/(tan(a)**2) +
|
||||
w*(-tan(a)**2 - 1)*sin(dt*v*tan(a)/w + theta)/(tan(a)**2))
|
||||
|
||||
print(dt*v*(tan(a)**2 + 1)*cos(dt*v*tan(a)/w + theta)/tan(a))
|
||||
print(w*(-tan(a)**2 - 1)*sin(theta)/(tan(a)**2))
|
||||
print(w*(-tan(a)**2 - 1)*sin(dt*v*tan(a)/w + theta)/(tan(a)**2))
|
||||
|
||||
'''v11 = (dt*v*(tan(a)**2 + 1)*sin(dt*v*tan(a)/w + theta)/tan(a) +
|
||||
w*(-tan(a)**2 - 1)*cos(theta)/tan(a)**2 -
|
||||
w*(-tan(a)**2 - 1)*cos(dt*v*tan(a)/w + theta)/tan(a)**2)
|
||||
|
||||
|
||||
print(dt*v*(tan(a)**2 + 1)*sin(dt*v*tan(a)/w + theta)/tan(a))
|
||||
print(w*(-tan(a)**2 - 1)*cos(theta)/tan(a)**2)
|
||||
print(w*(-tan(a)**2 - 1)*cos(dt*v*tan(a)/w + theta)/tan(a)**2)
|
||||
print(v11)
|
||||
print(V[1,1])
|
||||
1/0
|
||||
|
||||
V[1,1] = v11'''
|
||||
|
||||
print(V)
|
||||
|
||||
|
||||
# covariance of motion noise in control space
|
||||
M = array([[0.1*v**2, 0],
|
||||
[0, sigma_steer**2]])
|
||||
|
||||
|
||||
fpf = dot(F, self.P).dot(F.T)
|
||||
Q = dot(V, M).dot(V.T)
|
||||
print('FPF', fpf)
|
||||
print('V', V)
|
||||
print('Q', Q)
|
||||
print()
|
||||
self.P = dot(F, self.P).dot(F.T) + dot(V, M).dot(V.T)
|
||||
|
||||
|
||||
|
||||
def move(self, x, u, dt):
|
||||
h = x[2, 0]
|
||||
v = u[0]
|
||||
steering_angle = u[1]
|
||||
|
||||
dist = v*dt
|
||||
|
||||
if abs(steering_angle) < 0.0001:
|
||||
# approximate straight line with huge radius
|
||||
r = 1.e-30
|
||||
b = dist / self.wheelbase * tan(steering_angle)
|
||||
r = self.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]])
|
||||
|
||||
|
||||
def H_of(x, p):
|
||||
""" compute Jacobian of H matrix where h(x) computes the range and
|
||||
bearing to a landmark for state x """
|
||||
|
||||
px = p[0]
|
||||
py = p[1]
|
||||
hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2
|
||||
dist = np.sqrt(hyp)
|
||||
|
||||
H = array(
|
||||
[[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0],
|
||||
[ (py - x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1]])
|
||||
return H
|
||||
|
||||
|
||||
def Hx(x, p):
|
||||
""" takes a state variable and returns the measurement that would
|
||||
correspond to that state.
|
||||
"""
|
||||
px = p[0]
|
||||
py = p[1]
|
||||
dist = np.sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2)
|
||||
|
||||
Hx = array([[dist],
|
||||
[atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]])
|
||||
return Hx
|
||||
|
||||
dt = 1.0
|
||||
ekf = RobotEKF(dt, wheelbase=0.5)
|
||||
|
||||
#np.random.seed(1234)
|
||||
|
||||
m = array([[5, 10],
|
||||
[10, 5],
|
||||
[15, 15]])
|
||||
|
||||
ekf.x = array([[2, 6, .3]]).T
|
||||
u = array([.5, .01])
|
||||
ekf.P = np.diag([1., 1., 1.])
|
||||
ekf.R = np.diag([sigma_r**2, sigma_h**2])
|
||||
c = [0, 1, 2]
|
||||
|
||||
xp = ekf.x.copy()
|
||||
|
||||
plt.scatter(m[:, 0], m[:, 1])
|
||||
for i in range(300):
|
||||
xp = ekf.move(xp, u, dt/10.) # simulate robot
|
||||
plt.plot(xp[0], xp[1], ',', color='g')
|
||||
|
||||
if i % 10 == 0:
|
||||
|
||||
ekf.predict(u=u)
|
||||
|
||||
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=2,
|
||||
facecolor='b', alpha=0.3)
|
||||
|
||||
for lmark in m:
|
||||
d = sqrt((lmark[0] - xp[0, 0])**2 + (lmark[1] - xp[1, 0])**2) + randn()*sigma_r
|
||||
a = atan2(lmark[1] - xp[1, 0], lmark[0] - xp[0, 0]) - xp[2, 0] + randn()*sigma_h
|
||||
z = np.array([[d], [a]])
|
||||
|
||||
ekf.update(z, HJacobian=H_of, Hx=Hx, residual=residual,
|
||||
args=(lmark), hx_args=(lmark))
|
||||
|
||||
plot_covariance_ellipse((ekf.x[0,0], ekf.x[1,0]), ekf.P[0:2, 0:2], std=10,
|
||||
facecolor='g', alpha=0.3)
|
||||
|
||||
#plt.plot(ekf.x[0], ekf.x[1], 'x', color='r')
|
||||
|
||||
plt.axis('equal')
|
||||
plt.show()
|
||||
|
||||
@ -4,7 +4,7 @@
|
||||
@import url('http://fonts.googleapis.com/css?family=Arimo');
|
||||
|
||||
div.cell{
|
||||
width: 850px;
|
||||
width: 900px;
|
||||
margin-left: 0% !important;
|
||||
margin-right: auto;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user