def quaternion_right_prod(theta):
### normalize the angle first
theta = angle_normalize(theta)
### construct quaternion based on theta info
theta_norm = np.sqrt(theta[0] ** 2 + theta[1] ** 2 + theta[2] ** 2)
q_w = np.sin(theta_norm / 2)
q_v = theta / theta_norm * np.cos(theta_norm / 2)
Omega = np.zeros([4, 4])
Omega[0, 1:4] = -q_v.T
Omega[1:4, 0] = q_v
Omega[1:4, 1:4] = -skew_operator(q_v)
Omega = Omega + np.identity(4) * q_w
return Omega
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check):
# 3.1 Compute Kalman Gain
R = sensor_var*np.eye(3)
K = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + R)
# 3.2 Compute error state
delta_x = K @ (y_k - p_check)
# 3.3 Correct predicted state
p_hat = p_check + delta_x[:3]
v_hat = v_check + delta_x[3:6]
delta_phi = angle_normalize(delta_x[6:])
q_hat = Quaternion(euler=delta_phi).quat_mult_left(q_check)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - K @ h_jac) @ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check,
q_check):
# 3.1 Compute Kalman Gain
R = np.zeros([3, 3])
R = [[sensor_var, 0, 0], [0, sensor_var, 0], [0, 0, sensor_var]]
K = np.array([9, 3])
K = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T +
R)
# 3.2 Compute error state
delta_x = np.zeros(9)
delta_x = K @ (y_k - p_check)
# 3.3 Correct predicted state
p_hat = p_check + delta_x[0:3]
v_hat = v_check + delta_x[3:6]
delta_q = Quaternion(axis_angle=angle_normalize(delta_x[6:9]))
q_hat = Quaternion(*q_check).quat_mult_right(delta_q)
# 3.4 Compute corrected covariance
p_cov_hat = (np.identity(9) - K @ h_jac) @ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_chec, y_k, p_chec, v_chec, q_chec):
# 3.1 Compute Kalman Gain
r_km = np.eye(3) * sensor_var
kinv = np.linalg.inv((H_km.dot(p_cov_chec)).dot(H_km.T) + r_km)
k_k = (p_cov_chec.dot(H_km.T)).dot(kinv)
# 3.2 Compute error state
# print(p_chec)
delx = k_k.dot(y_k - p_chec)
# print(y_k)
# print(delx)
# 3.3 Correct predicted state
p_hat = p_chec + delx[0:3]
v_hat = v_chec + delx[3:6]
phi = (delx[6:9]).T
phi = phi.flatten()
q_error = Quaternion(euler=angle_normalize(phi))
q_hat = q_error.quat_mult_right(q_chec)
# 3.4 Compute corrected covariance
p_cov_hat = (np.eye(9) - (k_k.dot(H_km))).dot(p_cov_chec)
return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check,
q_check):
Rk = np.identity(3) * sensor_var
# 3.1 Compute Kalman Gain
Kk = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T +
Rk)
# 3.2 Compute error state
error = Kk @ (y_k - p_check)
error = np.squeeze(np.asarray(error))
# 3.3 Correct predicted state
p_hat = p_check + error[:3]
v_hat = v_check + error[3:6]
q_hat = Quaternion(euler=q_check).quat_mult_left(
Quaternion(euler=angle_normalize(error[6:])),
out='Quaternion').to_euler()
# q_hat = q_check
# 3.4 Compute corrected covariance
p_cov_hat = (np.identity(9) - Kk @ h_jac) @ p_cov_check
return p_hat, v_hat, q_hat, p_cov_hat
p_cov_euler_std = np.array(p_cov_euler_std)
# Get uncertainty estimates from P matrix
p_cov_std = np.sqrt(np.diagonal(p_cov[:, :6, :6], axis1=1, axis2=2))
titles = ['Easting', 'Northing', 'Up', 'Roll', 'Pitch', 'Yaw']
for i in range(3):
ax[0, i].plot(range(num_gt), gt.p[:, i] - p_est[:num_gt, i])
ax[0, i].plot(range(num_gt), 3 * p_cov_std[:num_gt, i], 'r--')
ax[0, i].plot(range(num_gt), -3 * p_cov_std[:num_gt, i], 'r--')
ax[0, i].set_title(titles[i])
ax[0, 0].set_ylabel('Meters')
for i in range(3):
ax[1, i].plot(range(num_gt), \
angle_normalize(gt.r[:, i] - p_est_euler[:num_gt, i]))
ax[1, i].plot(range(num_gt), 3 * p_cov_euler_std[:num_gt, i], 'r--')
ax[1, i].plot(range(num_gt), -3 * p_cov_euler_std[:num_gt, i], 'r--')
ax[1, i].set_title(titles[i + 3])
ax[1, 0].set_ylabel('Radians')
plt.show()
#### 7. Submission #############################################################################
################################################################################################
# Now we can prepare your results for submission to the Coursera platform. Uncomment the
# corresponding lines to prepare a file that will save your position estimates in a format
# that corresponds to what we're expecting on Coursera.
################################################################################################
# Pt. 1 submission
0])): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
pest = p_est[k - 1].reshape(3, 1)
vest = v_est[k - 1].reshape(3, 1)
qest = q_est[k - 1].reshape(4, 1)
imuf = (imu_f.data[k - 1]).reshape(3, 1)
imuw = (imu_w.data[k - 1]).reshape(3, 1)
C_ns = Quaternion(*q_est[k - 1]).to_mat()
CF = C_ns.dot(imuf)
# 1. Update state with IMU inputs
p_check = pest + delta_t * vest + ((delta_t**2) / 2) * (CF + g)
# print(CF+g)
v_check = vest + delta_t * (CF + g)
q_imu = Quaternion(axis_angle=angle_normalize(delta_t * imuw))
q_check = q_imu.quat_mult_right(qest)
# 1.1 Linearize the motion model and compute Jacobians
F_km = np.eye(9)
F_km[0, 3] = delta_t
F_km[1, 4] = delta_t
F_km[2, 5] = delta_t
insid = -(C_ns.dot(skew_symmetric(imuf))) * delta_t
F_km[3, 7] = insid[0, 1]
F_km[3, 8] = insid[0, 2]
F_km[4, 6] = insid[1, 0]
F_km[4, 8] = insid[1, 2]
F_km[5, 6] = insid[2, 0]
F_km[5, 7] = insid[2, 1]
Q_km = np.diag([
): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
# - Update Position, Velocity, Orientation respectively.
C_ns = Quaternion(*q_est[k - 1]).to_mat()
c_term = C_ns.dot(imu_f.data[k - 1]) + g
# print("Q_est={},\n mat={},\n iumu_f[k-1]={}".format(Q, Q.to_mat(),imu_f.data[k - 1]))
# print("c_term = {}".format(c_term))
p_check = p_est[k - 1] + delta_t * v_est[k - 1] + (0.5 *
(delta_t**2)) * c_term
v_check = v_est[k - 1] + delta_t * c_term
# q_check = Quaternion(*q_est[k-1]).quat_mult_right(Quaternion(axis_angle=(imu_w.data[k-1] * delta_t)))
theta = angle_normalize(imu_w.data[k - 1] * delta_t)
q_check = Quaternion(euler=theta).quat_mult_left(Quaternion(*q_est[k - 1]))
q_check = Quaternion(
euler=angle_normalize(Quaternion(*q_check).to_euler())).to_numpy()
# 1.1 Linearize the motion model and compute Jacobians
F_km1 = np.zeros((9, 9))
F_km1[0:3, 0:3] = np.eye(3)
F_km1[0:3, 3:6] = delta_t * np.eye(3)
F_km1[3:6, 3:6] = np.eye(3)
# F_km1[3:6, 6:9] = -C_ns.dot(skew_symmetric(imu_f.data[k-1].reshape(3,1)))*delta_t # Huge changes in Z axis
F_km1[3:6, 6:9] = angle_normalize(
-skew_symmetric(C_ns @ imu_f.data[k - 1]) * delta_t)
F_km1[6:, 6:] = np.eye(3)
L_km1 = np.zeros((9, 6))
for k in range(1, imu_f.data.shape[0]
): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
p_last = p_est[k - 1]
v_last = v_est[k - 1]
q_last = Quaternion(*q_est[k - 1]).to_euler()
p_cov_last = p_cov[k - 1]
C_ns = Quaternion(euler=q_last).to_mat()
acceleration = C_ns @ imu_f.data[k - 1] + g
v_check = v_last + (acceleration * delta_t)
p_check = p_last + (v_last * delta_t) + (0.5 * acceleration * delta_t**2)
q_check_temp = Quaternion(euler=q_last).quat_mult_left(
Quaternion(euler=angle_normalize(imu_w.data[k - 1] * delta_t)))
q_check = Quaternion(*q_check_temp).to_euler()
# 1.1 Linearize the motion model and compute Jacobians
Fk = np.identity(9)
Fk[0:3, 3:6] = np.identity(3) * delta_t
# Fk[3:6, 6:9] = -C_ns @ imu_f.data[k-1] * delta_t
Fk[3:6, 6:9] = -(C_ns @ skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1)))) * delta_t
# Fk = np.asmatrix(Fk)
Qk = np.diag([
var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w
]) * (delta_t**2)
# Qk = np.asmatrix(Qk)
p_check = p_est[k - 1]
# print(p_check.shape)
#p_check=p_check.reshape((1,3))
v_check = v_est[k - 1]
#v_check=v_check.reshape((1,3))
q_check = q_est[k - 1]
#q_check=q_check.reshape((1,4))
p_cov_check = p_cov[k - 1]
#p_cov_check=p_cov_check.reshape((1,3))
Cns = Quaternion(*q_check).to_mat() # asterix (*) here is tuple unpacking
theta = delta_t * imu_w.data[k - 1]
q_theta = Quaternion(axis_angle=angle_normalize(theta))
#q_check=Quaternion(axis_angle=dTheta).quat_mult(q_check, out='np')
p_check = p_check + delta_t * v_check + 0.5 * delta_t * delta_t * (
Cns @ imu_f.data[k - 1] + g)
v_check = v_check + delta_t * (Cns @ imu_f.data[k - 1] + g)
q_check = Quaternion(*q_est[k - 1]).quat_mult_left(q_theta)
# 1.1 Linearize the motion model and compute Jacobians
f_jac = np.eye(9)
f_jac[0:3, 3:6] = delta_t * np.eye(3)
f_jac[3:6, 6:9] = -(
Cns @ skew_symmetric(imu_f.data[k - 1].reshape(3, 1))) * delta_t
return True
else:
return False
#### 5. Main Filter Loop #######################################################################
################################################################################################
# Now that everything is set up, we can start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################################
for k in range(1, imu_f.data.shape[0]): # start at 1 b/c we have initial prediction from gt
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
C_ns = Quaternion(*q_est[k-1]).to_mat()
C_f = C_ns @ imu_f.data[k-1]
theta = angle_normalize(imu_w.data[k-1]*delta_t)
p_check = p_est[k-1] + delta_t*v_est[k-1] + 0.5*(delta_t**2)*(C_f + g)
v_check = v_est[k-1] + delta_t*(C_f + g)
q_check = Quaternion(axis_angle=theta).quat_mult_right(q_est[k-1])
# 1.1 Linearize the motion model and compute Jacobians
F_k_1 = np.eye(9)
F_k_1[0:3,3:6] = delta_t*np.eye(3)
F_k_1[3:6,6:9] = -(skew_symmetric(C_ns@imu_f.data[k-1].reshape((3,1))))*delta_t
Q = np.eye(6)
Q[0:3,0:3] = var_imu_f*np.eye(3)
Q[3:6,3:6] = var_imu_w*np.eye(3)
Q = (delta_t**2)*Q
