def integrate(p0, R0, pdot0, om0, t_out):
'''
integrate Newton-Euler equations, then integrate kinematics
t_out = times at which to return states
'''
# setup
t0 = t_out[0]
tf_out = t_out[-1]
n_t_out = t_out.size
# integration times
dt = .001
t = np.arange(t0, tf_out + dt,
dt) # tf_out+dt: make sure tf_out is included
tf = t[-1]
n_t = t.size
# integrate newton-euler (state is pdot & om)
pdot_om0 = np.concatenate((pdot0, om0))
sol = sp.integrate.solve_ivp(newton_euler, (t0, tf),
pdot_om0,
method='RK45',
t_eval=t)
pdot_om = sol.y
pdot = pdot_om[:3, :]
om = pdot_om[3:, :]
# 1st-order forward Euler integration of kinematics to get p and R
p = np.full((3, n_t), np.nan)
R = np.full((3, 3, n_t), np.nan)
p[:, 0] = p0
R[:, :, 0] = R0
for i in range(1, n_t):
p[:, i] = p[:, i - 1] + dt * pdot[:, i - 1]
R[:, :, i] = R[:, :, i - 1] @ so3.exp(so3.cross(dt * om[:, i - 1]))
# get tangent space representation of R so we can interpolate
s = np.full((3, n_t), np.nan)
for i in range(n_t):
s[:, i] = so3.skew_elements(so3.log(R[:, :, i]))
# interpolate t values using state x=(p,s,v,om)
x = np.concatenate((p, s, pdot, om), axis=0)
interp_fun = sp.interpolate.interp1d(t, x)
x_out = interp_fun(t_out)
# get x values
p_out = x_out[:3, :]
s_out = x_out[3:6, :]
pdot_out = x_out[6:9, :]
om_out = x_out[9:, :]
# convert tangent space s back into rotations
R_out = np.full((3, 3, n_t_out), np.nan)
for i in range(n_t_out):
R_out[:, :, i] = so3.exp(so3.cross(s_out[:, i]))
return p_out, R_out, pdot_out, om_out
def conversion_and_error(t, p, R, pdot, om, p_filt, R_filt, pdot_filt, om_filt,
p_meas, R_meas, COV_XX_ALL, save_dir):
# determine n_ims
n_ims = p.shape[1]
# translation: convert to cm
p *= conv.m_to_cm
p_meas *= conv.m_to_cm
p_filt *= conv.m_to_cm
pdot *= conv.m_to_cm
pdot_filt *= conv.m_to_cm
# translation: errors
p_err_filt = np.linalg.norm(p_filt - p, axis=0)
p_err_meas = np.linalg.norm(p_meas - p, axis=0)
# rotation: errors
R_err_filt = np.full(n_ims, np.nan)
R_err_meas = np.full(n_ims, np.nan)
s_err_filt = np.full((3,n_ims), np.nan)
for j in range(n_ims):
R_err_filt[j] = so3.geodesic_distance(R[:,:,j], R_filt[:,:,j])
s_err_filt[:,j] = so3.skew_elements(so3.log(R[:,:,j].T @ R_filt[:,:,j]))
R_err_meas[j] = so3.geodesic_distance(R[:,:,j], R_meas[:,:,j])
# rotation: convert to degrees
R_err_filt *= conv.rad_to_deg
R_err_meas *= conv.rad_to_deg
s_err_filt *= conv.rad_to_deg
om *= conv.rad_to_deg
om_filt *= conv.rad_to_deg
# translation and rotation: convert covariance
p_unit_vec = np.tile(conv.m_to_cm, 6)
R_unit_vec = np.tile(conv.rad_to_deg, 6)
unit_vec = np.block([p_unit_vec, R_unit_vec])
UNIT_MAT = np.diag(unit_vec)
COV_XX_ALL_OLD = np.copy(COV_XX_ALL)
for i in range(n_ims):
COV_XX_ALL[:,:,i] = UNIT_MAT @ COV_XX_ALL[:,:,i] @ UNIT_MAT
# save to file
filter_results_npz = os.path.join(save_dir, 'filter_results.npz')
np.savez(filter_results_npz, t=t,
p=p, R=R, pdot=pdot, om=om,
p_meas=p_meas, R_meas=R_meas,
p_err_meas=p_err_meas, R_err_meas=R_err_meas,
p_filt=p_filt, R_filt=R_filt,
pdot_filt=pdot_filt, om_filt=om_filt,
p_err_filt=p_err_filt, R_err_filt=R_err_filt,
s_err_filt=s_err_filt, COV_XX_ALL=COV_XX_ALL)
def filter(p0_hat, R0_hat, pdot0_hat, om0_hat, COV_xx_0_hat, COV_ww, COV_vv, U,
P_MEAS, R_MEAS, dt):
'''
Unscented Filter from Section 3.7 of Optimal Estimation of Dynamic Systems
(2nd ed.) by Crassidis & Junkins
'''
# system constants
alpha = 1e-2
beta = 2
kappa = 2
n = 12
n_w = COV_ww.shape[0]
n_v = COV_vv.shape[0]
n_t = P_MEAS.shape[1] # number of time points
n_y = 6
# constants
L = n + n_w + n_v # size of augmented state
lam = (alpha**2) * (L + kappa) - L # eq. 3.256
gam = np.sqrt(L + lam) # eq. 3.255
# weights
W_mean = np.full(2 * L + 1, np.nan)
W_cov = np.full(2 * L + 1, np.nan)
W_mean[0] = lam / (L + lam)
W_cov[0] = W_mean[0] + (1 - alpha**2 + beta)
W_mean[1:] = 1 / (2 * (L + lam))
W_cov[1:] = 1 / (2 * (L + lam))
# create arrays for saving values in the for loop
P_HAT = np.full((3, n_t), np.nan)
R_HAT = np.full((3, 3, n_t), np.nan)
PDOT_HAT = np.full((3, n_t), np.nan)
OM_HAT = np.full((3, n_t), np.nan)
COV_XX_ALL = np.full((n, n, n_t), np.nan)
Y_HAT = np.full((n_y, 2 * L + 1), np.nan) # y_hat^(i) for i=0,...,2L
# fill arraysw with initial estimates
P_HAT[:, 0] = p0_hat
R_HAT[:, :, 0] = R0_hat
PDOT_HAT[:, 0] = pdot0_hat
OM_HAT[:, 0] = om0_hat
s0_hat = np.array([0, 0, 0])
# initialize variables to be used in for loop
x_hat = np.block([p0_hat, s0_hat, pdot0_hat, om0_hat])
x_hat = x_hat[:, np.newaxis]
COV_xx = COV_xx_0_hat
R_hat = R0_hat
# iterate over all time points
for i in range(1, n_t):
# load control and measurement at time i
u = U[:, i]
s_meas = so3.skew_elements(so3.log(R_hat.T @ R_MEAS[:, :, i])) # eq. 5
y = np.block([P_MEAS[:, i], s_meas])
y = y[:, np.newaxis]
# cross-correlations (usually zero, see Crassidis after eq. 3.252)
COV_xw = np.full((n, n_w), 0.0)
COV_xv = np.full((n, n_v), 0.0)
COV_wv = np.full((n_w, n_v), 0.0)
# sigma points
COV_chi = np.block([[COV_xx, COV_xw, COV_xv],
[COV_xw.T, COV_ww, COV_wv],
[COV_xv.T, COV_wv.T, COV_vv]])
M = gam * np.linalg.cholesky(
COV_chi) # returns L such that COV_chi = L @ L.T
sig = np.block([M, -M])
chi_hat_pls = np.block([[x_hat], [np.zeros((n_w + n_v, 1))]])
CHI_HAT = np.block([chi_hat_pls, chi_hat_pls + sig
]) # all chi_hat^(i) for i=0,...,2L
X_HAT = CHI_HAT[:n, :] # all x_hat^(i) for i=0,...,2L
W_HAT = CHI_HAT[n:n + n_w, :] # all w_hat^(i) for i=0,...,2L
V_HAT = CHI_HAT[n + n_w:, :] # all v_hat^(i) for i=0,...,2L
# iteration (over all sigma points)
for k in range(2 * L + 1):
# dynamics
om_k = X_HAT[9:, k]
R_k = R_hat @ so3.exp(so3.cross(X_HAT[3:6, k]))
p_new, R_new, v_new, om_new = f(X_HAT[:3, k], R_k, X_HAT[6:9, k],
om_k, W_HAT[:, k], u, dt)
s_new = so3.skew_elements(so3.log(R_hat.T @ R_new))
X_HAT[:, k] = np.block([p_new, s_new, v_new, om_new])
# measurement
p_meas, R_meas = h(p_new, R_new, u, V_HAT[:, k])
s_meas = so3.skew_elements(so3.log(R_hat.T @ R_meas))
Y_HAT[:, k] = np.block([p_meas, s_meas])
# predictions
x_hat = np.sum(W_mean * X_HAT, axis=1)
x_hat = x_hat[:, np.newaxis]
COV_xx = W_cov * (X_HAT - x_hat) @ (X_HAT - x_hat).T # COV_xx minus
y_hat = np.sum(W_mean * Y_HAT, axis=1) # eq. 2.262
y_hat = y_hat[:, np.newaxis]
# covariances
COV_yy = W_cov * (Y_HAT - y_hat) @ (Y_HAT - y_hat).T
COV_xy = W_cov * (X_HAT - x_hat) @ (Y_HAT - y_hat).T
# update
e = y - y_hat # eq. 3.250
K = COV_xy @ np.linalg.inv(COV_yy) # eq. 3.251
x_hat = x_hat + K @ e # eq. 3.249a
COV_xx = COV_xx - K @ COV_yy @ K.T # eq. 3.249b
# rotation
R_hat = R_hat @ so3.exp(so3.cross(x_hat[3:6].ravel()))
x_hat[3:6] = 0
# save values
P_HAT[:, i] = x_hat[:3].ravel()
R_HAT[:, :, i] = R_hat
PDOT_HAT[:, i] = x_hat[6:9].ravel()
OM_HAT[:, i] = x_hat[9:].ravel()
COV_XX_ALL[:, :, i] = COV_xx
return P_HAT, R_HAT, PDOT_HAT, OM_HAT, COV_XX_ALL
def plot_velocities():
# load results from file
dat = np.load(filter_results_npz)
t = dat['t']
pdot = dat['pdot']
pdot_filt = dat['pdot_filt']
om = dat['om']
om_filt = dat['om_filt']
# errors over time
pdot_err = np.abs(pdot_filt - pdot)
om_err = np.abs(om_filt - om)
# setup
pp.figure()
pp.subplots_adjust(left=None, bottom=None, right=None, top=None,
wspace=None, hspace=.65)
# velocity
sp1 = pp.subplot(2,1,1)
sp1.set_title('translational velocity errors', fontsize=title_font_size)
sp1.plot(t, pdot_err[0,:], 'y', label='$| \hat{\dot{p}}_1 - \dot{p}_1 |$')
sp1.plot(t, pdot_err[1,:], 'm', label='$| \hat{\dot{p}}_2 - \dot{p}_2 |$')
sp1.plot(t, pdot_err[2,:], 'c', label='$| \hat{\dot{p}}_3 - \dot{p}_3 |$')
# labels
pp.xlabel('time [s]', fontsize=xlabel_font_size)
pp.ylabel('[cm/s]', fontsize=xlabel_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
pp.legend(fontsize=legend_font_size)
# angular velocity
sp2 = pp.subplot(2,1,2)
sp2.set_title('rotational velocity errors', fontsize=title_font_size)
sp2.plot(t, om_err[0,:], 'y', label='$| \hat{\omega}_1 - \omega_1 |$')
sp2.plot(t, om_err[1,:], 'm', label='$| \hat{\omega}_2 - \omega_2 |$')
sp2.plot(t, om_err[2,:], 'c', label='$| \hat{\omega}_3 - \omega_3 |$')
# labels
pp.ylim([-5, 80])
pp.xlabel('time [s]', fontsize=xlabel_font_size)
pp.ylabel('[\\textdegree/s]', fontsize=xlabel_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
pp.legend(fontsize=legend_font_size, loc='upper right')
# plot network's estimates of velocities based on a finite difference
plot_fd = 0
if plot_fd:
dt = t[1]-t[0]
pdot = dat['pdot']
p_meas = dat['p_meas']
R = dat['R']
R_meas = dat['R_meas']
t_fd = t[1:]
n_fd = t_fd.size
n_t = t.size
p_meas_min1 = np.roll(p_meas, 1, axis=1)
pdot_meas = (p_meas - p_meas_min1)/dt
pdot_meas_err = np.abs(pdot - pdot_meas)
pdot_meas_err = pdot_meas_err[:,1:]
sp1.plot(t_fd, pdot_meas_err[0,:], 'y:')
sp1.plot(t_fd, pdot_meas_err[1,:], 'm:')
sp1.plot(t_fd, pdot_meas_err[2,:], 'c:')
print(np.min(pdot_meas_err))
import net_filter.tools.so3 as so3
import net_filter.tools.unit_conversion as conv
dist = np.full(n_fd, np.nan)
s = np.full((3,n_t), np.nan)
s_meas = np.full((3,n_t), np.nan)
for i in range(n_t):
s_meas[:,i] = so3.skew_elements(so3.log(R_meas[:,:,i]))
s_meas_min1 = np.roll(s_meas, 1, axis=1)
om_meas = (s_meas - s_meas_min1)/dt
om_meas_err = np.abs(om - om_meas)
om_meas_err *= conv.rad_to_deg
om_meas_err = om_meas_err[:,1:]
import pdb; pdb.set_trace()
sp2.plot(t_fd, om_meas_err[0,:], 'y:')
sp2.plot(t_fd, om_meas_err[1,:], 'm:')
sp2.plot(t_fd, om_meas_err[2,:], 'c:')
# save
file_name = os.path.join(dirs.paper_figs_dir, 'simulation_velocities.png')
pp.savefig(file_name, dpi=300)
def plot_translation_and_rotation():
# load x data
data_dope = np.load(os.path.join(dirs.trans_x_dir, 'dope_pR.npz'))
p_dope_x = data_dope['p']
with open(os.path.join(dirs.trans_x_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
p_true_x = data_true.pos
# load y data
data_dope = np.load(os.path.join(dirs.trans_y_dir, 'dope_pR.npz'))
p_dope_y = data_dope['p']
with open(os.path.join(dirs.trans_y_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
p_true_y = data_true.pos
# load z data
data_dope = np.load(os.path.join(dirs.trans_z_dir, 'dope_pR.npz'))
p_dope_z = data_dope['p']
with open(os.path.join(dirs.trans_z_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
p_true_z = data_true.pos
# load x rotation data
data_dope = np.load(os.path.join(dirs.rot_x_dir, 'dope_pR.npz'))
R_dope_x = data_dope['R']
with open(os.path.join(dirs.rot_x_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
R_true_x = data_true.rot_mat
# load y rotation data
data_dope = np.load(os.path.join(dirs.rot_y_dir, 'dope_pR.npz'))
R_dope_y = data_dope['R']
with open(os.path.join(dirs.rot_y_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
R_true_y = data_true.rot_mat
# load z rotation data
data_dope = np.load(os.path.join(dirs.rot_z_dir, 'dope_pR.npz'))
R_dope_z = data_dope['R']
with open(os.path.join(dirs.rot_z_dir, 'to_render.pkl'), 'rb') as file:
data_true = pickle.load(file)
R_true_z = data_true.rot_mat
# convert position to cm
p_dope_x *= conv.m_to_cm
p_dope_y *= conv.m_to_cm
p_dope_z *= conv.m_to_cm
p_true_x *= conv.m_to_cm
p_true_y *= conv.m_to_cm
p_true_z *= conv.m_to_cm
# convert rotation to tangent space representation
n_ims = p_dope_x.shape[1]
s_x = np.full((3, n_ims), np.nan)
s_y = np.full((3, n_ims), np.nan)
s_z = np.full((3, n_ims), np.nan)
s_offset_x = np.full((3, n_ims), np.nan)
s_offset_y = np.full((3, n_ims), np.nan)
s_offset_z = np.full((3, n_ims), np.nan)
for i in range(n_ims):
# x
R_i = R_true_x[:, :, i]
R_est_i = R_dope_x[:, :, i]
R_offset_i = R_i.T @ R_est_i
s_x[:, i] = so3.skew_elements(so3.log(R_i @ R_upright.T))
s_offset_x[:, i] = so3.skew_elements(so3.log(R_offset_i))
# y
R_i = R_true_y[:, :, i]
R_est_i = R_dope_y[:, :, i]
R_offset_i = R_i.T @ R_est_i
s_y[:, i] = so3.skew_elements(so3.log(R_i @ R_upright.T))
s_offset_y[:, i] = so3.skew_elements(so3.log(R_offset_i))
# z
R_i = R_true_z[:, :, i]
R_est_i = R_dope_z[:, :, i]
R_offset_i = R_i.T @ R_est_i
s_z[:, i] = so3.skew_elements(so3.log(R_i @ R_upright.T))
s_offset_z[:, i] = so3.skew_elements(so3.log(R_offset_i))
# convert rotation to degrees
s_x *= conv.rad_to_deg
s_y *= conv.rad_to_deg
s_z *= conv.rad_to_deg
s_offset_x *= conv.rad_to_deg
s_offset_y *= conv.rad_to_deg
s_offset_z *= conv.rad_to_deg
# plot settings
pp.rc('text', usetex=True)
pp.rc('text.latex',
preamble=
r'\usepackage{amsmath} \usepackage{amsfonts} \usepackage{textcomp}')
x_font_size = 18
tick_font_size = 14
# make figure
fig = pp.figure(figsize=(7, 10))
pp.subplots_adjust(left=.15,
bottom=.10,
right=.95,
top=.95,
wspace=None,
hspace=0.9)
# x
ax1 = pp.subplot(6, 1, 1)
pp.ylim(-1.0, 1.0)
pp.plot(p_true_x[0, :], p_dope_x[0, :] - p_true_x[0, :])
pp.xlabel('$x$ [cm]', fontsize=x_font_size)
pp.ylabel('$\\hat{x} - x$ [cm]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax1.axhline(0, color='gray', linewidth=0.5)
# y
ax2 = pp.subplot(6, 1, 2)
pp.ylim(-35.0, 35.0)
pp.plot(p_true_y[1, :], p_dope_y[1, :] - p_true_y[1, :])
pp.xlabel('$y$ [cm]', fontsize=x_font_size)
pp.ylabel('$\\hat{y} - y$ [cm]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax2.axhline(0, color='gray', linewidth=0.5)
# z
ax3 = pp.subplot(6, 1, 3)
pp.ylim(-1.0, 1.0)
pp.plot(p_true_z[2, :], p_dope_z[2, :] - p_true_z[2, :])
pp.xlabel('$z$ [cm]', fontsize=x_font_size)
pp.ylabel('$\\hat{z} - z$ [cm]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax3.axhline(0, color='gray', linewidth=0.5)
# s_1
ax4 = pp.subplot(6, 1, 4)
pp.plot(s_x[0, :], s_offset_x[0, :], 'r')
pp.ylim(-3.4, 3.4)
pp.xlabel('$s_1$ [\\textdegree]', fontsize=x_font_size)
pp.ylabel('$\\hat{s}_1 - s_1$ [\\textdegree]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax4.axhline(0, color='gray', linewidth=0.5)
# s_2
ax5 = pp.subplot(6, 1, 5)
pp.plot(s_y[1, :], s_offset_y[1, :], 'r')
pp.ylim(-5.7, 5.7)
pp.xlabel('$s_2$ [\\textdegree]', fontsize=x_font_size)
pp.ylabel('$\\hat{s}_2 - s_2$ [\\textdegree]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax5.axhline(0, color='gray', linewidth=0.5)
# s_3
ax6 = pp.subplot(6, 1, 6)
pp.plot(s_z[2, :], s_offset_z[2, :], 'r')
pp.ylim(-2.3, 2.3)
pp.xlabel('$s_3$ [\\textdegree]', fontsize=x_font_size)
pp.ylabel('$\\hat{s}_3 - s_3$ [\\textdegree]', fontsize=x_font_size)
pp.xticks(fontsize=tick_font_size)
pp.yticks(fontsize=tick_font_size)
ax6.axhline(0, color='gray', linewidth=0.5)
# save plot
pp.savefig(os.path.join(dirs.paper_figs_dir, 'trans_rot.png'), dpi=300)
pp.show()