def run(sens_meas, GT):
meta = dict()
x, y, vx, vy, dt, t = Symbol('x'), Symbol('y'), Symbol('vx'), Symbol(
'vy'), Symbol('dt'), Symbol('t')
meta['F'] = np.array([[x + vx * dt], [y + vy * dt], [vx], [vy]])
meta['H'] = np.array([[sympy.sqrt(x**2 + y**2)], [sympy.atan2(y, x)],
[((x * vx + y * vy) / sympy.sqrt(x**2 + y**2))]
]).reshape(3, 1)
meta['Q'] = np.array([
[(dt**2) / 4, 0, (dt**3) / 2, 0], #It's set with random value
[0, (dt**2) / 4, 0, (dt**3) / 2],
[(dt**3) / 2, 0, (dt**2), 0],
[0, (dt**3) / 2, 0, (dt**2)]
])
meta['R'] = np.array([[1, 0, 0], [0, 0.01, 0],
[0, 0, 2]]) #Sensor measurement noise covarience
meta['symbols'] = [x, y, vx, vy, dt, t]
filter = EKF(meta)
estimated_states = []
for meas in sens_meas:
estimated_states.append(filter.eval(meas))
with open("sample_data", 'w') as f:
for i, (est, meas,
gt) in enumerate(zip(estimated_states, sens_meas, GT)):
f.write("{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n".format(
est[0][0], est[1][0], est[2][0], est[3][0], meas[0], meas[1],
meas[2], meas[3], gt[0], gt[1], gt[2], gt[3]))
print(estimated_states[-1])
return np.array(estimated_states)
def main():
"""
Main function: get command line args, read data, call EKF, and show visualization.
:rtype : None
"""
InputFile, OutputFile, PredictFrames, YawLimit, Visualize, Debug, Clean, Learn = gatherOpts()
if not os.access( InputFile, os.R_OK ):
print 'ERROR: %s is not accessible' % InputFile
sys.exit(1)
if PredictFrames < 1:
print 'ERROR: Number of frames to predict must be greater than 0'
sys.exit(1)
if Debug:
print 'InputFile: ', InputFile
print 'OutputFile: ', OutputFile
print 'PredictFrames: ', PredictFrames
print 'YawLimit: ', YawLimit
print 'Visualize: ', Visualize
print 'Clean:', Clean
print 'Learn:', Learn
# read in any prelearned data, including where the walls are and the covariance.
dataMinX = dataMinY = dataMaxX = dataMaxY = 0
pvector = None
if not Clean and os.access( 'learned', os.R_OK ):
f = open( 'learned' )
( minx, miny, maxx, maxy, pvector ) = load( f )
dataMinX = float( minx )
dataMinY = float( miny )
dataMaxX = float( maxx )
dataMaxY = float( maxy )
f.close()
rawData = readDataFile( InputFile )
ranger, extendedData, dataMinX, dataMinY, dataMaxX, dataMaxY = analyzeData( rawData, dataMinX, dataMinY, dataMaxX, dataMaxY )
EKFilter = EKF( ranger, extendedData, dataMinX, dataMinY, dataMaxX, dataMaxY, PredictFrames, YawLimit, Debug )
# if there is a prelearned covariance, then use it
if pvector is not None:
EKFilter.PLearned = pvector
f = open( OutputFile, 'w' )
for i in EKFilter.runFilter():
f.write( "%d,%d\n" % ( int(i[0]), int(i[1]) ) )
f.close()
if Learn:
f = open( 'learned', 'w' )
dump( ( dataMinX, dataMinY, dataMaxX, dataMaxY, EKFilter.PLearned ), f )
f.close()
if Visualize:
EKFilter.visualize()
def correct(self, measurement):
subject = measurement[1]
range = measurement[2]
bearing = measurement[3]
if subject < 5:
raise Exception("Invalid Subject")
truth = self.map[subject]
truth = [truth["X"], truth["Y"]]
if subject not in self.landmarkEKFs:
newEKF = EKF(copy.deepcopy(self.robotState), range, bearing)
self.landmarkEKFs[subject] = newEKF
self.weight = 1/self.n
else:
self.weight = self.landmarkEKFs[subject].correct(range,bearing,copy.deepcopy(self.robotState), truth)
return self.weight
#! /usr/bin/python # @brief This node runs the Extended Kalman Filter for the project. # @author Michael Karrs import rospy, tf import time from sensor_msgs.msg import NavSatFix, Imu from std_msgs.msg import String, Float32 from threading import Lock from copy import deepcopy, copy from EKF import EKF import math # TODO import encoder message ekf = EKF() gps_meas_msg = NavSatFix() enc_meas_msg = String() imu_meas_msg = Imu() gps_updated = False enc_updated = False imu_updated = False # Locks so the threads don't step on eachother gps_lock = Lock() enc_lock = Lock() imu_lock = Lock() # Tick count for the encoders theta_l_meas = 0
from TR import TRSystem as TR
from EKF import EKFSystem as EKF
from timeit import default_timer as timer
template_run = "[system] complete run: episode: {}, end condition: {}, reward: {}"
template_log = "[log] episode: {}/{}, collisions: {}, elapsed time: {}"
# sys = TR()
sys = EKF(err_threshold=0.8)
sys.print_config()
start = timer()
collision_counter = 0
num_sims = 2500000
print_interval = 500000
render_flag = False
print_flag = False # disable printing (but collision and interval log)
# sum_rob = 0
for i in range(num_sims):
sys.reset_init_state()
sys.run_system()
reward = sys.reward
condition = "GOAL"
if reward > 1:
condition = "COLLISION"
collision_counter += 1
print(template_run.format(i + 1, condition, reward))
sys.render()
if render_flag:
sys.render("{}".format(i))
if print_flag or i % print_interval == 0:
elapsed_time = timer() - start
reload(R2D2)
reload(World)
reload(Plotter)
reload(EKF)
def ez_wrap(ang):
ang -= np.pi*2 * np.floor((ang + np.pi) / (2*np.pi))
return ang
# Instantiate World, Robot, Plotter, and EKF
R2D2 = R2D2.R2D2()
World = World.World()
Plotter = Plotter.Plotter(R2D2.x0, R2D2.y0, R2D2.theta0, World.width, World.height, World.Landmarks)
EKF = EKF.EKF(R2D2, World)
# Set Timeline
Tf = 20 # Sec
Ts = 0.1 # Sec
time_data = np.arange(0., Tf, Ts)
time_data = time_data.reshape([1, int(Tf/Ts)])
# Generate Truth and Calculate "Noisy Measurements"
X = np.zeros([1, time_data.size])
Y = np.zeros([1, time_data.size])
TH = np.zeros([1, time_data.size])
R = np.zeros([World.Number_Landmarks, time_data.size]) # Range Measurements
PH = np.zeros([World.Number_Landmarks, time_data.size]) # Bearing Measurements
##################################################
# initialise the Parameters
param_set = 'basic'
A = param_sets[param_set]['A']
B = param_sets[param_set]['B']
R = param_sets[param_set]['R']
Sigma = param_sets[param_set]['Sigma_0']
# get Q from the actually trained model
Q = locator.C_W
##################################################
# create EKF
ekf = EKF(R, Q, A=A, B=B, funcH=locator.funcH, funcJacobianH=locator.JacH)
# find those points where there was actual motion only...
# Yes this will induce large leaps in between, which is why
# a constant velocity model won't work, but the data is
# just not there properly.
valid_points = locator.detect_motion()
img = np.zeros((600, 300, 3), np.uint8) + 100
locator.display_wifi_maps(img)
# start off at the wrong place (by 20m)
mu = locator.data[0:2, valid_points[0]] - 20
for p in valid_points:
# image to display everything
ekf_robot = Robot(screen, BLUE)
tracer_robot = Robot(screen, GREEN)
x = 0
y = 0
# TODO: Generate path to be taken
path_index = 0
l, r = generate_ticks()
#l = [1.5*x for x in l]
#r = [1.5*x for x in r]
delta_t = .1
state = State()
ekf_state = State()
tracer_state = State()
ekf = EKF()
gps_readings = []
if (seeded):
np.random.seed(56462)
while not done:
# Clear the screen and set the screen background
screen.fill(WHITE)
# Turn off the simulation
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
done = True # Flag that we are done so we exit this loop
#########################################################
# Insert Animation Code Here
def example3_EKF():
#2.5 sine period
#### Simulated Data #####
# Time steps
T = 500
tseq = np.linspace(0, 2.5*(2*math.pi), T)
X = np.zeros((1,T))
XT = np.zeros((1,T))
Z = np.zeros((2,T))
_stateDim = 1
_sensDim = 2
# Sensor noise var
R = np.array([[0.64, 0],
[0, 0.64]])
# Constant transition matrix for State vector
A = np.eye(_stateDim)
# Constant transition matrix for Control vector
B = np.eye(_stateDim)
# Constant transition matrix for Measurement from State
C = np.ones((_sensDim,_stateDim))
# Init prediction err
P = np.eye(_stateDim)
# Constant processing noise for both sensor
Q = np.array([[0.5]])
# Bias for sensor
V = np.array([[-1],
[1]])
# Control
U = np.zeros((_stateDim,1))
for t in range(T):
XT[0,t] = math.sin(tseq[t]) + 20
X[0,t] = XT[0,t] + np.random.randn(1)*Q
Z[0,t] = V[0,0] + X[0,t] + np.random.randn(1)*R[0,0]
Z[1,t] = V[1,0] + X[0,t] + np.random.randn(1)*R[1,1]
#### Simulated Data above ########
print("[INFO] Generated Data!")
ekf = EKF()
def f(_X, _U):
return _X.copy(), np.eye(_stateDim)
def h(_Z):
return _Z.copy(), np.ones((_sensDim, _stateDim))
ekf.f = f
ekf.h = h
t = 0
mean = np.mean(Z[:2,t:t+1])
_X = np.array([[mean]])
ekf.setInit(X=_X, Z=Z[:2,t:t+1], R=R, P=P, Q=Q)
plt.axis([0, T+10, 15, 25])
plt.plot(np.arange(T),X[0,:],np.arange(T),Z[0,:],np.arange(T),Z[1,:])
plt.grid(True)
plt.show()
plt.scatter(t+1, mean, c='g')
print("t:%d Z:%s X:%s"%(t, Z[0,t], ekf.X))
print("t:%d Z:%s X:%s"%(t, Z[1,t], ekf.X))
update = []
ti = []
for t in range(1,T):
plt.grid(True)
#plt.pause(0.05)
str_p = ["t:%d"%t]
str_p.append("[G: %s]"%(ekf.G))
#print(t, 'G', ekf.G)
#plt.scatter(t+1, Z[0,t],c='g')
str_p.append("[Z: %s]"%(Z[0,t]))
#print(t, 0, int(Z[0,t]), t+1)
# initialize State
ekf.predict()
# update State
ekf.update(Z=Z[:2,t:t+1])
ti.append(t+1)
update.append(ekf.X[0,0])
#plt.scatter(t+1, int(ekf.X[0,0]),c='r')
#plt.plot(ti, Z[0,0:t], c='b')
#plt.plot(ti, Z[1,0:t], c='g')
#plt.plot(ti, update, c='r')
str_p.append("[X+: %s]"%(ekf.X[0,0]))
#print(t, 1, int(ekf.X[0,0]), t+1)
print(" ".join(str_p))
## 1. Sensor one measurement
#plt.plot(ti, Z[0,0:t], c='b')
## 2. Sensor two measurement
#plt.plot(ti, Z[1,0:t], c='y')
## 3. Mean measurement
plt.plot(ti, np.mean(Z[:,:t],axis=0), c='b')
## 4. Simulated Data
plt.plot(ti, X[0,0:t], c='g')
## 5. Ground True data
plt.plot(ti, XT[0,0:t], c='c')
## 6. Kalman filtered data
plt.plot(ti, update, c='r')
keyboardClick = False
plt.show()
while (keyboardClick != True) and ((t==T-1)):
time.sleep(0.05)
keyboardClick=plt.waitforbuttonpress()
def example2_EKF():
#Jet landing alttitude
#Simulated data for noise V_t
#Set initialize state from 1000
#Generated Data by U, V, X_init, A, B, C
# time t = 0 - 100
T = 100
#### Data simulation #### (below)
## Parameters ##
# Sensor error
R = np.array([[100]])
# Prediction transition
A = np.array([[0.95]])
# Control transition
B = np.array([[0.15]])
# Measurement transition
C = np.array([[0.5]])
# Prediction Covariance init
P = np.array([[1]])
# True state at t = 0
X_true = np.array([[1000]])
X = np.zeros((1,T))
Z = np.zeros((1,T))
X[0,0] = X_true[0,0]
# Control Signal (assumption in tutorial)
U = np.zeros((1,T))
U[0,:] = np.arange(T)
# Noise added to measurement (assumption in tutorial)
V = np.random.randint(-200,200,(1,T))
Z[0,0] = C * X[0,0] + V[0,0]
# Observation
for t in range(1, T):
_AX = A * X[0,t-1]
_BU = B * U[0,t]
X[0,t] = _AX + _BU
_CX_V = C * X[0,t] + V[0,t]
Z[0,t] = _CX_V
##### Data simulation ####### (above)
print("[INFO] Generated Data!")
ekf = EKF()
t = 0
def f(X, _U):
return np.dot(A, X.copy()) + np.dot(B, _U) , A.copy()
def h(X): return C*X.copy(), np.eye(ekf._sensDim)*C
ekf.f = f
ekf.h = h
#ekf.setInit(X=Z[:1,t:t+1], Z=Z[:1,t:t+1], R=R, P=P)
ekf.setInit(X=Z[:1,t:t+1], Z=Z[:1,t:t+1], U=U[:1,t:t+1],
R=R, P=P)
plt.axis([0, T+10, -200, 1100])
plt.grid(True)
plt.plot(list(range(T)), X[0,:], color=(1,0.5,0))
plt.scatter(t+1, Z[0,t], c='g')
print("t:%d Z:%s X:%s"%(t, Z[0,t], ekf.X))
for t in range(1,T):
plt.grid(True)
#plt.pause(0.05)
str_p = ["t:%d"%t]
str_p.append("[G: %s]"%(ekf.G))
#print(t, 'G', ekf.G)
plt.scatter(t+1, int(Z[0,t]),c='g')
str_p.append("[Z: %s]"%(Z[0,t]))
#print(t, 0, int(Z[0,t]), t+1)
# initialize State
ekf.predict(U=U[:1,t:t+1])
# update State
ekf.update(Z[:1,t:t+1])
plt.scatter(t+1, int(ekf.X[0,0]),c='r')
str_p.append("[X+: %s]"%(ekf.X[0,0]))
#print(t, 1, int(ekf.X[0,0]), t+1)
print(" ".join(str_p))
keyboardClick = False
while (keyboardClick != True) and ((t%50==0) or (t==T-1)):
time.sleep(0.05)
keyboardClick=plt.waitforbuttonpress()
def example1_EKF():
"""
Jet landing alttitude
X_true - true state alttitude in meters
Z - observation(measurement) alttitude in meters
"""
# time t = 0 - 9
# True state
X_true = np.array([[1000, 750, 563, 422, 316, 237, 178, 133, 100, 75]])
# Observation
Z = np.array([[1090, 882, 554, 233, 345, 340, 340, 79, 299, -26]])
## Parameters ##
# Sensor error
R = np.array([[200]])
# Prediction transition
A = np.array([[0.75]])
# Prediction Covariance init
P = np.array([[1]])
## Initialize State ##
ekf = EKF()
t = 0
def f(X, U): return np.dot(A, X.copy()) , A.copy()
def h(Z): return Z.copy(), np.eye(ekf._sensDim)
ekf.f = f
ekf.h = h
ekf.setInit(X=Z[:1,t:t+1], Z=Z[:1,t:t+1], R=R, P=P)
plt.axis([0, 11, -50, 1100])
plt.grid(True)
plt.scatter(t+1, Z[0,t], c='g')
print("t:%d Z:%s X:%s"%(t, Z[0,t], ekf.X))
#plt.show()
## loop overvg bf
for t in range(1, X_true.shape[1]):
plt.grid(True)
plt.pause(0.05)
str_p = ["t:%d"%t]
str_p.append("[G: %s]"%(ekf.G))
#print(t, 'G', ekf.G)
plt.scatter(t+1, int(Z[0,t]),c='g')
str_p.append("[Z: %s]"%(Z[0,t]))
#print(t, 0, int(Z[0,t]), t+1)
# initialize State
ekf.predict()
# update State
ekf.update(Z[:1,t:t+1])
plt.scatter(t+1, int(ekf.X[0,0]),c='r')
str_p.append("[X+: %s]"%(ekf.X[0,0]))
#print(t, 1, int(ekf.X[0,0]), t+1)
print(" ".join(str_p))
keyboardClick = False
while keyboardClick != True:
time.sleep(0.05)
keyboardClick=plt.waitforbuttonpress()
if current_motor_angle < 100 and previous_motor_angle > 200: #this line is supposed to check that the angle has rolled over.
del frame[-1]
frames.append(frame)
frame = [res[i]]
if i == len(res) - 1:
frames.append(frame)
print("The scan has been cleaned, now updating odometry...")
##for frame in frames:
## print("NEW FRAME, LENGTH = "+str(len(frame)))
## for scan in frame:
## print(scan['Motor encoder'])
for index in range(0, len(frames), 1):
frame = frames[index]
if index > 0: break
frame = frames[1]
(x, dx_sum) = EKF.UpdatePosition(x, dx_sum, dt1, dt2)
scan = RANSAC.ConvertToCartesian(frame, x)
lenscan = len(scan)
print(
"All " + str(lenscan) +
" points have been moved to a new dictionary, now running RANSAC on frame "
+ str(index))
#for plotting the points later
xs = []
ys = []
zs = []
for point in scan.values():
xs.append(point[0])
ys.append(point[1])
zs.append(point[2])
## xfs = []