def do_plot_test():
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal as mnormal
from filterpy.stats import covariance_ellipse, plot_covariance
p = np.array([[32, 15], [15., 40.]])
x, y = mnormal(mean=(0, 0), cov=p, size=5000).T
sd = 2
a, w, h = covariance_ellipse(p, sd)
print(np.degrees(a), w, h)
count = 0
color = []
for i in range(len(x)):
if _is_inside_ellipse(x[i], y[i], 0, 0, a, w, h):
color.append('b')
count += 1
else:
color.append('r')
plt.scatter(x, y, alpha=0.2, c=color)
plt.axis('equal')
plot_covariance(mean=(0., 0.),
cov=p,
std=[1, 2, 3],
alpha=0.3,
facecolor='none')
print(count / len(x))
def do_plot_test():
import matplotlib.pyplot as plt
from numpy.random import multivariate_normal as mnormal
from filterpy.stats import covariance_ellipse, plot_covariance
p = np.array([[32, 15], [15., 40.]])
x, y = mnormal(mean=(0, 0), cov=p, size=5000).T
sd = 2
a, w, h = covariance_ellipse(p, sd)
print(np.degrees(a), w, h)
count = 0
color = []
for i in range(len(x)):
if _is_inside_ellipse(x[i], y[i], 0, 0, a, w, h):
color.append('b')
count += 1
else:
color.append('r')
plt.scatter(x, y, alpha=0.2, c=color)
plt.axis('equal')
plot_covariance(mean=(0., 0.),
cov=p,
std=[1,2,3],
alpha=0.3,
facecolor='none')
print(count / len(x))
def interactive_covariance(v_a, v_b, v_ab):
plt.figure(figsize=(9, 6))
mean_a = 0
mean_b = 0
std = [1, 2, 3]
P = [[v_a, v_ab], [v_ab, v_b]]
plot_covariance((mean_a, mean_b),
P,
fc='g',
alpha=0.2,
std=std,
title='|' + str(v_a) + ' ' + str(v_ab) + "|\n|" +
str(v_ab) + ' ' + str(v_b) + "|")
def plotResults(self):
plt.clf()
plt.ion()
for d in self._data_queue:
pos_vel = d.prediction
cov_matrix = d.covariance
measure = d.measurement
if cov_matrix is None: return
cov = np.array([[cov_matrix[0, 0], cov_matrix[3, 0]],
[cov_matrix[0, 3], cov_matrix[3, 3]]])
[x_pos, x_vel, x_ac, y_pos, y_vel, y_ac] = pos_vel[:, 0]
# cov = np.array([[cov_matrix[0, 0], cov_matrix[2, 0]], [cov_matrix[0, 2], cov_matrix[2, 2]]])
# [x_pos, x_vel, y_pos, y_vel] = pos_vel[:, 0]
mean = (x_pos, y_pos)
plot_covariance(mean, cov=cov, fc='r', alpha=0.2)
if measure is not None:
plt.plot(measure[0], measure[1], '.g', lw=1, ls='--')
plt.quiver(x_pos,
y_pos,
x_vel,
y_vel,
color='b',
scale_units='xy',
scale=1,
alpha=0.8,
width=0.0035)
# # # plotting acceleration
plt.quiver(x_pos,
y_pos,
x_ac,
y_ac,
color='y',
scale_units='xy',
scale=1,
alpha=0.8,
width=0.0035)
# plt.show()
plt.xlim([-10, 10])
plt.ylim([-10, 10])
plt.pause(0.0001)
plt.show()
def plotResults(measurements, predictions, covariances):
for pos_vel, cov_matrix in zip(predictions, covariances):
# cov = np.array([[cov_matrix[1, 1], cov_matrix[4, 1]], [cov_matrix[1, 4], cov_matrix[4, 4]]]) # cov de vx con vy
cov = np.array([[cov_matrix[0, 0], cov_matrix[3, 0]],
[cov_matrix[0, 3], cov_matrix[3, 3]]]) #cov de x con y
# cov1 = np.array([[cov_matrix[0, 0], cov_matrix[0, 1]],[cov_matrix[1, 0], cov_matrix[1, 1]]]) #cov de x con vx
# cov2 = np.array([[cov_matrix[3, 3], cov_matrix[4, 3]],[cov_matrix[3, 4], cov_matrix[4, 4]]]) #cov de y con vy
[x_pos, x_vel, x_ac, y_pos, y_vel, y_ac] = pos_vel[:, 0]
mean = (x_pos, y_pos)
# Plotting elipses
plot_covariance(mean, cov=cov, fc='r', alpha=0.2)
# plotting velocity
plt.quiver(x_pos,
y_pos,
x_vel,
y_vel,
color='b',
scale_units='xy',
scale=1,
alpha=0.8,
width=0.0035)
# plotting acceleration
plt.quiver(x_pos,
y_pos,
x_ac,
y_ac,
color='y',
scale_units='xy',
scale=1,
alpha=0.8,
width=0.0035)
x = []
y = []
for measure in measurements:
if not measure: continue
x.append(measure[0])
y.append(measure[1])
plt.plot(x, y, 'og', lw=1, ls='--')
# plt.axis('equal')
plt.show()
def plotP(predictor, state, index, plotType):
'''
描绘UKF算法中误差协方差yz分量的变化过程
:param state0: 【np.array】预测状态 （7，）
:param state: 【np.array】真实状态 （7，）
:param index: 【int】算法的迭代步数
:param plotType: 【tuple】描绘位置的分量 'xy' or 'yz'
:return:
'''
x, y = plotType
state_copy = state.copy() # 浅拷贝真实值，因为后面会修改state
xtruth = state_copy[:3] # 获取坐标真实值
mtruth = q2R(state_copy[3: 7])[:, -1] # 获取姿态真实值，并转换为z方向的矢量
pos, q = predictor.ukf.x[:3].copy(), predictor.ukf.x[3: 7] # 获取预测值，浅拷贝坐标值
em = q2R(q)[:, -1]
if plotType == (0, 1):
plt.ylim(-0.2, 0.4)
plt.axis('equal') # 坐标轴按照等比例绘图
elif plotType == (1, 2):
xtruth[1] += index * 0.1
pos[1] += index * 0.1
else:
raise Exception("invalid plotType")
P = predictor.ukf.P[x: y+1, x: y+1] # 坐标的误差协方差
plot_covariance(mean=pos[x: y+1], cov=P, fc='g', alpha=0.3, title='胶囊定位过程仿真')
plt.text(pos[x], pos[y], int(index), fontsize=9)
plt.plot(xtruth[x], xtruth[y], 'ro') # 画出真实值
plt.text(xtruth[x], xtruth[y], int(index), fontsize=9)
# 添加磁矩方向箭头
scale = 0.05
plt.annotate(text='', xy=(pos[x] + em[x] * scale, pos[y] + em[y] * scale), xytext=(pos[x], pos[y]),
color="blue", weight="bold", arrowprops=dict(arrowstyle="->", connectionstyle="arc3", color="b"))
plt.annotate(text='', xy=(xtruth[x] + mtruth[x] * scale, xtruth[y] + mtruth[y] * scale),
xytext=(xtruth[x], xtruth[y]),
color="red", weight="bold", arrowprops=dict(arrowstyle="->", connectionstyle="arc3", color="r"))
# 添加坐标轴标识
plt.xlabel('{}/m'.format('xyz'[x]))
plt.ylabel('{}/m'.format('xyz'[y]))
# 添加网格线
plt.gca().grid(b=True)
# 增加固定时间间隔
plt.pause(0.05)
def run_localization(wr=2, wl=2, step=10, ellipse_step=20, ylim=None):
ekf = EKFrobot()
ekf.mu = np.array([[100, 100, .1, 0., 0.1]]).T #initialize states
# randx = np.mod(np.random.randn() * 1000, 500)
# randy = np.mod(np.random.randn() * 1000, 750)
# randtheta = np.mod(np.random.randn() * 10, np.pi)
# randw = np.mod(np.random.randn() * 10, 1)
# randb = np.mod(np.random.randn() * 10, 1)
# ekf.mu = np.array([[randx, randy, randtheta, randw, randb]]).T
ekf.P = np.diag([0.01, 0.01, 0.01, 0.01, 0.01]) #covariance matrix sigma
ekf.R = np.diag([STD_RANGE**2, STD_RANGE**2, STD_IMU**2, STD_IMU**2])
sim_pos = ekf.mu.copy()
u = np.array([wr, wl])
plt.figure()
plt.title("EKF Wheeled Robot Localization")
track = []
for i in range(200):
sim_pos = ekf.move(sim_pos, u, DT / 10.)
track.append(sim_pos)
print(sim_pos)
#NEED TO MAKE LANDMARK GENERATION!
if i % step == 0:
ekf.time_update(u=u)
if i % ellipse_step == 0:
plot_covariance((ekf.mu[0, 0], ekf.mu[1, 0]),
ekf.P[0:4, 0:4],
std=5,
facecolor='r',
alpha=0.3)
x, y = sim_pos[0, 0], sim_pos[1, 0]
front_landmark = front_landmark_generation(sim_pos, ekf.mu[2, 0])
plt.scatter(front_landmark[0], front_landmark[1], marker='s', s=20)
right_landmark = right_landmark_generation(sim_pos, ekf.mu[2, 0],
front_landmark)
plt.scatter(right_landmark[0], right_landmark[1], marker='s', s=20)
z = z_landmark(front_landmark, right_landmark, sim_pos)
if z[0] <= 120 or z[1] <= 120:
u = np.array([1, 0])
if z[0] >= 100 and z[1] >= 100:
u = np.array([1.1, 1])
ekf_update(ekf, z, front_landmark, right_landmark)
if i % ellipse_step == 0:
plot_covariance((ekf.mu[0, 0], ekf.mu[1, 0]),
ekf.P[0:4, 0:4],
std=5,
facecolor='k',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='b', lw=2)
# plt.axis('equal')
plt.title("EKF Wheeled Robot Localization")
plt.grid()
plt.show()
return ekf
def run_localization(cmds,
landmarks,
sigma_vel,
sigma_steer,
sigma_range,
sigma_bearing,
ellipse_step=1,
step=10):
plt.figure()
points = MerweScaledSigmaPoints(n=3,
alpha=.00001,
beta=2,
kappa=0,
subtract=residual_x)
ukf = UKF(dim_x=3,
dim_z=2 * len(landmarks),
fx=move,
hx=Hx,
dt=dt,
points=points,
x_mean_fn=state_mean,
z_mean_fn=z_mean,
residual_x=residual_x,
residual_z=residual_h)
ukf.x = np.append(np.mean(landmarks, axis=0),
(np.mean(cmds, axis=0)[1])) # set initial guess to mean
ukf.P = np.diag([.1, .1, .05])
ukf.R = np.diag([sigma_range**2, sigma_bearing**2] * len(landmarks))
ukf.Q = np.eye(3) * 0.0001
sim_pos = ukf.x.copy()
# plot landmarks
if len(landmarks) > 0:
plt.scatter(landmarks[:, 0], landmarks[:, 1], marker='s', s=60)
track = []
for i, u in enumerate(cmds):
sim_pos = move(sim_pos, dt / step, u, wheelbase)
print('Sim position:', sim_pos)
track.append(sim_pos)
if i % step == 0:
ukf.predict(u=u, wheelbase=wheelbase)
if i % ellipse_step == 0:
plot_covariance((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='k',
alpha=0.3)
x, y = sim_pos[0], sim_pos[1]
z = []
for lmark in landmarks:
dx, dy = lmark[0] - x, lmark[1] - y
d = math.sqrt(dx**2 + dy**2) + np.random.randn() * sigma_range
bearing = math.atan2(lmark[1] - y, lmark[0] - x)
a = (normalize_angle(bearing - sim_pos[2] +
np.random.randn() * sigma_bearing))
z.extend([d, a])
ukf.update(z, landmarks=landmarks)
if i % ellipse_step == 0:
plot_covariance((ukf.x[0], ukf.x[1]),
ukf.P[0:2, 0:2],
std=6,
facecolor='g',
alpha=0.8)
track = np.array(track)
plt.plot(track[:, 0], track[:, 1], color='k', lw=2)
plt.title("UKF Robot localization")
plt.xlim(0, 4.2)
plt.ylim(0, 6.05)
plt.savefig('kalman_filter/%d.png' % len(os.listdir('kalman_filter/')))
plt.close()
return ukf
kf.F = np.array([[1., dt], [0., 1.]]) # state transition matrix
kf.H = np.array([[1., 0]]) # Measurement function
kf.R *= std**2 # measurement uncertainty
kf.P *= 1 # covariance matrix
kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.5)
# run the kalman filter and store the results
filtered_xs, cov = [], []
for index, z in enumerate(measurements_zs):
kf.predict()
kf.update(z)
filtered_xs.append(kf.x)
cov.append(kf.P)
if index % 5 == 0:
plot_covariance(kf.x, kf.P, edgecolor='r')
plt.show()
filtered_xs = np.array(filtered_xs)
filtered_xs.shape = (count, 2)
ts = ts[burn_in:]
clean_xs = clean_xs[burn_in:]
measurements_zs = measurements_zs[burn_in:]
filtered_xs = filtered_xs[burn_in:]
plot(ts, clean_xs, label='Actual')
plot(ts, measurements_zs, 'o', label='Measured')
plot(ts, filtered_xs[:, 0], 'o', label='Filtered')
plt.ylabel('position')
plt.legend()
def Universe(robot, inc_time, stop_time, Change_Speed):
cur_state = robot.cur_state
motor_spd = robot.motor_spd
rr = robot.rr
# spd = np.array((np.pi/2, np.pi/2))
length = 750
width = 500
overall_refresh = rr / inc_time
x_points = []
y_points = []
x_old, y_old = [], []
x_new, y_new = [], []
x_msr, y_msr = [], []
# the control step controls the frequency of plotted points of robot's state.
ctrl_steps = 20
plt.figure(figsize=(20, 15))
full_time = int(stop_time//inc_time)
# the speed of two wheels.
spd = np.array((np.pi/2, np.pi/2))
for i in range(full_time):
if Change_Speed:
if i >= 100:
spd = np.array([1.5, 1])
if i >= 150:
spd = np.array([1, 1.5])
if i >= 200:
spd = np.array([1, 1])
if i >= 300:
spd = np.array([2, 1])
if i >= 400:
spd = np.array([1, 2])
if i >= 450:
spd = np.array([1, 1.2])
real_spd = spd + motor_spd * np.random.randn(2)
cur_state = robot.new_state(cur_state, real_spd, inc_time)
cur_x = cur_state[0]
cur_y = cur_state[1]
x_points.append(cur_x)
y_points.append(cur_y)
# Add noise to current state and update the state of robot with incremental steps defined by overall_refresh.
if i % overall_refresh == 0:
# plot the old state info if iter number reaches ctrl_steps value.
if i % ctrl_steps == 0:
plot_covariance((robot.cur_state[0], robot.cur_state[1]), robot.P[0:2, 0:2], std=6, edgecolor='r', facecolor='r', alpha=0.4)
robot.pred(spd)
x_old.append(robot.cur_state[0])
y_old.append(robot.cur_state[1])
# add noise based on the current state.
cur_state_noise = robot.add_noise(cur_state)
# add noise to the current state and take it as the measurement.
x_msr.append(cur_state_noise[0])
y_msr.append(cur_state_noise[1])
# use current state after adding noise to calculate the residual.
res = robot.res(cur_state_noise)
# use the residual to update robot movement and state info.
robot.update(res)
# get new state info
x_new.append(robot.cur_state[0])
y_new.append(robot.cur_state[1])
# plot the new state info
if i % ctrl_steps == 0:
plot_covariance((robot.cur_state[0], robot.cur_state[1]), robot.P[0:2, 0:2], std=6, edgecolor='b', facecolor='b', alpha=1)
plt.xlim((0, length))
plt.ylim((0, width))
plt.plot(x_points, y_points, 'ko', linewidth=0.5, markersize=1)
plt.xlabel('Length of Environment')
plt.ylabel('Width of Environment')
plt.title('EKF based two wheeled robot moving trajectory')
plt.show()
