#### 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
q_prev = Quaternion(
*q_est[k - 1, :]) # previous orientation as a quaternion object
q_curr = Quaternion(axis_angle=(imu_w.data[k - 1] *
delta_t)) # current IMU orientation
c_ns = q_prev.to_mat() # previous orientation as a matrix
f_ns = (c_ns @ imu_f.data[k - 1]) + g # calculate sum of forces
p_check = p_est[k - 1, :] + delta_t * v_est[k - 1, :] + 0.5 * (delta_t**
2) * f_ns
v_check = v_est[k - 1, :] + delta_t * f_ns
q_check = q_prev.quat_mult_left(q_curr)
# 1.1 Linearize the motion model and compute Jacobians
f_jac = np.eye(9) # motion model jacobian with respect to last state
f_jac[0:3, 3:6] = np.eye(3) * delta_t
f_jac[3:6, 6:9] = -skew_symmetric(c_ns @ imu_f.data[k - 1]) * delta_t
# 2. Propagate uncertainty
q_cov = np.zeros((6, 6)) # IMU noise covariance
q_cov[0:3, 0:3] = delta_t**2 * np.eye(3) * var_imu_f
q_cov[3:6, 3:6] = delta_t**2 * np.eye(3) * var_imu_w
#### 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
#calculating rotation matrix
quat = Quaternion(q_est[k - 1, 0], q_est[k - 1, 1], q_est[k - 1, 2],
q_est[k - 1, 3])
Cns = quat.to_mat()
temp = Cns.dot(imu_f.data[k - 1])
t3 = np.add(temp, g)
t2 = delta_t * v_est[k - 1]
t1 = (0.5 * delta_t**2) * t3
p_check = np.add(p_est[k - 1], np.add(t2, t1)) #updating p
v_check = np.add(v_est[k - 1], delta_t * t3) #updating v
q_euler = delta_t * imu_w.data[k - 1]
q_check = Quaternion(euler=q_euler).quat_mult_right(q_est[k -
1]) #updating q
# 1.1 Linearize the motion model and compute Jacobians
F = np.identity(9)
F[0:3, 3:6] = delta_t * np.eye(3)
################################################################################################
# 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]
p_check = p_est[k-1]
v_check = v_est[k-1]
q_check = q_est[k-1]
p_cov_check = p_cov[k-1]
# 1. Update state with IMU inputs
# convert q_check numpy array to q_check_quat Quaternion object to call to_mat
q_check_quat = Quaternion(w=q_check[0], x=q_check[1], y=q_check[2], z=q_check[3], axis_angle=None, euler=None)
C_ns = q_check_quat.to_mat()
p_check = p_check + delta_t * v_check + (0.5 * delta_t**2)*(C_ns.dot(imu_f.data[k - 1]) + g)
v_check = v_check + delta_t * (C_ns.dot(imu_f.data[k - 1]) + g)
q_check = Quaternion(euler=imu_w.data[k-1]*delta_t).quat_mult_right(q_check)
# 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] = -delta_t * skew_symmetric(C_ns.dot(imu_f.data[k - 1]))
#IMU process/motion model has accelerometer(a_x,a_y,a_z) and gyroscope(w_x,w_y,w_z) 6 degrees of uncorrelated uncertainty
# Now that everything is set up, we can start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################################
last_gnss_idx = 0
last_lidar_idx = 0
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]
curr_t = imu_f.t[k]
f_k_1 = imu_f.data[k - 1]
w_k_1 = imu_w.data[k - 1]
p_k_1 = p_est[k - 1]
v_k_1 = v_est[k - 1]
q_k_1 = Quaternion(q_est[k - 1, 0], q_est[k - 1, 1], q_est[k - 1, 2],
q_est[k - 1, 3])
Cns = q_k_1.to_mat()
# 1. Update state with IMU inputs
p_check = p_k_1 + delta_t * v_k_1 + (delta_t**2) / 2 * (Cns @ f_k_1 + g)
v_check = v_k_1 + delta_t * (Cns @ f_k_1 + g)
q_check = q_k_1.quat_mult_left(Quaternion(axis_angle=(w_k_1 * delta_t)))
# 1.1 Linearize the motion model and compute Jacobians
F_k_1 = np.identity(9)
F_k_1[0:3, 3:6] = np.identity(3) * delta_t
F_k_1[3:6, 6:9] = -1 * skew_symmetric(Cns @ f_k_1) * delta_t
# 2. Propagate uncertainty
Q_k = np.identity(6)
def ekf(self):
"""
Main loop of the extended Kalman filter sensor fusion procedure.
"""
lid_i, lid_state = self.initialize_lidar()
t = self.imu_t[0] # initialize time step
st = round(time.time()) # initialize runtime
print("Initiating Kalman filter loop...")
for k in range(1, self.n):
# Progress report
if k % 100 == 0:
tm_new = round(time.time())
tm_el = tm_new - st
tm_rm = (self.n - k) * tm_el // k
print(
"Iteration {0}/{1}: Time elapsed {2}m{3}sec - Est. time remaining {4}m{5}sec"
.format(k, self.n, tm_el // 60, tm_el % 60, tm_rm // 60,
tm_rm % 60))
delta_t = (self.imu_t[k] - self.imu_t[k - 1])
# Assign previous state
p_km = self.p_est[k - 1].reshape(3, 1)
v_km = self.v_est[k - 1].reshape(3, 1)
q = self.imu_q[k, :].reshape(4, 1)
del_u_km = self.del_u_est[k - 1].reshape(6, 1)
quat = Quaternion(*q).normalize()
C_ns = quat.to_mat()
# Assign control inputs and calibrate with estimated sensor error
f_km = self.imu_f[k - 1].reshape(3, 1) - del_u_km[:3]
w_km = self.imu_w[k - 1].reshape(3, 1) - del_u_km[3:]
# Update state
p_check = p_km + delta_t * v_km + delta_t**2 / 2 * (
C_ns.dot(f_km) - self.g.reshape(3, 1))
v_check = v_km + delta_t * (C_ns.dot(f_km) - self.g.reshape(3, 1))
q_check = q
del_u_check = del_u_km
# Linearize motion model
F, L = SLAM.state_space_model(f_km, C_ns, delta_t)
# Propagate uncertainty
p_cov_km = self.p_cov[k - 1, :, :]
p_cov_check = F.dot(p_cov_km).dot(F.T) + L.dot(self.Q_imu).dot(L.T)
# Check availability of LIDAR measurements
if lid_i != len(self.lid_r) and t >= self.lid_t[lid_i]:
if self.algorithm == "icp":
y_k = SLAM.icp_state(self.gf, self.lid_r[lid_i], lid_state)
if self.algorithm == "feature":
y_k = SLAM.feature_state(self.lid_r[lid_i - 1],
self.lid_r[lid_i], lid_state)
new_state = SLAM.measurement_update(y_k, p_check, v_check,
q_check, del_u_check,
p_cov_check, self.R_lid)
p_check, v_check, q_check, p_cov_check, del_u_check, lid_state = new_state
if self.algorithm == "feature":
x_hat = p_check[0, 0]
y_hat = p_check[1, 0]
yaw_hat = Quaternion(*q_check).normalize().to_euler()[2, 0]
update_map(self.gf, self.lid_r[lid_i], x_hat, y_hat,
yaw_hat)
lid_i += 1
# Store into state and uncertainty arrays
self.p_est[k, :] = p_check.reshape(3, )
self.v_est[k, :] = v_check.reshape(3, )
self.q_est[k, :] = q_check.reshape(4, )
self.del_u_est[k, :] = del_u_check.reshape(6, )
self.p_cov[k, :, :] = p_cov_check
# Increment time
t += delta_t
# Report finished progress
tm_el = round(time.time()) - st
print("Finished iterating after {0}m{1}sec".format(
tm_el // 60, tm_el % 60))
