def likelihood(self, trap_errors=False):
self.ifchdat()
# x = EKF.ekf(self.Data, self.Eve, self.Sys, DLikeDt_hvec = self.dlikedt)
# x = float(x)
# return -x
x = 0
if not trap_errors:
for data in self.Data:
x1 = EKF.ekf(data,
self.Eve,
self.Sys,
DLikeDt_hvec=self.dlikedt)
#print x1
x += x1
x = float(x)
#self.xlvec.append(self.getParm().x[0])
#self.ylvec.append(x)
return -x
else:
try:
for data in self.Data:
x1 = EKF.ekf(data,
self.Eve,
self.Sys,
DLikeDt_hvec=self.dlikedt)
#print x1
x += x1
x = float(x)
#self.xlvec.append(self.getParm().x[0])
#self.ylvec.append(x)
return -x
except:
self.likefailed = True
return float("1e12")
def main():
print('hi')
myfile = open("simulation_data.txt", 'r') # file to read
state_file_2 = open('EKF_states',
'w') # file to write states for comparison
state_file_2.write("x".ljust(10) + "y".ljust(10) + "theta".ljust(10) +
"theta_dot".ljust(10) + "\n")
global Fk
global Gk
global Hk
global Pk
global xPost
global x_best
global id_mat
global data
global K
global PPost
for line in myfile:
if line == 'pwml pwmr d1 d2 mx my gyro timestamp\n':
pass
else:
data = line.split(',')
pmwL.append(float(data[0]))
pmwR.append(float(data[1]))
d1.append(float(data[2]))
d2.append(float(data[3]))
mx.append(float(data[4]))
my.append(float(data[5]))
gyro.append(float(data[6]))
timestamp.append(float(data[7].rstrip('\n')))
inputVal = np.array([pmwL, pmwR])
outputVal = np.array([d1, d2, gyro])
inputVal = inputVal.transpose()
outputVal = outputVal.transpose()
EKF.init()
[x_estimates, P_estimates] = EKF.EKF(x_best, Pk, inputVal, outputVal, Qk,
Rk)
for i in x_estimates:
for k in i:
state_file_2.write(str(round(k[0], 3)).ljust(10, ' '))
state_file_2.write('\n')
# x_estimates is a list of 4 by 1 matrices, P_estimates is a list of 4 by 4 matrices
#plot_all_graphs(x_estimates, P_estimates)
print(x_estimates)
print(P_estimates)
state_file_2.close()
myfile.close()
def data_change(self):
print "data_change", self
inj_invl = self.inj_invl
covGrowthTime = self.covGrowthTime
varTerm = self.varTerm
hhB = self.hhB
nNa = self.nNa
nK = self.nK
c = self.Eve.Obs.C
scl = self.Eve.Sto.scale
pn = self.processNoise
sd = self.Sdiag
g = self.g
geq0 = self.geq0
leq1 = self.leq1
sumto1 = self.sumto1
self.__init__(self.rf)
self.inj_invl = inj_invl
self.covGrowthTime = covGrowthTime
self.varTerm = varTerm
self.hhB = hhB
self.nNa = nNa
self.nK = nK
self.geq0 = geq0
self.leq1 = leq1
self.sumto1 = sumto1
self.Eve.Sto.hhB = hhB
self.Eve.Sto.nNa = nNa
self.Eve.Sto.nK = nK
self.Eve.Sto.scale = scl
cnew = self.Eve.Obs.C
for i in range(len(c)):
cnew[i].sigma = c[i].sigma
for i in range(len(pn)):
self.processNoise[i].x = pn[i].x
self.Eve.Sto.B[i, i] = pn[i].x
for i in range(len(sd)):
self.Sdiag[i].x = sd[i].x
self.Eve.Sto.InitialCovSqrt[i, i] = sd[i].x
self.Initial_changed()
self.inj_invl_changed()
self.g = g
EKF.constraintsOn(self.geq0, self.leq1, self.sumto1)
def LaserStates(data_dir, Nt, O_Par):
global Y
aM = Nt * [None]
ll = EKF.LogLike(O_Par, Y[0:Nt], aM=aM)
f = open(join(data_dir, 'LaserStates'), 'w')
for x in aM:
print(x.A[0, 0], x.A[2, 0], file=f)
f.close()
return aM
def gating_distance(self,
mean,
covariance,
measurements,
only_position=False,
use_3d=True):
"""Compute gating distance between state distribution and measurements.
A suitable distance threshold can be obtained from `chi2inv95`. If
`only_position` is False, the chi-square distribution has 4 degrees of
freedom, otherwise 2.
Parameters
----------
mean : ndarray
Mean vector over the state distribution (8 dimensional).
covariance : ndarray
Covariance of the state distribution (8x8 dimensional).
measurements : ndarray
An Nx4 dimensional matrix of N measurements, each in
format (x, y, a, h) where (x, y) is the bounding box center
position, a the aspect ratio, and h the height.
only_position : Optional[bool]
If True, distance computation is done with respect to the bounding
box center position only.
Returns
-------
ndarray
Returns an array of length N, where the i-th element contains the
squared Mahalanobis distance between (mean, covariance) and
`measurements[i]`.
"""
if not use_3d:
corner_points, corner_points_3d = self.calculate_corners(mean)
H_2d = self.get_2d_measurement_matrix(mean, corner_points,
corner_points_3d)
min_x, min_y = np.amin(corner_points, axis=0)[:2]
max_x, max_y = np.amax(corner_points, axis=0)[:2]
cov = self.project_cov_2d(mean, covariance, H_2d)
mean = np.array([min_x, min_y, max_x - min_x, max_y - min_y])
else:
mean, cov = mean[:7], self.project_cov(mean, covariance)
if only_position:
if use_3d:
mean, cov = mean[[0, 2]], np.reshape(
cov[[0, 0, 2, 2], [0, 2, 0, 2]], (2, 2))
measurements = measurements[:, [0, 2]]
else:
mean, cov = mean[:2], cov[:2, :2]
measurements = measurements[:, :2]
self.LIMIT = 0.3
if np.amax(cov) > self.LIMIT:
cov_2 = cov * self.LIMIT / np.amax(cov)
return EKF.squared_mahalanobis_distance(mean, cov2, measurements)
def loglike(self, A0=None, A1=None):
simParams = self.getParams()
if A0 == None:
A0 = simParams[0]
if A1 == None:
A1 = simParams[1]
Mtest = copy.deepcopy(self.M)
self.setParams(A0, A1, Mtest)
LL = EKF.ekf(self.Data, Mtest)
return LL
def run(self):
mu0 = array([[-4.0, -4.0, math.pi / 2]]) # FILL ME IN: initial mean
Sigma0 = np.eye(3) #[]# FILL ME IN: initial covariance
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = mu0 # Array in which to append mu_t as a row vector after each iteration
self.ekf = EKF(mu0, Sigma0, self.R, self.Q)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
for t in range(size(self.U, 0)):
self.ekf.prediction(self.U[t, :])
self.ekf.update(self.Z[t, :])
self.MU = concatenate((self.MU, self.ekf.getMean()))
self.VAR = concatenate((self.VAR, self.ekf.getVariances()))
print(self.ekf.getMean())
print(self.ekf.getSigma())
def initEKF(sensors):
initial_state = numpy.matrix([[0], [0], [0], [0], [0], [0], [0], [0]])
initial_probability = numpy.eye(8)
process_covariance = numpy.eye(8) * 1e-3
return EKF.ExtendedKalmanFilter(EKF.transition_funct,
EKF.transition_jacobian_funct,
[s.observation_funct for s in sensors],
[s.jacobian_funct
for s in sensors], initial_state,
initial_probability, process_covariance,
[s.covariance for s in sensors])
def estimateEKF(camPoses):
x = np.zeros(3)
P = np.zeros((3, 3))
V = np.diag([(0.2)**2, (1 * 3.14 / 180)**2])
R = np.diag([10**2, 10**2, 2**2])
EKF1 = EKF.EKF(x, P, V, R, L=1) #Create a Kalman filter for the odometry
EKFposes = [[], []] #create a list of the estiamted poses
for i in range(len(speed)):
EKF1.Prediction([speed[i], angle[i] * 0.01745])
EKF1.Update([camPoses[0][i], camPoses[1][i], camPoses[2][i]])
x = EKF1.x
EKFposes[0].append(x[0])
EKFposes[1].append(x[1])
return EKFposes
def run(self):
mu0 = array([-4.0, -4.0, math.pi / 2])
Sigma0 = eye(3) #[]# FILL ME IN: initial covariance
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = array(
[mu0]
) # Array in which to append mu_t as a row vector after each iteration
self.ekf = EKF(mu0, Sigma0, self.R, self.Q)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
for t in range(size(self.U, 0)):
self.ekf.prediction(self.U[t, :])
self.ekf.update(self.Z[t, :])
self.MU = concatenate((self.MU, [self.ekf.getMean()]))
self.VAR = concatenate((self.VAR, self.ekf.getVariances()))
# Your code goes here
print("Please add code if desired")
self.plot()
def depthToPoint(drone, sensorIndex, depth):
# p1 = np.add(drone.sensors[sensorIndex].offset, [drone.x, drone.y])
# p2 = [p1[0], p1[1]+depth]
# p2 = rotate(p1, p2, drone.sensors[sensorIndex].angle)
# #p1 = rotate([self.pos[0], self.pos[1]], p1, self.rot)
# p2 = rotate([drone.x, drone.y], p2, drone.yaw)
# return p2
# ignore sensor offset
theta = drone.sensors[sensorIndex].angle + drone.yaw
theta = EKF.wraptopi(theta)
p1 = [depth * math.cos(theta), depth * math.sin(theta)]
p2 = [drone.x + p1[0], drone.y+ p1[1]]
return p2
def gating_distance(self,
mean,
covariance,
measurements,
only_position=False,
use_3d=False):
"""Compute gating distance between state distribution and measurements.
A suitable distance threshold can be obtained from `chi2inv95`. If
`only_position` is False, the chi-square distribution has 4 degrees of
freedom, otherwise 2.
Parameters
----------
mean : ndarray
Mean vector over the state distribution (8 dimensional).
covariance : ndarray
Covariance of the state distribution (8x8 dimensional).
measurements : ndarray
An Nx4 dimensional matrix of N measurements, each in
format (x, y, a, h) where (x, y) is the bounding box center
position, a the aspect ratio, and h the height.
only_position : Optional[bool]
If True, distance computation is done with respect to the bounding
box center position only.
Returns
-------
ndarray
Returns an array of length N, where the i-th element contains the
squared Mahalanobis distance between (mean, covariance) and
`measurements[i]`.
"""
projected_mean, projected_covariance = self.project(mean, covariance)
if only_position:
projected_mean, projected_covariance = projected_mean[:
2], projected_covariance[:
2, :
2]
measurements = measurements[:, :2]
max_val = np.amax(projected_covariance)
# LIMIT = max(mean[2], mean[3]) #*(1 + abs(3*mean[0]/self.img_center - 1))
if max_val > self.LIMIT:
projected_covariance *= self.LIMIT / max_val
return EKF.squared_mahalanobis_distance(projected_mean,
projected_covariance,
measurements)
def NegLogLike(P):
"""Use EKF.LogLike to calculate the log likelihood of the first Nt
values in the Y sequence.
"""
global Y, Nt, best
rad, Tr = P[0:2]
devEp, devEta = P[8:10]
if rad * rad > 0.25 or Tr < 0.5 or devEp < 0.0 or devEta < 0.0:
return 1e20 # If parameters are bad, return big number
try:
L = ParamCalc(*P[:10])
cost = -EKF.LogLike(L, Y[0:Nt]) + (devEta / 1e-2)**2
except (ValueError, RuntimeError) as err:
#print 'NegLogLike returning 1e20 because ', err
return 1e20
if cost < best - 1:
best = cost
print("best=%f" % best)
return cost
def LaserLogLike(
data_dir,
O_Par,
N_step, # Number of steps away from peak in each direction
Nt):
s = O_Par[4]
b = O_Par[5]
Frange = numpy.arange(-N_step, N_step + 1, 1.0)
s_range = (Frange * 5e-3 / N_step + 1.0) * s
b_range = (Frange * 5e-3 / N_step + 1.0) * b
Par = copy.copy(O_Par)
f = open(join(data_dir, 'LaserLogLike'), 'w')
for ss in s_range:
for bb in (b_range):
Par[4] = ss
Par[5] = bb
ll = EKF.LogLike(Par, Y[0:Nt])
print(ss, bb, ll, file=f)
print(' ', file=f)
f.close()
def calculate(
DevEta, # sqrt(variance of measurement noise)
DevEpsilon, # sqrt(variance of state noise)
ts, # Sample interval
Nt, # Number of samples
Ystep # Quantization size for y
):
""" Integrate the Lorenz system create X and Y and quantize Y
"""
(X, Y, F, G) = EKF.TanGen0(
DevEta=DevEta,
DevEpsilon=DevEpsilon,
s=s,
r=r,
b=b,
ts=ts,
Nt=Nt,
)
Y = np.array(Y)
Y = np.ceil(Y / Ystep - 0.5) * Ystep
return (np.array(X), Y)
def setupEKF():
z1 = lambda x: x[0] + 0 * x[1] + 0 * x[2]
z2 = lambda x: 0 * x[0] + x[1] + 0 * x[2]
z3 = lambda x: 0 * x[0] + 0 * x[1] + x[2]
x = lambda x, u: x + u * EKF.my_dt
y = lambda x, u: x + u * EKF.my_dt
theta = lambda x, u: x + u * EKF.my_dt
g = [x, y, theta]
h = [z1, z2, z3]
loc = np.matrix([[0], [0], [0]])
Q = np.eye(np.matrix(loc).shape[0])
R = np.eye(np.matrix(loc).shape[0])
sigma = np.eye(loc.shape[0])
return EKF.EKF(g, h, sigma, Q, R, loc)
def NLL_Fixed_Noise(P):
"""Use EKF.LogLike to calculate the log likelihood of the first Nnll
values in the Y sequence. Like NegLogLike() but noise is fixed.
"""
global Y, best
Nnll = 250
rad = P[0]
Tr = P[1]
if rad**2 > 0.25 or Tr < 0.5:
return 1e20
try:
L = ParamCalc(*(list(P[0:8]) + [4.0 / P[7], 1e-7]))
# P[7] is scale. The above line sticks dev_eps = 4.0 channels
# and dev_eta = 1e-7 on to the end of the parameter list and
# converts from (rad,Tr) to ic.
LL = EKF.LogLike(L, Y[0:Nnll])
except ValueError as err:
#print 'NLL_Fixed_noise returning 1e20 because ', err
return 1e20
if -LL < best - 1:
best = -LL
print("best=%f" % best)
return (-LL)
class RunEKF(object):
def __init__(self, Q_factor, R_factor):
self.R = array([[2.0, 0.0, 0.0], [0.0, 2.0, 0.0],
[0.0, 0.0, (2.0 * math.pi) / 180]]) * R_factor
self.Q = array([[1.0, 0.0], [0.0, math.pi / 180]]) * Q_factor
self.U = [
] # Array that stores the control data where rows increase with time
self.Z = [
] # Array that stores the measurement data where rows increase with time
self.XYT = [
] # Array that stores the ground truth pose where rows increase with time
self.MU = [
] # Array in which to append mu_t as a row vector after each iteration
self.VAR = [] # Array in which to append var(x), var(y), var(theta)
# from the EKF covariance as a row vector after each iteration
# Read in the control and measurement data from their respective text files
# and populates self.U and self.Z
def readData(self, filenameU, filenameZ, filenameXYT):
print "Reading control data from %s and measurement data from %s" % (
filenameU, filenameZ)
self.U = loadtxt(filenameU, comments='#', delimiter=',')
self.Z = loadtxt(filenameZ, comments='#', delimiter=',')
self.XYT = loadtxt(filenameXYT, comments='#', delimiter=',')
# Iterate from t=1 to t=T performing the two filtering steps
def run(self):
mu0 = array([[-4.0, -4.0, math.pi / 2]]) # FILL ME IN: initial mean
Sigma0 = eye(3) #[]# FILL ME IN: initial covariance
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = mu0 # Array in which to append mu_t as a row vector after each iteration
self.ekf = EKF(mu0, Sigma0, self.R, self.Q)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
for t in range(size(self.U, 0)):
self.ekf.prediction(self.U[t, :])
self.ekf.update(self.Z[t, :])
self.MU = concatenate((self.MU, self.ekf.getMean()))
self.VAR = concatenate((self.VAR, self.ekf.getVariances()))
print "FINAL MEAN VECTOR IS:\n", self.ekf.getMean()
print "\nFINAL COVARIANCE MATRIX IS:\n", self.ekf.getCovariance()
# Plot the resulting estimate for the robot's trajectory
def plot(self):
# Plot the estimated and ground truth trajectories
ground_truth = plt.plot(self.XYT[:, 0],
self.XYT[:, 1],
'g.-',
label='Ground Truth')
mean_trajectory = plt.plot(self.MU[:, 0],
self.MU[:, 1],
'r.-',
label='Estimate')
plt.legend()
plt.savefig(PLOT_DIR + "true_trajectory.png")
plt.close()
# Try changing this to different standard deviations
sigmas = np.arange(0, 3)
for sigma in sigmas:
# Plot the errors with error bars
Error = self.XYT - self.MU
T = range(size(self.XYT, 0))
f, axarr = plt.subplots(3, sharex=True)
axarr[0].plot(T, Error[:, 0], 'r-')
axarr[0].plot(T, sigma * sqrt(self.VAR[:, 0]), 'b--')
axarr[0].plot(T, -sigma * sqrt(self.VAR[:, 0]), 'b--')
axarr[0].set_title('X error')
axarr[0].set_ylabel('Error (m)')
axarr[1].plot(T, Error[:, 1], 'r-')
axarr[1].plot(T, sigma * sqrt(self.VAR[:, 1]), 'b--')
axarr[1].plot(T, -sigma * sqrt(self.VAR[:, 1]), 'b--')
axarr[1].set_title('Y error')
axarr[1].set_ylabel('Error (m)')
axarr[2].plot(T, degrees(unwrap(Error[:, 2])), 'r-')
axarr[2].plot(T, sigma * degrees(unwrap(sqrt(self.VAR[:, 2]))),
'b--')
axarr[2].plot(T, -sigma * degrees(unwrap(sqrt(self.VAR[:, 2]))),
'b--')
axarr[2].set_title('Theta error (degrees)')
axarr[2].set_ylabel('Error (degrees)')
axarr[2].set_xlabel('Time')
plt.savefig(PLOT_DIR + str(sigma) + "_errors.png")
plt.close()
return
z1 = lambda x : x[0] + 0 * x[1] + 0 * x[2] + 0 * x[3] z2 = lambda x :0 * x[0] + x[1] + 0 * x[2] + 0 * x[3] z3 = lambda x :0 * x[0] + 0 * x[1] + x[2] + 0 * x[3] z4 = lambda x :0 * x[0] + 0 * x[1] + 0 * x[2] + x[3] g = [ v, v, p, p ] h = [z1, z2, z3, z4] x = 0 pos = np.matrix([ [x], [x], [x], [x] ]) vx = 0 u = [ vx, vx, vx, vx ] Q = np.eye(pos.shape[0]) * 2 R = np.eye(pos.shape[0]) * .5 sigma = np.eye(pos.shape[0]) # filters ekf = EKF.EKF(g, h, sigma, Q, R, pos) md_filter = md_filter.md_filter(2, prop, n, [0, 1]) poly_x = poly_filter.poly_filter(100, 3) poly_y = poly_filter.poly_filter(100, 3) # hold pos data new_x = [] new_y = [] new_z = [] out_x = [] out_y = [] out_z = [] count = [] # varible to hold the last position to for plotting reasons last = pos
my_y = df['field.pose.position.y'].values.tolist()
p = lambda x, u, : x + u * .1
v = lambda x, u, : u + x * 0
z1 = lambda x: x[0] + 0 * x[1] + 0 * x[2] + 0 * x[3]
z2 = lambda x: 0 * x[0] + x[1] + 0 * x[2] + 0 * x[3]
z3 = lambda x: 0 * x[0] + 0 * x[1] + x[2] + 0 * x[3]
z4 = lambda x: 0 * x[0] + 0 * x[1] + 0 * x[2] + x[3]
g = [v, v, p, p]
h = [z1, z2, z3, z4]
x = 0
loc = np.matrix([[x], [x], [x], [x]])
Q = np.eye(loc.shape[0]) * 2
R = np.eye(loc.shape[0]) * .5
sigma = np.eye(loc.shape[0])
ekf = EKF.EKF(g, h, sigma, Q, R, loc)
real_path = []
predicted_path = []
vx = 1
for ii in xrange(len(my_x)):
real_path.append(my_x[ii])
u = [vx, vx, vx, vx]
z = [[0], [0], [my_x[ii]], [my_y[ii]]]
temp = np.array(ekf.update(u, z))
predicted_path.append(temp[2][0])
f1 = plt.figure()
ax = f1.add_subplot(111)
class RunEKF(object):
def __init__(self):
self.R = array([[2.0, 0.0, 0.0], [0.0, 2.0, 0.0],
[0.0, 0.0, radians(2)]]) * 1E-4
self.Q = array([[1.0, 0.0], [0.0, radians(1)]]) * 1E-6
self.U = [
] # Array that stores the control data where rows increase with time
self.Z = [
] # Array that stores the measurement data where rows increase with time
self.XYT = [
] # Array that stores the ground truth pose where rows increase with time
self.MU = [
] # Array in which to append mu_t as a row vector after each iteration
self.VAR = [] # Array in which to append var(x), var(y), var(theta)
# from the EKF covariance as a row vector after each iteration
# Read in the control and measurement data from their respective text files
# and populates self.U and self.Z
def readData(self, filenameU, filenameZ, filenameXYT):
print(
"Reading control data from {} and measurement data from {}".format(
filenameU, filenameZ))
self.U = loadtxt(filenameU, comments='#', delimiter=',')
self.Z = loadtxt(filenameZ, comments='#', delimiter=',')
self.XYT = loadtxt(filenameXYT, comments='#', delimiter=',')
return
# Iterate from t=1 to t=T performing the two filtering steps
def run(self):
mu0 = array([-4.0, -4.0, math.pi / 2])
Sigma0 = eye(3) #[]# FILL ME IN: initial covariance
self.VAR = array([[Sigma0[0, 0], Sigma0[1, 1], Sigma0[2, 2]]])
self.MU = array(
[mu0]
) # Array in which to append mu_t as a row vector after each iteration
self.ekf = EKF(mu0, Sigma0, self.R, self.Q)
# For each t in [1,T]
# 1) Call self.ekf.prediction(u_t)
# 2) Call self.ekf.update(z_t)
# 3) Add self.ekf.getMean() to self.MU
for t in range(size(self.U, 0)):
self.ekf.prediction(self.U[t, :])
self.ekf.update(self.Z[t, :])
self.MU = concatenate((self.MU, [self.ekf.getMean()]))
self.VAR = concatenate((self.VAR, self.ekf.getVariances()))
# Your code goes here
print("Please add code if desired")
self.plot()
# Plot the resulting estimate for the robot's trajectory
def plot(self):
# Plot the estimated and ground truth trajectories
ground_truth = plt.plot(self.XYT[:, 0],
self.XYT[:, 1],
'g.-',
label='Ground Truth')
mean_trajectory = plt.plot(self.MU[:, 0],
self.MU[:, 1],
'r.-',
label='Estimate')
plt.legend()
# Try changing this to different standard deviations
sigma = 1 # 2 or 3
# Plot the errors with error bars
Error = self.XYT - self.MU
T = range(size(self.XYT, 0))
f, axarr = plt.subplots(3, sharex=True)
axarr[0].plot(T, Error[:, 0], 'r-')
axarr[0].plot(T, sigma * sqrt(self.VAR[:, 0]), 'b--')
axarr[0].plot(T, -sigma * sqrt(self.VAR[:, 0]), 'b--')
axarr[0].set_title('X error')
axarr[0].set_ylabel('Error (m)')
axarr[1].plot(T, Error[:, 1], 'r-')
axarr[1].plot(T, sigma * sqrt(self.VAR[:, 1]), 'b--')
axarr[1].plot(T, -sigma * sqrt(self.VAR[:, 1]), 'b--')
axarr[1].set_title('Y error')
axarr[1].set_ylabel('Error (m)')
axarr[2].plot(T, degrees(unwrap(Error[:, 2])), 'r-')
axarr[2].plot(T, sigma * degrees(unwrap(sqrt(self.VAR[:, 2]))), 'b--')
axarr[2].plot(T, -sigma * degrees(unwrap(sqrt(self.VAR[:, 2]))), 'b--')
axarr[2].set_title('Theta error (degrees)')
axarr[2].set_ylabel('Error (degrees)')
axarr[2].set_xlabel('Time')
plt.show()
return
for i in range(N): # u = control[:, i:i+1] # steer = atan2(path[target][1] - cycle_sim.state[1,0], path[target][0] - cycle_sim.state[0,0]) - cycle_sim.state[2,0] # steer = (steer + pi)%(2*pi) - pi; # if(fabs(steer) > pi/2) and sqrt((path[target][1] - cycle_sim.state[1,0])**2 + (path[target][0] - cycle_sim.state[0,0])**2) < 5: # steer = 0.; # target = (target + 1)%L; # steer = min(pi/2, max(-pi/2, steer)) # u = np.matrix([[vmax - cycle_sim.state[3,0]],[ steer]]) u = np.matrix([[2. + 0.*np.random.randn() - cycle_sim.state[3,0]],[ 0.5*sin(0.005*i) + 0.*np.random.randn()]]) # Move actual system nX, nZ = cycle_sim.move(u) xEst, Qt = EKF.run(cycle_model, u, nZ); # xEst, nY = cycle_model.step(cycle_model.dt, cycle_model.state, u) cycle_model.set(xEst) if(i%W.frequency == 0): W.plot() # velocity def summary(name, i, type = "X"): fig1, ax = plt.subplots()
car.setOdometry(True)
car.setOdometry([0.05, 0.5]) #*3.14/180
#car.setOdometry([0,0])
P = np.diag([0.00005, 0.00005, 0.008])
V = np.diag([(0.05)**2, (0.5 * 3.14 / 180)**2])
speed, angle = [], []
for a in xrange(4): #create a retangular path
for i in xrange(400):
angle.append(0)
for i in xrange(7):
angle.append(5)
for i in xrange(len(angle)): #set the speed to a constant along the path
speed.append(0.5)
car.sim_Path(speed, angle)
kalman = EKF.EKF()
x = [0, 0, 0]
u = car.readOdometry()
uncertainty = []
poses = [[], [], []]
#print len(u[0])
for a in xrange(len(u[0])):
poses[0].append(x[0])
poses[1].append(x[1])
poses[2].append(x[2])
u_ = [
float(u[0][a]) + np.random.normal(0, 0.05),
(float(u[1][a]) + np.random.normal(0, 0.5)) * 0.01745
]
print u_
x, P = kalman.Prediction(x, u_, P, V)
def __init__(self, ho):
#h.mulfit_after_quad_pycallback = self.after_quad
pc = h.ParallelContext()
nhost = int(pc.nhost_bbs())
if nhost > 1:
fitglobals.verbose = 0
self.xlvec = h.Vector()
self.ylvec = h.Vector()
self.g = None
self.rf = ho
ol = []
vl = self.rf.yvarlist
fl = self.rf.fitnesslist
tlast = 0
self.n_chdat = 0
for i in range(len(vl)):
self.n_chdat += fl.o(i).n_chdat
tl = list(fl.o(i).xdat_)
o = obs.NeuronObservable(vl.o(i), tl)
o.sigma = 0.01
ol.append(o)
if (tlast < tl[-1]):
tlast = tl[-1]
s = h.Vector()
cvodewrap.active(1)
cvodewrap.states(s)
assert (len(s) > 0)
assert (len(vl) > 0)
self.covGrowthTime = 100
self.varTerm = 1
self.processNoise = []
self.Sdiag = []
Sto = sto.StochasticModel(len(s), tlast)
for i in range(len(s)):
self.Sdiag.append(WrappedVal(Sto.InitialCovSqrt[i, i]))
for i in range(len(s)):
self.processNoise.append(WrappedVal(Sto.B[i, i]))
Obs = obs.ObservationModel(ol)
self.Eve = eve.EventTable(Sto, Obs)
self.hhB = self.Eve.Sto.hhB
self.nNa = self.Eve.Sto.nNa
self.nK = self.Eve.Sto.nK
self.Sys = detsys.NeuronModel()
self.inj_invl = 1.0
self.Eve.newInjectionInterval(self.inj_invl)
# self.inj_invl_changed(Sys, P.tstop)
# self.M = models.Model(Sys, Obs, P)
self.Data = self.__data(fl, self.Eve)
self.pf = self.getParmFitness()
self.pf.verbose = fitglobals.verbose
self.dlikedt = h.Vector()
self.likefailed = False
#CONSTRAINTS GUI INIT
s = h.Vector()
cvodewrap.states(s)
nstates = len(s)
self.geq0 = []
self.leq1 = []
self.sumto1 = []
self.cvxopt_sel = True
self.custom_sel = True
self.nsums = 2
for j in range(self.nsums):
self.sumto1.append([])
for i in range(nstates):
self.geq0.append(xstruct())
self.leq1.append(xstruct())
for j in range(self.nsums):
self.sumto1[j].append(xstruct())
EKF.constraintsOn(self.geq0, self.leq1, self.sumto1)
anchor_distance[i, :] = distance2D(
[gps, gps_all[GlobalVals.ANCHOR[i] - 1]])
else:
temp = distance2D(
[gps, gps_all[i - 1], rssi.distance])
anchor_distance[i, :] = distance2D([
gps, gps_all[GlobalVals.ANCHOR[i] - 1],
rssi.distance
])
# print(anchor_distance)
# print("===============")
if Q_Xsens:
q_sensor = imu.raw_qt.reshape(4)
node = EKF(settings, dt, node, IMU_i, anchor_position,
GPS_data, anchor_distance, Q_Xsens, q_sensor) # EKF
x_h = np.array([node.x_h[:, -1]]).T
output.writerow([GlobalVals.SYSID, x_h[0][0],x_h[1][0],x_h[2][0],x_h[3][0],x_h[4][0],x_h[5][0],x_h[6][0],x_h[7][0],x_h[8][0],x_h[9][0],node.roll,node.pitch,node.yaw,\
gps_all[0].lat, gps_all[0].lon, gps_all[0].alt, gps_all[1].lat, gps_all[1].lon, gps_all[1].alt, gps_all[2].lat, gps_all[2].lon, gps_all[2].alt, gps_all[3].lat, gps_all[3].lon, gps_all[3].alt,
imu.gyros[0][0],imu.gyros[1][0],imu.gyros[2][0],accel[0][0],accel[1][0],accel[2][0],imu.raw_qt[0][0],imu.raw_qt[1][0],imu.raw_qt[2][0],imu.raw_qt[3][0],epoch,\
imu.mag_vector[0][0],imu.mag_vector[1][0],imu.mag_vector[2][0],rssi.distance])
posENU_EKF = np.array([x_h[0][0], x_h[1][0], x_h[2][0]]).T
llaEKF = enu2lla(posENU_EKF, gps_ref)
print('Lon: ', llaEKF[1], ', Lat: ', llaEKF[0], ', Alt: ',
llaEKF[2], 'Time: ', timeLocal, '\n')
with GlobalVals.LLA_EKF_BUFFER_MUTEX:
GlobalVals.LLA_EKF_BUFFER.append(
GPS(sysID, llaEKF[0], llaEKF[1], llaEKF[2], epoch))
def constraintsButton(self):
EKF.constraintsOn(self.geq0, self.leq1, self.sumto1)
for i in xrange(400):
angle.append(0)
for i in xrange(9):
angle.append(10)
for i in xrange(len(angle)): #set the speed to a constant along the path
speed.append(1)
robot.sim_Path(speed, angle) #run in a rectangular path
speed, angle = robot.readOdometry() #reads the sensors
x = np.zeros(3)
P = np.zeros((3, 3))
V = np.diag([(0.2)**2, (1 * 3.14 / 180)**2])
R = np.diag([10**2, 10**2, 2**2])
EKF1 = EKF.EKF(x, P, V, R, L=1) #Create a Kalman filter for the odometry
EKF2 = EKF.EKF(x, P, V, R, L=1) #Create a Kalman filter for the odometry
cam = upper_cam.UpperCam([10, 10, 0.5], robot)
cam.readPath()
camPoses = cam.readPoses()
Pposes = [[], []]
Uposes = [[], []]
P_ = []
P2_ = []
noiseposes = [[], [], []]
for i in range(len(speed)):
Pposes[0].append(x[0])
Pposes[1].append(x[1])
EKF1.Prediction([speed[i], angle[i] * 0.01745])
#EKF.Update(cam.noisePose([robot.poses[0][i],robot.poses[1][i],robot.poses[2][i]*0.01745]))
Xe = np.array([y,x,0.,0.])[np.newaxis,:].T
P = np.zeros((4,4))
dt = 1/30
if flag % 7 == 0:
prev_x = x
prev_y = y
flag = flag + 1
if not np.isnan(x) and not np.isnan(y):
if not np.isnan(prev_x) and not np.isnan(prev_y):
dist = math.hypot(x - prev_x, y - prev_y)
if (not_detect < 5 and dist >60):
(Xe,P) = EKF.predict(Xe,P,dt)
elif(not_detect > 5 or dist <60):
(Xe,P) = EKF.ApplyEKF(Xe,P,dt,y,x)
not_detect = 0
elif(not_detect> 5):
print("else: ",not_detect)
(Xe,P) = EKF.ApplyEKF(Xe,P,dt,y,x)
if Xe[2] > 0:
R_avg, scored = houghMethod(frame1,flag, R_avg)
if scored==1:
score = score +1
for j in range(7):
ret, frame = cam.read()
frame[:,:,:] = 0
cv2.putText(frame, "Scored!!!", (300,360), cv2.FONT_HERSHEY_SIMPLEX, 5, (255,255,255), 2)
def Filter(
Y, # Observation sequence
mux, # Initial state mean
Ystep, # Quantization of forecasts
sigma_epsilon, # scalar variance of measurement noise
sigma_eta, # scalar variance of state noise
X):
'''Run Kalman filter on sequence Y. Calculate and return AILLY
(Average Integrated Log Likelihood of Y)
'''
Nt = len(Y)
# Initial state variance
Sigmax = np.eye(3) * 1e-3
# Define functions for EKF
state_noise = np.eye(3) * sigma_eta
SigmaEta = lambda t, x: state_noise
measurement_noise = np.eye(1) * sigma_epsilon
SigmaEpsilon = lambda t, x: measurement_noise
F = lambda t, x: lorenz.Ltan_one(x, s, b, r, ts)
# Call the filtering function
RD = {'mu_y': [], 'I_y': []}
EKF.ForwardEKF(Y, mux, Sigmax, SigmaEta, SigmaEpsilon, F, G_func, RD)
# Generate error time series
AILLY = 0.0 # Average Incremental Log Likelihood of Y
for t in range(Nt):
y_hat = RD['mu_y'][t]
Y_t = Y[t][0]
var_y = 1 / RD['I_y'][t][0, 0]
#Integrate over bin size here
top = Y_t + Ystep / 2
bottom = Y_t - Ystep / 2
if var_y < (Ystep**
2) * 1e-4: # small var_y. Forecast either in or out
if bottom < y_hat and y_hat < top:
like0 = 1.0
else:
like0 = 0.0
else:
dev = np.sqrt(var_y * 2.0) #2.0 for erf definition
z0 = (bottom - y_hat) / dev
z1 = (top - y_hat) / dev
like0 = (ERF(z1) - ERF(z0)) / 2
# Use gaussian mixture forecast. The safety component has
# dev=20 and weight a = 1e-3
dev = 20.0
z0 = (Y[t][0] - Ystep / 2 - y_hat) / dev
z1 = (Y[t][0] + Ystep / 2 - y_hat) / dev
like1 = (ERF(z1) - ERF(z0)) / 2
a = 1.0e-3
like = (1 - a) * like0 + a * like1
if like > 0:
AILLY += np.log(like)
else:
print('''Error: t=%d like=%f, like0=%f, like1=%f,
y_hat=%f Y_t=%f var_y=%f
top=%f bottom=%f
dev=%f z0=%f z1=%f''' % (t, like, like0, like1, y_hat, Y_t, var_y, top, bottom,
dev, z0, z1))
AILLY /= Nt
return AILLY
break
else:
# blocking receive - if we dont have data and we already processed the last packet then we good
raw = socketToNode.recv(4096)
#load json data from node
payloadJSON = json.loads(raw)
# if its 'data'...
if payloadJSON["command"] == "ra":
#print(payloadJSON)
writeData(raw)
# only count on good data
counter += 1
# update our idea of where we are and whats around us
globalState = EKF.processData(payloadJSON, globalState)
# use updated state to figure out what to do
command = flightPlanner.planFlight(globalState, counter)
x.append(globalState.x)
y.append(globalState.y)
if counter%60 == 0:
startx.append(globalState.x)
starty.append(globalState.y)
dirx.append(math.cos(globalState.yaw)*0.02)
diry.append(math.sin(globalState.yaw)*0.02)
p = depthToPoint(globalState, 0, globalState.depths[0])
lidar1x.append(p[0])
lidar1y.append(p[1])
#print("dt: "+str(globalState.dt))
# p = depthToPoint(globalState, 1, globalState.depths[1])