body_radius = 0.3
fig, lines, lines_est, msensor, robot_body, robot_head = InitPlot(
twr, body_radius)
mu = ekf.mu
K = ekf.K
two_sig_x = np.array([[2 * np.sqrt(ekf.cov.item((0, 0)))],
[-2 * np.sqrt(ekf.cov.item((0, 0)))]])
two_sig_y = np.array([[2 * np.sqrt(ekf.cov.item((1, 1)))],
[-2 * np.sqrt(ekf.cov.item((1, 1)))]])
two_sig_theta = np.array([[2 * np.sqrt(ekf.cov.item((2, 2)))],
[-2 * np.sqrt(ekf.cov.item((2, 2)))]])
for i in range(int(twr.t_end / twr.dt)):
# truth model updates
twr.Propagate()
ekf.Propogate(twr.Getu(), twr.Getz())
# plotter updates
mu = np.hstack((mu, ekf.mu))
K = np.hstack((K, ekf.K))
# zpos = np.hstack((zpos, twr.Getzpos())) # Historical sensor plotting
zpos = twr.Getzpos() # Immediate sensor plotting
UpdatePlot(fig, lines, lines_est, msensor, robot_body, body_radius,
robot_head, twr, mu, zpos)
two_sig_x = np.hstack((two_sig_x,
np.array([[2 * np.sqrt(ekf.cov.item((0, 0)))],
[-2 * np.sqrt(ekf.cov.item(
(0, 0)))]])))
two_sig_y = np.hstack((two_sig_y,
np.array([[2 * np.sqrt(ekf.cov.item((1, 1)))],
[-2 * np.sqrt(ekf.cov.item(
