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 gen_and_eval(mode):
# to be filled
p = np.full((3, n_ims), np.nan)
R = np.full((3, 3, n_ims), np.nan)
if mode == 'trans_x':
x = np.linspace(-.2, .2, n_ims)
for i in range(n_ims):
R[:, :, i] = R_upright
p[:, i] = np.array([x[i], .8, 0])
img_dir = dirs.trans_x_dir
elif mode == 'trans_y':
y = np.linspace(.4, 1.8, n_ims)
for i in range(n_ims):
R[:, :, i] = R_upright
p[:, i] = np.array([0, y[i], 0])
img_dir = dirs.trans_y_dir
elif mode == 'trans_z':
z = np.linspace(-.15, .15, n_ims)
for i in range(n_ims):
R[:, :, i] = R_upright
p[:, i] = np.array([0, .8, z[i]])
img_dir = dirs.trans_z_dir
elif mode == 'rot_x':
# rotate eps is needed to avoided wrapping of the exp/log maps
ang = np.linspace(-np.pi + rot_eps, np.pi - rot_eps, n_ims)
for i in range(n_ims):
s_i = np.array([ang[i], 0, 0])
R[:, :, i] = so3.exp(so3.cross(s_i)) @ R_upright
p[:, i] = np.array([0, .8, 0])
img_dir = dirs.rot_x_dir
elif mode == 'rot_y':
ang = np.linspace(-np.pi + rot_eps, np.pi - rot_eps, n_ims)
for i in range(n_ims):
s_i = np.array([0, ang[i], 0])
R[:, :, i] = so3.exp(so3.cross(s_i)) @ R_upright
p[:, i] = np.array([0, .8, 0])
img_dir = dirs.rot_y_dir
elif mode == 'rot_z':
ang = np.linspace(-np.pi + rot_eps, np.pi - rot_eps, n_ims)
for i in range(n_ims):
s_i = np.array([0, 0, ang[i]])
R[:, :, i] = so3.exp(so3.cross(s_i)) @ R_upright
p[:, i] = np.array([0, .8, 0])
img_dir = dirs.rot_z_dir
# render and predict
dt = 1 # this doesn't matter for this simulation
br.soup_gen(dt, p, R, img_dir, lighting_energy, world_RGB)
p, R, p_est, R_est = db.get_predictions(img_dir, print_errors=False)
def h(p_prev, R_prev, u, v):
'''
measurement function
'''
p_new = p_prev + v[:3]
R_noise = so3.exp(so3.cross(v[3:]))
R_new = R_prev @ R_noise
return p_new, R_new
def newton_euler(t, pdot_om):
'''
Newton-Euler equations for a rigid body
assumes velocity (pdot_A) is in inertial frame (A) coordinates,
and omega (om_B) is in body-frame (B) coordinates
ref: eq. 4.16 (p. 167) in A Mathematical Introduction to Robotic
Manipulation
'''
pdot_A = pdot_om[:3]
om_B = pdot_om[3:]
om_col_B = om_B[:, np.newaxis] # omega as column vector
# translational dynamics: all expressed in inertial frame A
F_A = np.array([[0, 0, -m * g]]).T # inertial frame force of gravity
pdot_dot_A = np.linalg.inv(m * np.eye(3)) @ F_A
# rotational dynamics: all expressed in body frame B
tau_B = np.array([[0, 0, 0]]).T # inertial frame torque
omdot_B = np.linalg.inv(J) @ (tau_B - so3.cross(om_B) @ J @ om_col_B)
pdot_omdot = np.concatenate((pdot_dot_A, omdot_B)).ravel()
return pdot_omdot
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
R0 = t3d.quaternions.quat2mat(q0)
pdot0 = np.array([0.9, 1.1, 2.3])
om0 = np.array([5, 8, 4])
# rigid body dynamics
p, R, pdot, om = rb.integrate(p0, R0, pdot0, om0, t)
save_dir = os.path.join(dirs.results_dir, 'filter_test/')
# simulate measurements
noise_p = .04
noise_R = .17
p_meas = p + noise_p*np.random.uniform(-1,1,(3,n_ims))
R_meas = np.full((3,3,n_ims), np.nan)
for i in range(n_ims):
s_noise = noise_R*np.random.uniform(.5,-.5,3)
R_noise = so3.exp(so3.cross(s_noise))
R_meas[:,:,i] = R_noise @ R[:,:,i]
# filter initial estimates
p0_hat = p_meas[:,0] # use the true value to make it a fair comparison
R0_hat = R_meas[:,:,0] # use the true value to make it a fair comparison
pdot0_hat = pdot0 + .2*np.array([-1.1, 1.2, 1.1])
om0_hat = om0 + .6*np.array([-1.0, 1.1, 1.0])
cov_xx_0_hat = np.ones(12)
COV_xx_0_hat = np.diag(cov_xx_0_hat)
# run filter
U = np.full((1, n_ims), 0.0) # there is no control in this problem
COV_ww, COV_vv = df.get_noise_covariances()
p_filt, R_filt, pdot_filt, om_filt, COV_XX_ALL = uf.filter(p0_hat, R0_hat, pdot0_hat, om0_hat, COV_xx_0_hat, COV_ww, COV_vv, U, p_meas, R_meas, dt)