class TestUKF(unittest.TestCase):
def setUp(self):
def fx(x, T): return x
def hx(x): return x
x0 = np.array([0, 0])
P0 = np.array([[1, 0.5], [0.5, 1]])
Q = np.eye(2)
R = np.eye(2)
self.ukf = UKF(fx, hx, Q, R, x0, P0)
def test_predict(self):
self.ukf.predict(T=1)
(x, P) = self.ukf.get_state()
x = np.round(x, decimals=5)
P = np.round(P, decimals=5)
self.assertTrue((x == np.array([0, 0])).all())
self.assertTrue((P == np.array([[2, 0.5], [0.5, 2]])).all())
def test_update(self):
self.ukf.predict(T=1)
self.ukf.update(np.array([1, 1]))
(x, P) = self.ukf.get_state()
x = np.round(x, decimals=5)
P = np.round(P, decimals=5)
self.assertTrue((x == np.array([0.71429, 0.71429])).all())
self.assertTrue((P == np.array([[0.65714, 0.05714], [0.05714, 0.65714]])).all())
def main():
np.set_printoptions(precision=3)
# Process Noise
q = np.eye(5)
q[0][0] = 0.001
q[1][1] = 0.001
q[2][2] = 0.001
q[3][3] = 0.001
q[4][4] = 0.001
# q[5][5] = 0.0025
realx = []
realy = []
realv = []
realtheta = []
realw = []
estimatex = []
estimatey = []
estimatev = []
estimatetheta = []
estimatew = []
estimate2y = []
estimate2x = []
estimatev2 = []
realyaw = []
estimateyaw = []
estimateyaw2 = []
realyawr = []
estimateyawr = []
estimateyawr2 = []
estimate3x = []
estimate3y = []
ex = []
ey = []
# create measurement noise covariance matrices
r_imu = np.zeros([1, 1])
r_imu[0][0] = 0.001
r_compass = np.zeros([1, 1])
r_compass[0][0] = 0.001
r_encoder = np.zeros([1, 1])
r_encoder[0][0] = 0.001
r_gpsx = np.zeros([1, 1])
r_gpsx[0][0] = 0.001
r_gpsy = np.zeros([1, 1])
r_gpsy[0][0] = 0.001
ini = np.array([0.000, 0.000, 0.000, 0.000, 0.000])
xest = np.array([[0.000], [0.000], [0.000], [0.000], [0.000]])
pest = np.eye(5)
jf = np.eye(5)
#ガウス分布の粒子の生成
p = 100
x_p = np.zeros((p, 5))
pw = np.zeros((p, 5))
x_p_update = np.zeros((p, 5))
z_p_update = np.zeros((p, 5))
for i in range(0, p):
x_p[i] = np.random.randn(5)
xp = np.zeros((1, 5))
# pass all the parameters into the UKF!
# number of state variables, process noise, initial state, initial coariance, three tuning paramters, and the iterate function
state_estimator = UKF(5, q, ini, np.eye(5), 0.04, 0.0, 2.0, iterate_x)
with open('example.csv', 'r') as csvfile:
reader = csv.reader(csvfile)
headers = next(reader)
last_time = 0
# read data
for row in reader:
row = [float(x) for x in row]
cur_time = row[0]
d_time = cur_time - last_time
real_state = np.array([row[i] for i in [5, 6, 3, 4, 2]])
# create an array for the data from each sensor
compass_hdg = row[9]
compass_data = np.array([compass_hdg])
encoder_vel = row[10]
encoder_data = np.array([encoder_vel])
gps_x = row[11]
gpsx_data = np.array([gps_x])
gps_y = row[12]
gpsy_data = np.array([gps_y])
imu_yaw_rate = row[8]
imu_data = np.array([imu_yaw_rate])
last_time = cur_time
observation_data = np.array([[row[11]], [row[12]], [row[10]],
[row[9]], [row[8]]])
observation_con = np.eye(5)
observation_con[0][0] = 0.001
observation_con[1][1] = 0.001
observation_con[2][2] = 0.001
observation_con[3][3] = 0.001
observation_con[4][4] = 0.001
# prediction is pretty simple
state_estimator.predict(d_time)
# updating isn't bad either
# remember that the updated states should be zero-indexed
# the states should also be in the order of the noise and data matrices
state_estimator.update([4], imu_data, r_imu)
state_estimator.update([3], compass_data, r_compass)
state_estimator.update([2], encoder_data, r_encoder)
state_estimator.update([1], gpsy_data, r_gpsy)
state_estimator.update([0], gpsx_data, r_gpsx)
#ekf
ret2 = state_estimator.motion_model(xest)
jf = state_estimator.jacobi(ret2)
xest, pest = state_estimator.ekf_estimation(
xest, pest, observation_data, observation_con, jf, ret2)
"""
#particle filter
for i in range(0,p):
x_p_update[i,:] = motion_model2(x_p[i,:])
z_p_update[i,:] = x_p_update[i,:]
pw[i] = cul_weight(observation_data,z_p_update[i,:])
sum1=0.000
sum2=0.000
sum3=0.000
sum4=0.000
sum5=0.000
for i in range(0,p):
sum1=sum1+pw[i,0]
sum2=sum1+pw[i,1]
sum3=sum1+pw[i,2]
sum4=sum1+pw[i,3]
sum5=sum1+pw[i,4]
#normalize
for i in range(0,p):
pw[i,0]=pw[i,0]/sum1
pw[i,1]=pw[i,1]/sum2
pw[i,2]=pw[i,2]/sum3
pw[i,3]=pw[i,3]/sum4
pw[i,4]=pw[i,4]/sum5
#resampling
for i in range(0,p):
u = np.random.rand(1,5)
qt1=np.zeros((1,5))
for j in range(0,p):
qt1=qt1+pw[j]
if np.all(qt1) > np.all(u):
x_p[i]=x_p_update[j]
break
xest2 =np.zeros((1,5))
for i in range(0,p):
xest2[0,0]=xest2[0,0]+x_p[i,0]
xest2[0,1]=xest2[0,1]+x_p[i,1]
xest2[0,2]=xest2[0,2]+x_p[i,2]
xest2[0,3]=xest2[0,3]+x_p[i,3]
xest2[0,4]=xest2[0,4]+x_p[i,4]
xest2[0,0] = xest2[0,0] / p
xest2[0,1] = xest2[0,1] / p
xest2[0,2] = xest2[0,2] / p
xest2[0,3] = xest2[0,3] / p
xest2[0,4] = xest2[0,4] / p
"""
print("----------------------------------------------------------")
print("Real state: ", real_state)
print("UKF Estimated state: ", state_estimator.get_state())
print("EKF Estimated state: ", xest.T)
ex.append(row[11])
ey.append(row[12])
realx.append(real_state[0])
realy.append(real_state[1])
estimatex.append(state_estimator.get_state(0))
estimatey.append(state_estimator.get_state(1))
realv.append(real_state[2])
estimatev.append(state_estimator.get_state(2))
estimate2x.append(xest[0, 0])
estimate2y.append(xest[1, 0])
estimatev2.append(xest[2, 0])
realyaw.append(real_state[3])
estimateyaw.append(state_estimator.get_state(3))
estimateyaw2.append(xest[3, 0])
realyawr.append(real_state[4])
estimateyawr.append(state_estimator.get_state(4))
estimateyawr2.append(xest[4, 0])
#figl=plt.figure(4)
#plt.subplot(411)
plt.plot(realx, realy, label="real_state")
plt.plot(estimatex, estimatey, label="ufk_estimator")
plt.plot(estimate2x, estimate2y, label="efk_estimator")
#plt.plot(ex,ey,label="estimator")
#plt.plot(estimate3x,estimate3y,label="pk_estimator")
plt.xlabel("x")
plt.ylabel("y")
plt.legend(loc="best")
#plt.xlim(0,2)
#plt.ylim(0,2)
"""
plt.subplot(412)
plt.plot(realv)
plt.plot(estimatev)
plt.plot(estimatev2)
plt.xlabel("sample")
plt.ylabel("v")
plt.subplot(413)
plt.plot(realyaw)
plt.plot(estimateyaw)
plt.plot(estimateyaw2)
plt.xlabel("sample")
plt.ylabel("yaw")
plt.subplot(414)
plt.plot(realyawr)
plt.plot(estimateyawr)
plt.plot(estimateyawr2)
plt.xlabel("sample")
plt.ylabel("yawr")
"""
sigma1 = 0
sigma2 = 0
sigma3 = 0
sigma4 = 0
sigma5 = 0
sigma6 = 0
sigma7 = 0
sigma8 = 0
sigma9 = 0
sigma10 = 0
for i in range(0, len(realx)):
sigma1 += (realx[i] - estimatex[i])**2
sigma2 += (realy[i] - estimatey[i])**2
sigma3 += (realx[i] - estimate2x[i])**2
sigma4 += (realx[i] - estimate2y[i])**2
sigma5 += (realx[i] - estimatev[i])**2
sigma6 += (realy[i] - estimatev2[i])**2
sigma7 += (realx[i] - estimateyaw[i])**2
sigma8 += (realx[i] - estimateyaw2[i])**2
sigma9 += (realx[i] - estimateyawr[i])**2
sigma10 += (realy[i] - estimateyawr2[i])**2
REME_x = math.sqrt(sigma1 / len(realx))
REME_y = math.sqrt(sigma2 / len(realx))
REME_x2 = math.sqrt(sigma3 / len(realx))
REME_y2 = math.sqrt(sigma4 / len(realx))
REME_v1 = math.sqrt(sigma5 / len(realx))
REME_v2 = math.sqrt(sigma6 / len(realx))
REME_y1 = math.sqrt(sigma7 / len(realx))
REME_y2 = math.sqrt(sigma8 / len(realx))
REME_yr1 = math.sqrt(sigma9 / len(realx))
REME_yr2 = math.sqrt(sigma10 / len(realx))
print("RMSE(ukf)_x=", REME_x, "RMSE(ekf)_x=", REME_x2)
print("RMSE(ukf)_y=", REME_y, "RMSE(ekf)_y=", REME_y2)
print("RMSE(ukf)_v=", REME_v1, "RMSE(ekf)_v=", REME_v2)
print("RMSE(ukf)_yaw=", REME_y1, "RMSE(ekf)_yaw=", REME_y2)
print("RMSE(ukf)_yawr=", REME_yr1, "RMSE(ekf)_yawr=", REME_yr2)
"""
ukf_mses = []
ekf_mses = []
lstm_mses = []
lstm_stacked_mses = []
bar = ProgressBar()
for s in bar(range(num_sims)):
#x_0 = np.random.normal(0, x_var, 1)
x_0 = np.array([0])
sim = UNGM(x_0, R, Q, x_var)
ukf = UKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, 5., first_x_0, 1)
ekf = EKF(sim.f, sim.F, sim.h, sim.H, sim.Q, sim.R, first_x_0, 1)
for t in range(T):
x, y = sim.process_next()
ukf.predict()
ukf.update(y)
ekf.predict()
ekf.update(y)
ukf_mses.append(MSE(ukf.get_all_predictions(), sim.get_all_x()))
ekf_mses.append(MSE(ekf.get_all_predictions(), sim.get_all_x()))
all_x.append(np.array(sim.get_all_x()))
all_y.append(np.array(sim.get_all_y()))
X = np.array(all_y)[:, :-1, :]
y = np.array(all_x)[:, 1:, :]
lstm10_pred = lstm10.predict(X)
lstm100_pred = lstm100.predict(X)
rnn10_pred = rnn10.predict(X)
rnn100_pred = rnn100.predict(X)
def main():
np.set_printoptions(precision=3)
# Process Noise
q = np.eye(6)
q[0][0] = 0.0001
q[1][1] = 0.0001
q[2][2] = 0.0004
q[3][3] = 0.0025
q[4][4] = 0.0025
q[5][5] = 0.0025
# create measurement noise covariance matrices
r_imu = np.zeros([2, 2])
r_imu[0][0] = 0.01
r_imu[1][1] = 0.03
r_compass = np.zeros([1, 1])
r_compass[0][0] = 0.02
r_encoder = np.zeros([1, 1])
r_encoder[0][0] = 0.001
# pass all the parameters into the UKF!
# number of state variables, process noise, initial state, initial coariance, three tuning paramters, and the iterate function
state_estimator = UKF(6, q, np.zeros(6), 0.0001*np.eye(6), 0.04, 0.0, 2.0, iterate_x)
i=0
csvfile=open('example.csv', 'r')
for reader in csvfile:
#print()
#reader = csv.reader(csvfile)
#reader=next(reader)
row=reader.split(',')
#print(reader)
last_time = 0
# read data
if i!=0:
#for row in reader:
print(row)
'''
try:
row = [float(x) for x in row]
except ValueError:
print("error")
'''
cur_time = float(row[0])
d_time = cur_time - last_time
real_state = np.array([float(row[i]) for i in [5, 6, 4, 3, 2, 1]])
# create an array for the data from each sensor
compass_hdg = float(row[9])
compass_data = np.array([compass_hdg])
encoder_vel = float(row[10])
encoder_data = np.array([encoder_vel])
imu_accel = float(row[7])
imu_yaw_rate = float(row[8])
imu_data = np.array([imu_yaw_rate, imu_accel])
last_time = cur_time
# prediction is pretty simple
state_estimator.predict(d_time)
# updating isn't bad either
# remember that the updated states should be zero-indexed
# the states should also be in the order of the noise and data matrices
state_estimator.update([4, 5], imu_data, r_imu)
state_estimator.update([2], compass_data, r_compass)
state_estimator.update([3], encoder_vel, r_encoder)
print ("--------------------------------------------------------")
print ("Real state: ", real_state)
print ("Estimated state: ", state_estimator.get_state())
print ("Difference: ", real_state - state_estimator.get_state())
i+=1
def move_wheelchair(self):
self.ini_cwo_l = 2 * np.pi * np.random.random_sample() * -np.pi
self.ini_cwo_r = 2 * np.pi * np.random.random_sample() * -np.pi
while rospy.get_time() == 0.0:
continue
rospy.sleep(1)
# self.ini_val = [self.wheel_cmd.angular.z, -self.wheel_cmd.linear.x, -self.odom_y, self.odom_x, self.odom_th, self.th_to_al(self.l_caster_angle), self.th_to_al(self.r_caster_angle)]
self.ini_val = [
0.0, 0.0, 0.0, 0.0, 0.0, self.ini_cwo_l, self.ini_cwo_r
]
# self.ini_val = np.random.uniform(low=-1.0, high=1.0, size=(7,)).tolist()
# UKF initialization
def fx(x, dt):
sol = self.ode2(x)
return np.array(sol)
def hx(x):
return np.array([x[3], x[2], normalize_angle(x[4])])
# points = MerweScaledSigmaPoints(n=7, alpha=.00001, beta=2., kappa=-4.)
points = JulierSigmaPoints(n=7, kappa=-4., sqrt_method=None)
# points = SimplexSigmaPoints(n=7)
kf = UKF(dim_x=7,
dim_z=3,
dt=self.dt,
fx=fx,
hx=hx,
points=points,
sqrt_fn=None,
x_mean_fn=self.state_mean,
z_mean_fn=self.meas_mean,
residual_x=self.residual_x,
residual_z=self.residual_z)
x0 = np.array(self.ini_val)
kf.x = x0 # initial mean state
kf.Q *= np.diag([.0001, .0001, .0001, .0001, .0001, .01, .01])
kf.P *= 0.000001 # kf.P = eye(dim_x) ; adjust covariances if necessary
kf.R *= 0.0001
count = 0
rospy.loginfo("Moving robot...")
start = rospy.get_time()
self.r.sleep()
while (rospy.get_time() - start <
self.move_time) and not rospy.is_shutdown():
self.pub_twist.publish(self.wheel_cmd)
z = np.array([self.odom_x, -self.odom_y, self.odom_th])
self.zs.append(z)
kf.predict()
kf.update(z)
self.xs.append(kf.x)
self.pose_x_data.append(self.odom_x)
self.pose_y_data.append(self.odom_y)
self.pose_th_data.append(self.odom_th)
self.l_caster_data.append(self.l_caster_angle)
self.r_caster_data.append(self.r_caster_angle)
count += 1
self.r.sleep()
# Stop the robot
self.pub_twist.publish(Twist())
self.xs = np.array(self.xs)
rospy.sleep(1)
def main():
np.set_printoptions(precision=3)
# Process Noise
q = np.eye(5)
q[0][0] = 0.001
q[1][1] = 0.001
q[2][2] = 0.004
q[3][3] = 0.025
q[4][4] = 0.025
# q[5][5] = 0.0025
realx = []
realy = []
realv = []
realtheta = []
realw = []
estimatex = []
estimatey = []
estimatev = []
estimatetheta = []
estimatew = []
estimate2y = []
estimate2x = []
# create measurement noise covariance matrices
r_imu = np.zeros([1, 1])
r_imu[0][0] = 0.01
r_compass = np.zeros([1, 1])
r_compass[0][0] = 0.02
r_encoder = np.zeros([1, 1])
r_encoder[0][0] = 0.001
r_gpsx = np.zeros([1, 1])
r_gpsx[0][0] = 0.1
r_gpsy = np.zeros([1, 1])
r_gpsy[0][0] = 0.1
ini = np.array([0, 0, 0.3, 0, 0.3])
# pass all the parameters into the UKF!
# number of state variables, process noise, initial state, initial coariance, three tuning paramters, and the iterate function
state_estimator = UKF(5, q, ini, np.eye(5), 0.04, 0.0, 2.0, iterate_x,
r_imu, r_compass, r_encoder, r_gpsx, r_gpsy)
xest = np.zeros((5, 1))
pest = np.eye(5)
jf = []
with open('example.csv', 'r') as csvfile:
reader = csv.reader(csvfile)
headers = next(reader)
last_time = 0
# read data
for row in reader:
row = [float(x) for x in row]
cur_time = row[0]
d_time = cur_time - last_time
real_state = np.array([row[i] for i in [5, 6, 3, 4, 2]])
# create an array for the data from each sensor
compass_hdg = row[9]
compass_data = np.array([compass_hdg])
encoder_vel = row[10]
encoder_data = np.array([encoder_vel])
gps_x = row[11]
gpsx_data = np.array([gps_x])
gps_y = row[12]
gpsy_data = np.array([gps_y])
imu_yaw_rate = row[8]
imu_data = np.array([imu_yaw_rate])
last_time = cur_time
observation_data = np.array(
[row[11], row[12], row[10], row[9], row[8]])
observation_datac = np.array([0.1, 0.1, 0.001, 0.02, 0.01])
# prediction is pretty simple
state_estimator.predict(d_time)
# updating isn't bad either
# remember that the updated states should be zero-indexed
# the states should also be in the order of the noise and data matrices
state_estimator.update([4], imu_data, r_imu)
state_estimator.update([3], compass_data, r_compass)
state_estimator.update([2], encoder_data, r_encoder)
state_estimator.update([1], gpsy_data, r_gpsy)
state_estimator.update([0], gpsx_data, r_gpsx)
jf = state_estimator.jacobi(xest)
xest, pest = state_estimator.ekf_estimation(
xest, pest, observation_data, observation_datac, jf)
print("--------------------------------------------------------")
print("Real state: ", real_state)
print("Estimated state: ", state_estimator.get_state())
print("Difference: ", real_state - state_estimator.get_state())
print("Estimated state2: ", xest)
realx.append(real_state[0])
realy.append(real_state[1])
estimatex.append(state_estimator.get_state(0))
estimatey.append(state_estimator.get_state(1))
realv.append(real_state[2])
estimatev.append(state_estimator.get_state(2))
estimate2x.append(xest[0])
estimate2y.append(xest[1])
figl = plt.figure(2)
plt.subplot(211)
plt.plot(estimatex, estimatey, "-b", label="ufk_estimator")
plt.plot(realx, realy, "-r", label="real_position")
plt.plot(estimate2x, estimate2y, "-g", label="efk_estimator")
plt.xlabel("x")
plt.ylabel("y")
plt.legend(loc="best")
plt.subplot(212)
plt.plot(realv, "-r", label="real_v")
plt.plot(estimatev, "-b", label="estimator_v")
plt.legend(loc="best")
def ukf_move_wheelchair(self):
while rospy.get_time() == 0.0:
continue
rospy.sleep(1)
def fx(x, dt):
x[0], x[1] = normalize_angle(x[0]), normalize_angle(x[1])
self.prev_alpha1 = x[0]
self.prev_alpha2 = x[1]
return self.caster_model_ukf(x, dt)
def hx(x):
# print "2: ", self.prev_alpha1
delta_alpha1 = x[0] - self.prev_alpha1
delta_alpha2 = x[1] - self.prev_alpha2
alpha1dot = delta_alpha1 / self.dt
alpha2dot = delta_alpha2 / self.dt
sol = self.meas_model(x[0], x[1], alpha1dot, alpha2dot)
return sol
self.ini_pose = [
self.wheel_cmd.angular.z, -self.wheel_cmd.linear.x, -self.odom_y,
self.odom_x, self.odom_th
]
self.save_pose_data.append(
[self.ini_pose[2], self.ini_pose[3], self.ini_pose[4]])
points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=1., kappa=-1.)
kf = UKF(dim_x=2,
dim_z=2,
dt=self.dt,
fx=fx,
hx=hx,
points=points,
sqrt_fn=None,
x_mean_fn=self.state_mean,
z_mean_fn=self.meas_mean,
residual_x=self.residual_x,
residual_z=self.residual_z)
# self.ini_val = [normalize_angle(self.l_caster_angle-np.pi), normalize_angle(self.r_caster_angle-np.pi)]
self.ini_val = [3.1, -3.14]
self.save_caster_data.append(self.ini_val)
kf.x = np.array(self.ini_val) # initial mean state
kf.P *= 0.0001 # kf.P = eye(dim_x) ; adjust covariances if necessary
# kf.R *= 0
# kf.Q *= 0
zs = []
xs = []
# xs.append(self.ini_val)
count = 0
# print "Est1: ", normalize_angle(kf.x[0]+np.pi), normalize_angle(kf.x[1]+np.pi)
rospy.loginfo("Moving robot...")
last_odom_x = self.odom_x
last_odom_th = self.odom_th
start = rospy.get_time()
self.r.sleep()
while (rospy.get_time() - start <
self.move_time) and not rospy.is_shutdown():
curr_odom_x, curr_odom_th = self.odom_x, self.odom_th
delta_x, delta_th = curr_odom_x - last_odom_x, curr_odom_th - last_odom_th
z = np.array([delta_x / self.dt, delta_th / self.dt])
# z = np.array([self.odom_vx, self.odom_vth])
if count % self.factor == 0:
zs.append(z)
kf.predict()
kf.update(z)
xs.append([
normalize_angle(kf.x[0] + np.pi),
normalize_angle(kf.x[1] + np.pi)
])
# print "Est: ", normalize_angle(kf.x[0]+np.pi), normalize_angle(kf.x[1]+np.pi)
# print "Act: ", normalize_angle(self.l_caster_angle), normalize_angle(self.r_caster_angle)
self.save_caster_data.append(
[self.l_caster_angle, self.r_caster_angle])
self.save_pose_data.append(
[self.odom_x, self.odom_y, self.odom_th])
# print len(self.save_caster_data)
self.pub_twist.publish(self.wheel_cmd)
count += 1
last_odom_x, last_odom_th = curr_odom_x, curr_odom_th
self.r.sleep()
# Stop the robot
self.pub_twist.publish(Twist())
self.caster_sol_ukf = np.array(xs)
self.caster_sol_act = np.array(self.save_caster_data)
self.pose_act = np.array(self.save_pose_data)
# self.pose_act = np.reshape(self.pose_act, (len(self.save_pose_data), 3))
rospy.sleep(1)
def main():
np.set_printoptions(precision=3)
# Process Noise
q = np.eye(6)
q[0][0] = 0.0001
q[1][1] = 0.0001
q[2][2] = 0.0004
q[3][3] = 0.0025
q[4][4] = 0.0025
q[5][5] = 0.0025
realx = [0]
realy = [0]
estimatex = [0]
estimatey = [0]
t = [0]
# create measurement noise covariance matrices
r_imu = np.zeros([2, 2])
r_imu[0][0] = 0.01
r_imu[1][1] = 0.03
r_compass = np.zeros([1, 1])
r_compass[0][0] = 0.02
r_encoder = np.zeros([1, 1])
r_encoder[0][0] = 0.001
# pass all the parameters into the UKF!
# number of state variables, process noise, initial state, initial coariance, three tuning paramters, and the iterate function
state_estimator = UKF(6, q, np.zeros(6), 0.0001 * np.eye(6), 0.04, 0.0,
2.0, iterate_x)
with open('example.csv', 'r') as csvfile:
reader = csv.reader(csvfile)
headers = next(reader)
last_time = 0
# read data
for row in reader:
row = [float(x) for x in row]
cur_time = row[0]
d_time = cur_time - last_time
real_state = np.array([row[i] for i in [5, 6, 4, 3, 2, 1]])
# create an array for the data from each sensor
compass_hdg = row[9]
compass_data = np.array([compass_hdg])
encoder_vel = row[10]
encoder_data = np.array([encoder_vel])
imu_accel = row[7]
imu_yaw_rate = row[8]
imu_data = np.array([imu_yaw_rate, imu_accel])
last_time = cur_time
# prediction is pretty simple
state_estimator.predict(d_time)
# updating isn't bad either
# remember that the updated states should be zero-indexed
# the states should also be in the order of the noise and data matrices
state_estimator.update([4, 5], imu_data, r_imu)
state_estimator.update([2], compass_data, r_compass)
state_estimator.update([3], encoder_vel, r_encoder)
print("--------------------------------------------------------")
print("Real state: ", real_state)
print("Estimated state: ", state_estimator.get_state())
print("Difference: ", real_state - state_estimator.get_state())
realx.append(real_state[3])
realy.append(real_state[1])
estimatex.append(state_estimator.get_state(3))
estimatey.append(state_estimator.get_state(1))
t.append(d_time)
plt.plot(estimatex, "-b")
plt.plot(realx, "-r")
err = truth - mu
err_hist.append(err)
#find which measurements to use
while l_time.item(meas_index) < odom_t.item(i):
meas_index += 1
if meas_index >= l_time.size:
break
meas_index -= 1
lm_ind = np.argwhere(np.isfinite(l_depth[:, meas_index]))
r = l_depth[lm_ind, meas_index]
phi = l_bearing[lm_ind, meas_index]
zt = np.hstack((r, phi)).T
mu, Sigma, K = ukf.update(mu, Sigma, zt, lm_ind, vel_odom[0, i],
vel_odom[1, i], dt)
covar_hist.append(np.diagonal(Sigma))
fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True)
mu_hist = np.array(mu_hist).T
odom_t = odom_t.flatten()
th_truth = unwrap(th_truth)
ax1[0].plot(t_truth, x_truth, label="Truth")
ax1[0].plot(odom_t, mu_hist[0, :], label="Est")
ax1[0].set_ylabel("x (m)")
ax1[0].legend()
ax1[1].plot(t_truth, y_truth, label="Truth")
ax1[1].plot(odom_t, mu_hist[1, :], label="Est")
ax1[1].set_ylabel("y (m)")
ax1[1].legend()
ax1[2].plot(t_truth, th_truth, label="Truth")
#stuff for plotting
x_hist.append(state)
mu_hist.append(mu)
err = state - mu
err[2] = unwrap(err[2])
err_hist.append(err)
x_covar_hist.append(Sigma[0,0])
y_covar_hist.append(Sigma[1,1])
psi_covar_hist.append(Sigma[2,2])
Car.animateCar(state, mu, dead_reckon)
plt.pause(0.02)
state = ukf.propagateState(state, v[i], w[i])
zt = getMeasurements(state)
mu, Sigma, K = ukf.update(mu, Sigma, zt, vc[i], wc[i])
dead_reckon = ukf.propagateState(dead_reckon, vc[i], wc[i])
K_hist.append(K)
fig1, ax1 = plt.subplots(nrows=3, ncols=1, sharex=True)
x_hist = np.array(x_hist).T
mu_hist = np.array(mu_hist).T
ax1[0].plot(t, x_hist[0,:], label="Truth")
ax1[0].plot(t, mu_hist[0,:], label="Est")
ax1[0].set_ylabel("x (m)")
ax1[0].legend()
ax1[1].plot(t, x_hist[1,:], label="Truth")
ax1[1].plot(t, mu_hist[1,:], label="Est")
ax1[1].set_ylabel("y (m)")
ax1[1].legend()