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