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())
