def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain, y_k:(3,) p_check:(3,) p_cov_check:(9,9) R = np.eye(3) * sensor_var #Kk:(9,3) Kk = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + R) #print(Kk.shape) # 3.2 Compute error state dXk:(9,) dXk = Kk @ (y_k - p_check) #print(dXk.shape) # 3.3 Correct predicted state p_hat = p_check + dXk[0:3] v_hat = v_check + dXk[3:6] q_Phi = Quaternion(axis_angle=angle_normalize(dXk[6:9])) q_hat = Quaternion(*q_check).quat_mult_right(q_Phi) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - Kk @ h_jac) @ p_cov_check return p_hat, v_hat, q_hat, p_cov_hat
def __init__(self, ekf_p_pub, ekf_v_pub): # Define estimated states. self.p_est = np.zeros([3]) self.v_est = np.zeros([3]) self.q_est = Quaternion(euler=np.array([0, 0, 0])).to_numpy() self.p_cov_est = np.zeros([9, 9]) # Define corrected states. self.p_km = np.zeros([3]) self.v_km = np.zeros([3]) self.q_km = Quaternion(euler=np.array([0, 0, 0])).to_numpy() self.p_cov_km = np.zeros([9, 9]) self.bias_comp_counter = 0 self.imu_f_bias = np.zeros([3]) self.imu_w_bias = np.zeros([3]) self.ekf_p_pub = ekf_p_pub self.pose_msg = PoseStamped() self.pose_msg.header.frame_id = "base_link" self.ekf_v_pub = ekf_v_pub self.twist_msg = TwistStamped() self.twist_msg.header.frame_id = "base_link" # Main Filter Loop self.F = np.identity(9) self.Q = np.identity(6) self.last_imu_time = None
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain kg = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + sensor_var) # 3.2 Compute error state error = kg @ (y_k - p_check) # 3.3 Correct predicted state p_hat = np.zeros(p_check.shape) v_hat = np.zeros(v_check.shape) q_hat = np.zeros(q_check.shape) p_cov_hat = np.zeros(p_cov_check.shape) delta_theta = np.zeros(p_check.shape) for i in range(3): p_hat[i] = p_check[i] + error[i] for i in range(3): v_hat[i] = v_check[i] + error[i + 3] for i in range(3): delta_theta[i] = delta_theta[i] + error[i + 6] quaterr = Quaternion(axis_angle=delta_theta).to_numpy() #print(quaterr) q_hat = (Quaternion(*quaterr).quat_mult_left(Quaternion(*q_check), out='np')) q_hat = (Quaternion(*q_hat).normalize()).to_numpy() # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - kg @ 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): # construct H_k = [I, 0, 0] (size = 3 x 9) H_k = np.zeros([3, 9]) H_k[0:3, 0:3] = np.identity(3) # 3.1 Compute Kalman Gain # evaluate size chain: (9 x 9) x (9 x 3) x ( (3 x 9) x (9 x 9) x (9 x 3) + (3 x 3) ) # K_k should have a size: (9 x 3) K_k = p_cov_check @ H_k.T @ inv(H_k @ p_cov_check @ H_k.T + sensor_var) # 3.2 Compute error state # evaluate size chain: (9 x 3) x ( (3 x 1) - (3 x 1) ) # delta_x_k should have a size: (9 x 1) delta_x_k = K_k @ (y_k - p_check) # 3.3 Correct predicted state p_hat = p_check + delta_x_k[0:3] v_hat = v_check + delta_x_k[3:6] ## use of self built function: # q_hat = quaternion_left_prod( delta_x_k[6:9] ) @ q_check # q_hat = normalize_quaternion(q_hat) ## use of pre-built functions: q_obj = Quaternion(euler=delta_x_k[6:9]).quat_mult_left(q_check) q_hat = Quaternion(*q_obj).normalize().to_numpy() # q_hat = Quaternion(*q_obj).to_numpy() # Note: after test, it tuns out we don't have to normalize the quaternion # 3.4 Compute corrected covariance # evaluate size chain: ( (9 x 9) - (9 x 3) x (3 x 9) ) x (9 x 9) p_cov_hat = (np.identity(9) - K_k @ H_k) @ p_cov_check return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update_rotation(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain R_cov = sensor_var * np.eye(3) K = p_cov_check.dot( q_jac.T.dot(np.linalg.inv(q_jac.dot(p_cov_check.dot(q_jac.T)) + R_cov))) # 3.2 Compute error state q_rpy = Quaternion(w=q_check[0], x=q_check[1], y=q_check[2], z=q_check[3]).to_euler() ## TODO: Here we only update yaw angle, and y_k is your current observation #! est_k = [est_height, est_roll, est_pitch] #! y_k = [cam_height, cam_roll, cam_pitch] est_k = np.array([p_check[2], q_rpy[0], q_rpy[1]]) delta_x = K.dot(y_k - est_k) # 3.3 Correct predicted state p_check = p_check + delta_x[:3] v_check = v_check + delta_x[3:6] q_check = Quaternion(axis_angle=delta_x[6:]).quat_mult(q_check) # 3.4 Compute corrected covariance p_cov_check = (np.eye(9) - K.dot(q_jac)).dot(p_cov_check) return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain # K_k = P_k * H_k.T * inv( H_k * P_k * H_k.T + R_k ) try: temp = matmul(h_jac, matmul(p_cov_check, h_jac.T)) + sensor_var * np.eye(3) inv = np.linalg.inv(temp) # print("temp: ", temp.shape, "sensor_var: ", sensor_var) K = matmul( p_cov_check, matmul(h_jac.T, inv) ) # np.linalg.inv(matmul(h_jac, matmul(p_cov_check, h_jac.T)) + sensor_var ))) except np.linalg.LinAlgError as err: if 'Singular matrix' in str(err): raise "A singular matrix " # 3.2 Compute error state # print("y_k size: ", y_k.shape, "h_jac size: ", h_jac.shape, "p_check size: ", p_check.shape, "P_CHECK: ", p_check) error_state = y_k - p_check # matmul(h_jac[:3, :3], p_check) # 3.3 Correct predicted state p_hat = p_check + matmul(K, error_state)[:3] v_hat = v_check + matmul(K, error_state)[3:6] # print("error_state ", error_state.shape, "K: ", K.shape, "q_check: ", q_check.shape) q_hat = Quaternion( axis_angle=matmul(K, error_state)[6:]).quat_mult_right(q_check) # 3.4 Compute corrected covariance p_cov_hat = matmul(np.eye(9) - matmul(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 H = np.zeros([3, 9]) H[:3, :3] = np.identity(3) # [3, 9] if sensor_var == "gnss": R = np.identity(3) * var_gnss # [3, 3] elif sensor_var == "lidar": R = np.identity(3) * var_lidar # [3, 3] K = p_cov_check @ H.T @ np.linalg.inv(H @ p_cov_check @ H.T + R) # [9, 3] # 3.2 Compute error state delta_x = K @ (y_k - p_check[:3]) # [9] # 3.3 Correct predicted state delta_p = delta_x[0:3] # [3] delta_v = delta_x[3:6] # [3] delta_phi = delta_x[6:9] # [3] p_check = p_check + delta_p v_check = v_check + delta_v q_check = Quaternion(axis_angle=delta_phi).quat_mult(q_check) # 3.4 Compute corrected covariance p_cov_check = (np.identity(9) - K @ H) @ p_cov_check # [9, 9] return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check, a_bias_check, w_bias_check, g_check): # measurement model jacobian h_jac = np.zeros([3, 18]) h_jac[:, :3] = np.eye(3) R = sensor_var * np.identity(3) #step1 Compute Kalman Gain K_k = p_cov_check.dot(h_jac.T).dot( np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T) + R)) #step2 Compute error state error = K_k.dot(y_k - p_check) #step3 Correct predicted state p_hat = p_check + error[0:3] v_hat = v_check + error[3:6] phi_del = error[6:9] q_hat = Quaternion(euler=phi_del).quat_mult_right(q_check) a_bias_hat = a_bias_check + error[9:12] w_bias_hat = w_bias_check + error[12:15] g_hat = g_check + error[15:18] #step4 Compute corrected covariance p_cov_hat = (np.identity(18) - K_k.dot(h_jac)).dot(p_cov_check) p_conv_hat = 0.5 * (p_cov_hat.T + p_cov_hat) return p_hat, v_hat, q_hat, a_bias_hat, w_bias_hat, g_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 if sensor_var == "gnss": # [3, 3] R = np.eye(3) * var_gnss elif sensor_var == "lidar": # [3, 3] R = np.eye(3) * var_lidar # [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 # (y_k - p_check) is the innovation or difference between our predicted vehicle position (p_check) and the measured position (y_k) delta_x = K @ (y_k - p_check[:3]) # [9] # 3.3 Correct predicted state delta_p = delta_x[0:3] # [3] position delta_v = delta_x[3:6] # [3] velocity delta_phi = delta_x[6:9] # [3] quaternion orientation p_check = p_check + delta_p v_check = v_check + delta_v q_check = Quaternion(axis_angle=delta_phi).quat_mult_left( q_check ) # global orientation error - The operation update involves multiplying by the error state quaternion on the left # 3.4 Compute corrected covariance p_cov_check = (np.eye(9) - K @ h_jac) @ p_cov_check # [9, 9] return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain #R matrice delle vairanze dei sensori R = np.identity(3) * sensor_var K_gain = p_cov_check.dot(h_jac.T).dot( np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T) + R)) # 3.2 Compute error state dx = K_gain.dot(y_k - p_check) # 3.3 Correct predicted state p_hat = p_check + dx[0:3] v_hat = v_check + dx[3:6] q_hat = Quaternion(euler=dx[6:]).quat_mult_right(q_check) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - K_gain.dot(h_jac)).dot(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 if sensor_var == "gnss": # [3, 3] R = np.eye(3) * var_gnss elif sensor_var == "lidar": # [3, 3] R = np.eye(3) * var_lidar # [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 = K @ (y_k - p_check[:3]) # [9] # 3.3 Correct predicted state delta_p = delta_x[0:3] # [3] delta_v = delta_x[3:6] # [3] delta_phi = delta_x[6:9] # [3] p_check = p_check + delta_p v_check = v_check + delta_v q_check = Quaternion(axis_angle=delta_phi).quat_mult_left(q_check) # 3.4 Compute corrected covariance p_cov_check = (np.eye(9) - K @ h_jac) @ p_cov_check # [9, 9] return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain K = p_cov_check @ h_jac.T @ ( np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + sensor_var * np.eye(3)) ) # (9*9).(9*3) yields a (9*3) vector and np.inv((3*9).(9*9).(9*3)+(3*9).(9*3)) yields a (3*3) vector # print(np.shape(K)) # The Kalman gain is (9*3) matrix # print(np.shape(p_check)) # print(np.shape(y_k)) #Both of the above are a vector with 3 elements # 3.2 Compute error state del_xk = K @ (y_k - p_check) #(9*3).(3*1) yields a (9*1) vector del_pk = del_xk[:3] #Assigning x,y and z position del_vk = del_xk[3:6] #Assigning x,y and z velocity del_qk = del_xk[6:] #Assigning x,y and z orientation # 3.3 Correct predicted state p_hat = p_check + del_pk v_hat = v_check + del_vk q_hat = Quaternion(euler=del_qk).quat_mult_right( q_check ) #Pass the euler angles to convert them to quaternion and then do a quaternion multiplication # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - K.dot(h_jac)).dot( p_cov_check) #((9*9) - (9*3).(9*9)).(9*9) yields a (9*9) matrix # print(np.shape(p_cov_hat)) 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): Measurement = h_jac @ np.array( [y_k[0], y_k[1], y_k[2], 0., 0., 0., 0., 0., 0.]).T + sensor_var #print('Measurement: ', Measurement, 'Measurement shape', np.shape(Measurement)) # 3.1 Compute Kalman Gain K = p_cov_check @ h_jac.T @ inv(h_jac @ p_cov_check @ h_jac.T + np.eye(3) * sensor_var) #print('Kalman Gain:', K, 'Kalman Gain shape:', np.shape(K)) # 3.2 Compute error state Error_state = K @ (Measurement - p_check) #print('Error State:', Error_state, 'Error state shape:', np.shape(Error_state)) # 3.3 Correct predicted state p_hat = p_check + Error_state[0:3] #print('p_hat', p_hat) v_hat = v_check + Error_state[3:6] #print('v_hat', v_hat) q_hat = Quaternion(axis_angle=Error_state[6:9]).quat_mult_right(q_check) #print('q_hat', q_hat) # 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.0 shaping y_k = y_k.reshape([3, 1]) p_check = p_check.reshape([3, 1]) v_check = v_check.reshape([3, 1]) # 3.1 Compute Kalman Gain K_k = p_cov_check @ h_jac.T @ inv(h_jac @ p_cov_check @ h_jac.T + sensor_var) # 3.2 Compute error state dx_k = K_k @ (y_k - p_check) # 3.3 Correct predicted state p_hat = p_check + dx_k[:3] v_hat = v_check + dx_k[3:6] q_hat = Quaternion(euler=dx_k[6:]).quat_mult_left(q_check) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - K_k @ h_jac) @ p_cov_check # print(p_hat.shape) # print(v_hat.shape) # print(q_hat.shape) return p_hat, v_hat, q_hat, p_cov_hat
def measurement_update(self, sensor_var, y_k): """ Perform corrective EKF update using data from GNSS or LiDAR. Parameters ---------- sensor_var: Variance of sensor used to capture data represented in corrective data. y_k: Corrective data used to update ego-state. """ # 3.1 Compute Kalman Gain I = np.identity(3) R = I * sensor_var K = self.p_cov_est.dot(h_jac.T).dot( np.linalg.inv(h_jac.dot(self.p_cov_est).dot(h_jac.T) + R)) # 3.2 Compute error state error = K.dot(y_k - self.p_est) # 3.3 Correct predicted state p_del = error[:3] v_del = error[3:6] phi_del = error[6:] self.p_km = self.p_est + p_del self.v_km = self.v_est + v_del self.q_km = Quaternion(euler=phi_del).quat_mult_right(self.q_km) # 3.4 Compute corrected covariance self.p_cov_km = (np.identity(9) - K.dot(h_jac)).dot(self.p_cov_est) self.pub_state(self.p_km, self.v_km, self.q_km)
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.identity(3) #calculating sensor covariance matrix #R = np.identity(3) #R = np.diag(sensor_var) temp = h_jac.dot(p_cov_check.dot(h_jac.T)) temp = np.add(temp, R) temp = inv(temp) K = p_cov_check.dot(h_jac.T.dot(temp)) # 3.2 Compute error state del_x = K.dot(np.subtract(y_k, p_check)) # 3.3 Correct predicted state p_hat = np.add(p_check, del_x[0:3]) v_hat = np.add(v_check, del_x[3:6]) q_hat = Quaternion(euler=del_x[6:9]).quat_mult_left(q_check) # 3.4 Compute corrected covariance temp1 = np.subtract(np.identity(9), K.dot(h_jac)) p_cov_hat = temp1.dot(p_cov_check) return p_hat, v_hat, q_hat, p_cov_hat
def pub_state(self, p, v, q): """ Publish ego-state to cooresponding ROS topics. Parameters ---------- p: Ego-state position. v: Ego-state velocity. q: Ego-state quaternion in NumPy representation. """ quat = Quaternion(*q) self.pose_msg.pose.position.x = p[0] self.pose_msg.pose.position.y = p[1] # self.pose_msg.pose.position.z = p[2] self.pose_msg.pose.position.z = 0 self.pose_msg.pose.orientation.w = quat.w self.pose_msg.pose.orientation.x = quat.x self.pose_msg.pose.orientation.y = quat.y self.pose_msg.pose.orientation.z = quat.z self.pose_msg.header.stamp = rospy.Time.now() self.twist_msg.twist.linear.x = v[0] self.twist_msg.twist.linear.y = v[1] self.twist_msg.twist.linear.z = v[2] self.twist_msg.header.stamp = rospy.Time.now() self.ekf_p_pub.publish(self.pose_msg) self.ekf_v_pub.publish(self.twist_msg)
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain #print('Entered measurement update') inverse = inv( h_jac.dot(p_cov_check).dot(h_jac.T) + sensor_var * np.eye(3)) Kk = p_cov_check.dot(h_jac.T).dot(inverse) # 3.2 Compute error state delta_xk = Kk.dot(y_k - p_check) # 3.3 Correct predicted state # print(p_check,'<--pcheck') # print(delta_xk.shape,'<--deltaXk shape') # print(Kk.shape,'<--kk shape') p_hat = p_check + delta_xk[0:3] v_hat = v_check + delta_xk[3:6] q_hat = Quaternion(euler=delta_xk[6:]).quat_mult_left( q_check) #proveri je l left ili right # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - Kk.dot(h_jac)).dot(p_cov_check) return p_hat, v_hat, q_hat, p_cov_hat
def imu_update(self, imu_f, imu_w, delta_t): """ Perform update of ego-state using incoming IMU acceleration and rotation data. Assumptions Made ---------------- 1. The effects of gravity have been negated. That is, the last value of imu_f (imu_f[2]) is set to a constant value of 0. Parameters ---------- imu_f: 3D Vector containing IMU acceleration data. imu_w: 3D Vector containing IMU rotational velocity data. delta_t: Difference, in seconds, between current and previous IMU data. """ imu_f -= self.imu_f_bias imu_w -= self.imu_w_bias # Rotation matrix cooresponding to current ego-orientation. C_ns = Quaternion(*self.q_km).normalize().to_mat() # Compute ego-state using available data inputs. self.p_est = self.p_km + (delta_t * self.v_km) + \ (delta_t**2 / 2) * (C_ns @ imu_f + g) self.v_est = self.v_km + delta_t * (C_ns @ imu_f + g) self.q_est = Quaternion(axis_angle=imu_w * delta_t).quat_mult_right( self.q_km) # Linearize motion model and compute Jacobians. self.F = np.identity(9) self.F[:3, 3:6] = delta_t * np.identity(3) self.F[3:6, 6:] = -skew_symmetric(C_ns @ imu_f) self.Q = np.identity(6) self.Q[:, :3] *= var_imu_f * delta_t**2 self.Q[:, 3:] *= var_imu_w * delta_t**2 # Propagate uncertainty self.p_cov_est = self.F @ self.p_cov_km @ self.F.T + l_jac @ self.Q @ l_jac.T # Update previous ego-state before updating again. self.p_km = self.p_est.copy() self.v_km = self.v_est.copy() self.q_km = self.q_est.copy() self.p_cov_km = self.p_cov_est.copy() # Publish ego-state to cooresponding ROS topics. self.pub_state(self.p_km, self.v_km, self.q_km)
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 dx_k = K @ (y_k - p_check) # 3.3 Correct predicted state p_check += dx_k[0:3] v_check += dx_k[3:6] q_check = Quaternion(euler=dx_k[6:9]).quat_mult(Quaternion(*q_check)) # 3.4 Compute corrected covariance p_cov_check = (np.eye(9) - K @ h_jac) @ p_cov_check return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain k_k = p_cov_check @ h_jac.T @ np.linalg.inv( h_jac @ p_cov_check @ h_jac.T + np.eye(3) * sensor_var) # gain: k_k 9 x 3 matrix # 3.2 Compute error state delta_x = k_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] q_hat = Quaternion(euler=delta_x[6:9]).quat_mult_left(q_check) q_hat = Quaternion(*q_hat).normalize().to_numpy() # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - k_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): Kal = p_cov_check @ h_jac.T @ np.linalg.pinv( h_jac @ p_cov_check @ h_jac.T + sensor_var * np.eye(3)) # 3.2 Compute error state delta_x = np.dot(Kal, (y_k - p_check)) # 3.3 Correct predicted state p_check = p_check + delta_x[0:3] v_check = v_check + delta_x[3:6] q_check = Quaternion(axis_angle=delta_x[6:9]).quat_mult( Quaternion(*q_check)) # 3.4 Compute corrected covariances p_cov_check = (np.eye(9) - Kal @ h_jac) @ p_cov_check return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain Kk = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + np.identity(3) * sensor_var) # 3.2 Compute error state dxk = Kk.dot(y_k - p_check) # 3.3 Correct predicted state p_check = p_check + dxk[0:3] v_check = v_check + dxk[3:6] q_check = Quaternion(euler=dxk[6:9]).normalize().quat_mult( Quaternion(*q_check)) # 3.4 Compute corrected covariance p_cov_check = (np.identity(9) - Kk @ h_jac) @ p_cov_check return p_check, v_check, q_check, p_cov_check
def measurement_update(sensor_var, p_cov_check, y_k, p_check, v_check, q_check): # 3.1 Compute Kalman Gain tempoo = np.linalg.inv((h_jac @ p_cov_check @ h_jac.T) + sensor_var * np.eye(3)) K_k = p_cov_check @ h_jac.T @ tempoo # NNNNNnote here that the variance is not so true because we have to multiply it by the identity # 3.2 Compute error state delta_x = (y_k - p_check) #print(delta_x.shape) # 3.3 Correct predicted state p_hat = p_check + (K_k @ delta_x)[:3] v_hat = v_check + (K_k @ delta_x)[3:6] delta_q = Quaternion(axis_angle=(K_k @ delta_x)[6:]) q_hat = delta_q.quat_mult_right(q_check) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - K_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_cov = np.eye(3) * sensor_var k_gain = p_cov_check @ h_jac.T @ np.linalg.inv( (h_jac @ p_cov_check @ h_jac.T) + r_cov) # 3.2 Compute error state error_state = k_gain @ (y_k - p_check) # 3.3 Correct predicted state p_hat = p_check + error_state[0:3] v_hat = v_check + error_state[3:6] q_hat = Quaternion(axis_angle=error_state[6:9]).quat_mult_left( Quaternion(*q_check)) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - k_gain @ 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, R): # 3.1 Compute Kalman Gain K_k = np.dot( np.dot(p_cov_check, np.transpose(h_jac)), np.linalg.inv(np.dot(h_jac, np.dot(p_cov_check, np.transpose(h_jac))) + R)) # 3.2 Compute error state error_state = np.dot(K_k, (y_k - p_check)) # 3.3 Correct predicted state p_hat = p_check + error_state[0:3] v_hat = v_check + error_state[3:6] q_hat = Quaternion(axis_angle= error_state[6:9]).quat_mult_left(q_check) # 3.4 Compute corrected covariance p_cov_hat = np.dot(np.eye(9) - np.dot(K_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.eye(3) * sensor_var # 9x9 @ 9x3 @ (3x9 @ 9x9 @ 9x3 + 3x3) K = p_cov_check @ h_jac.T @ np.linalg.inv(h_jac @ p_cov_check @ h_jac.T + R) # 3.2 Compute error state Dx = K @ (y_k - p_check) # 3.3 Correct predicted state p_hat = p_check + Dx[0:3] v_hat = v_check + Dx[3:6] q_Phi = Quaternion(axis_angle=Dx[6:9]) q_hat = q_Phi.quat_mult_left(q_check, out='Quaternion') # 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 K_k = p_cov_check@h_jac.T@inv(h_jac@p_cov_check@h_jac.T + np.eye(3)*sensor_var) # 3.2 Compute error state delta_x = np.dot(K_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] q_hat = Quaternion(axis_angle=delta_x[6:9]).quat_mult_right(q_check) # 3.4 Compute corrected covariance p_cov_hat = (np.eye(9) - K_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
import numpy as np from rotations import Quaternion q = Quaternion() q.set(0.5, 0.2, 0.1, 0.3) print(q.to_string())