#### 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 nominal state with IMU inputs
Rotation_Mat = Quaternion(*q_est[k - 1]).to_mat()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t**2) * (
Rotation_Mat.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[k -
1] + delta_t * (Rotation_Mat.dot(imu_f.data[k - 1]) - g)
q_est[k] = Quaternion(euler=delta_t * imu_w.data[k - 1]).quat_mult(
q_est[k - 1])
# 1.1 Linearize Motion Model and compute Jacobians
F = np.eye(9)
imu = imu_f.data[k - 1].reshape((3, 1))
F[0:3, 3:6] = delta_t * np.eye(3)
F[3:6, 6:9] = Rotation_Mat.dot(-skew_symmetric(imu)) * delta_t
# 2. Propagate uncertainty
Q = np.eye(6)
Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3]
Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
Cns @ imu_f.data[k - 1] + g) #Update position
v_est[k] = v_est[k - 1] + delta_t * (Cns @ imu_f.data[k - 1] + g
) #Update velocity
# q_est[k] = Quaternion(euler = imu_w.data[k-1]*delta_t).quat_mult_right(q_est[k-1])
q_est[k] = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right(
q_est[k - 1]) #Update orientation
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(
9
) # Linearizing motion model jacobian. It has 9 states and thus there are 9 differentiables
F[0:3, 3:6] = delta_t * np.eye(
3
) # Based on the position jacobian when differentiated w.r.t velocities
# F[3:6,6:9] = -skew_symmetric(Cns@imu_f.data[k-1])*delta_t # This representation is wrong
F[3:6, 6:] = -(Cns.dot(skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1))))) * delta_t #relation between velocity and orientation
Q = np.eye(
6
) #IMU has 6 inputs of specific force and rotational velocity respectively
Q[0:3, 0:3] *= delta_t**2 * var_imu_f #Assignning specific force variance
Q[3:6,
3:6] *= delta_t**2 * var_imu_w #Assigning rotational velocity variance
# 2. Propagate uncertainty/predicted error covariance
p_cov[k] = F @ p_cov[
k -
1] @ F.T + l_jac @ Q @ l_jac.T #(9*9).(9*9).(9*9) + (9*6).(6*6).(6*9) eventually yields a 1*9*9 vector
# print(np.shape(p_cov))
# 3. Check availability of GNSS and LIDAR measurements
# Check 1: If both lidar and gnss data are available and their counters are less than the maximum number of measurements available
#### 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()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (
(delta_t**2) / 2) * (C_ns.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[k - 1] + (C_ns.dot(imu_f.data[k - 1]) + g) * delta_t
q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).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[:3, 3:6] = delta_t * np.eye(3)
F_k_1[3:6, 6:9] = -(C_ns @ skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1)))) * delta_t
Q_k_1 = (delta_t**2) * np.diag(
[var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w])
# 2. Propagate uncertainty
p_cov[k] = F_k_1.dot(p_cov[k - 1]).dot(F_k_1.T) + l_jac.dot(Q_k_1).dot(
l_jac.T)
# 3. Check availability of GNSS and LIDAR measurements
if lidar_i < lidar.t.shape[0] and lidar.t[lidar_i] <= imu_f.t[k - 1]:
#### 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
# - 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))
# 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, len(
imu_f.data[:,
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]
#### 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]
#print(*q_est[k-1])
rotation_matrix = Quaternion(*q_est[k - 1]).to_mat()
# 1. Update state with IMU inputs
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (delta_t**2 / 2) * (
rotation_matrix.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[
k - 1] + delta_t * (rotation_matrix.dot(imu_f.data[k - 1]) + g)
q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_right(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
# 2. Propagate uncertainty
F = np.identity(9)
Q = np.identity(6)
F[:3, 3:6] = delta_t * np.identity(3)
F[3:6, 6:] = -(rotation_matrix.dot(
skew_symmetric(imu_f.data[k - 1].reshape((3, 1)))))
Q[:, :3] *= delta_t**2 * var_imu_f
def main():
path = 'data/pt1_data.pkl'
gt, imu_a, imu_w, lidar, gnss = getData(path)
C_li, t_i_li = calibrationRotMatrix()
#transform the lidar data to inertial frame
lidar.data = (C_li @ lidar.data.T).T + t_i_li
#lidar index
lidar_idx = 0
#gnss index
gnss_idx = 0
##pre_computed matrix
I = np.identity(3)
#pertubation matrix
L_jac = np.zeros((18, 12))
L_jac[3:15, :] = np.identity(12)
# save the estimated result
p_est = np.zeros([imu_a.data.shape[0], 3]) # position estimates
v_est = np.zeros([imu_a.data.shape[0], 3]) # velocity estimates
q_est = np.zeros([imu_a.data.shape[0],
4]) # orientation estimates as quaternions
a_bias_est = np.zeros([imu_a.data.shape[0], 3]) #acc bias estimation
w_bias_est = np.zeros([imu_a.data.shape[0], 3]) #w bias estimation
g_est = np.zeros([imu_a.data.shape[0], 3])
p_cov = np.zeros([imu_a.data.shape[0], 18,
18]) # covariance matrices at each timestep
# intial state
p_est[0] = gt.p[0]
v_est[0] = gt.v[0]
q_est[0] = Quaternion(euler=gt.r[0]).to_numpy()
#bias are initially set to 0
g = np.array([0, 0, -9.81]) # gravity
g_est[0] = g
p_cov[0] = np.zeros(18) # covariance of estimate
#start the motion prediction
for k in range(1, imu_a.data.shape[0]):
# compute dt
delta_t = imu_a.t[k] - imu_a.t[k - 1]
# computed transform matrix to the common/navigayion frame
rotation_matrix = Quaternion(*q_est[k - 1]).to_mat()
# step 1 motion propogation using nonlinear function for normial state
p_est[k] = p_est[k - 1] + v_est[k - 1] * delta_t + (
0.5 * delta_t**2) * (rotation_matrix.dot(
imu_a.data[k - 1] - a_bias_est[k - 1]) + g_est[k - 1])
v_est[k] = v_est[k - 1] + delta_t * (rotation_matrix.dot(
imu_a.data[k - 1] - a_bias_est[k - 1]) + g_est[k - 1])
q_est[k] = Quaternion(
axis_angle=(imu_w.data[k - 1] - w_bias_est[k - 1]) *
delta_t).quat_mult_right(q_est[k - 1])
a_bias_est[k] = a_bias_est[k - 1]
w_bias_est[k] = w_bias_est[k - 1]
g_est[k] = g_est[k - 1]
#step 2 covariance propogation
F_m = np.identity(18)
F_m[0:3, 3:6] = delta_t * I
F_m[3:6, 6:9] = -(delta_t * rotation_matrix.dot(
skew_symmetric(imu_a.data[k - 1] - a_bias_est[k - 1])))
F_m[3:6, 9:12] = -(delta_t * rotation_matrix)
F_m[3:6, 15:18] = delta_t * I
F_m[6:9, 12:15] = -(delta_t * rotation_matrix)
Q = np.zeros((12, 12))
Q[0:3, 0:3] = (var_imu_f * delta_t**2) * I
Q[3:6, 3:6] = (var_imu_w * delta_t**2) * I
Q[6:9, 6:9] = (var_bias_f * delta_t**2) * I
Q[9:12, 9:12] = (var_bias_w * delta_t**2) * I
p_cov[k] = F_m.dot(p_cov[k - 1]).dot(F_m.T) + L_jac.dot(Q).dot(L_jac.T)
#step3 measurement correction
if lidar_idx < lidar.data.shape[0] and imu_a.t[
k - 1] == lidar.t[lidar_idx]:
p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[
k], p_cov[k] = measurement_update(var_lidar, p_cov[k],
lidar.data[lidar_idx],
p_est[k], v_est[k], q_est[k],
a_bias_est[k], w_bias_est[k],
g_est[k])
lidar_idx += 1
if gnss_idx < gnss.data.shape[0] and imu_a.t[k -
1] == gnss.t[gnss_idx]:
p_est[k], v_est[k], q_est[k], a_bias_est[k], w_bias_est[k], g_est[
k], p_cov[k] = measurement_update(var_gnss, p_cov[k],
gnss.data[gnss_idx],
p_est[k], v_est[k], q_est[k],
a_bias_est[k], w_bias_est[k],
g_est[k])
gnss_idx += 1
#convert from quaternion to euler angle
euler_est = np.zeros((q_est.shape[0], 3))
for i in range(len(q_est)):
euler_est[i] = Quaternion(*q_est[i]).to_euler()
#convert to degree
euler_est = 180 / np.pi * euler_est
plotResult(gt.p, gt.v, gt.r, p_est, v_est, euler_est, a_bias_est,
w_bias_est, g_est, imu_a.t, gt._t)
# evaluate rmse of pos
rmse_px, rmse_py, rmse_pz = rmse(gt.p, p_est)
print(rmse_px, rmse_py, rmse_pz)
# evalue rmse of velocity
rmse_vx, rmse_vy, rmse_vz = rmse(gt.v, v_est)
print(rmse_vx, rmse_vy, rmse_vz)
#### 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 nominal state with IMU inputs
C_n = Quaternion(*q_est[k - 1]).to_mat()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t**2) * (
C_n.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[k - 1] + delta_t * (C_n.dot(imu_f.data[k - 1]) - g)
q_est[k] = Quaternion(euler=delta_t * imu_w.data[k - 1]).quat_mult(
q_est[k - 1])
# 1.1 Linearize Motion Model and compute Jacobians
F = np.eye(9)
imu = imu_f.data[k - 1].reshape((3, 1))
F[0:3, 3:6] = delta_t * np.eye(3)
F[3:6, 6:9] = C_n.dot(-skew_symmetric(imu)) * delta_t
# 2. Propagate uncertainty
Q = np.eye(6)
Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3]
Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
def get_imu_data(self, stby=700, offset_quat=True, gravity_bias_est=False):
"""
Loads, cleans and assigns IMU data to object attributes.
Args:
stby (int):
Number of initial IMU readings (standby readings)
before first movement. Default: 700.
offset_quat (bool):
If True, the quaternion readings will be offset such that
initial heading is zero. Default: True.
gravity_bias_est (bool):
If True, the initial bias estimation, including estimating
gravity component in the z-direction, will be done by averaging
across the first ´stby´ number of IMU readings. Default: False
"""
imu = pd.read_csv(
self.imu_file,
usecols=[
"field.header.stamp", "field.orientation.x",
"field.orientation.y", "field.orientation.z",
"field.orientation.w", "field.angular_velocity.x",
"field.angular_velocity.y", "field.angular_velocity.z",
"field.linear_acceleration.x", "field.linear_acceleration.y",
"field.linear_acceleration.z"
])
imu.columns = [
"timestamp", "q_x", "q_y", "q_z", "q_w", "om_x", "om_y", "om_z",
"a_x", "a_y", "a_z"
]
# Assign arrays for timestamp, linear acceleration and rotational velocity
self.imu_t = np.round(
imu.timestamp.values / 1e9,
3) # converting to seconds with three decimal places
self.imu_f = imu[["a_x", "a_y", "a_z"]].values
self.imu_w = imu[["om_x", "om_y", "om_z"]].values
self.imu_q = imu[["q_w", "q_x", "q_y", "q_z"]].values
self.n = self.imu_f.shape[0] # number of IMU readings
if offset_quat:
# Transform quaternions to Euler angles
phi = np.ndarray((self.n, 3))
for i in range(self.n):
phi[i, :] = Quaternion(
*self.imu_q[i, :]).normalize().to_euler()
# Shift yaw angle such that the inital yaw equals zero
inityaw = np.mean(phi[:stby, 2]) # Estimated initial yaw
tf_yaw = wraptopi(phi[:, 2] - inityaw) # Transformed yaw
# Transform Euler angles back to quaternions
phi[:, 2] = tf_yaw
for i in range(self.n):
self.imu_q[i, :] = Quaternion(
euler=phi[i, :]).normalize().to_numpy()
if gravity_bias_est:
imu_f_trans = np.ndarray((stby, 3))
for i in range(stby):
C_ns = Quaternion(*self.imu_q[i, :]).normalize().to_mat()
imu_f_trans[i] = C_ns.dot(self.imu_f[i, :])
self.g = np.mean(imu_f_trans, 0) # bias + gravity
else:
self.g = np.array([0, 0, 9.80665]) # gravity
p_last = p_est[k - 1] # last position
v_last = v_est[k - 1] # last velocity
q_last = q_est[k - 1] # last orientation
f_last = imu_f.data[k - 1] # last force calculated by IMU
w_last = imu_w.data[k - 1] # last rotational rate calculated bu IMU
p_cov_last = p_cov[k - 1] # last covariance matrix
# 1. Update state with IMU inputs
# The quaternion will keep track of the orientation of our sensor frame S with respect to navigation frame N
Cns = Quaternion(*q_last).to_mat()
# 1.1 Linearize the motion model and compute Jacobians
p_predict = p_last + delta_t * v_last + (
(delta_t**2) / 2) * (Cns.dot(f_last) + g)
v_predict = v_last + delta_t * (Cns.dot(f_last) + g)
q_predict = Quaternion(axis_angle=w_last * delta_t).quat_mult_right(q_last)
# 2. Propagate uncertainty
F_last = np.eye(9) # F Jacobian matrix; L is constant
F_last[:3, 3:6] = np.eye(3) * delta_t
F_last[3:6, 6:9] = -skew_symmetric(Cns @ f_last) * delta_t
# f, w location (Covariance of measurement noise)
Q_last = np.eye(6)
Q_last[:3, :3] *= (delta_t**2) * var_imu_f
Q_last[-3:, -3:] *= (delta_t**2) * var_imu_w
# We may perform this propagation step multiple times before a measurement arrives from either GNSS receiver or the LIDAR sensor
p_cov_predict = F_last @ p_cov_last @ F_last.T + l_jac @ Q_last @ l_jac.T
#### 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
rotation_matrix = Quaternion(*q_est[k - 1]).to_mat()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (delta_t**2 / 2) * (
rotation_matrix.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[
k - 1] + delta_t * (rotation_matrix.dot(imu_f.data[k - 1]) + g)
q_est[k] = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right(
q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
imu = imu_f.data[k - 1].reshape((3, 1))
F[0:3, 3:6] = delta_t * np.eye(3)
F[3:6, 6:9] = rotation_matrix.dot(-skew_symmetric(imu)) * delta_t
# 2. Propagate uncertainty
Q = np.eye(6)
Q[0:3, 0:3] = var_imu_f * Q[0:3, 0:3]
Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
# 1. Update state with IMU inputs
C_ns = Quaternion(*q_est[k - 1]).to_mat()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + (
(delta_t**2) / 2) * (C_ns @ imu_f.data[k - 1] + g)
v_est[k] = v_est[k - 1] + delta_t * (C_ns @ imu_f.data[k - 1] + g)
q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_right(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
# 2. Propagate uncertainty
F_jac = np.eye(9)
F_jac[:3, 3:6] = np.eye(3) * delta_t
F_jac[3:6,
6:] = -(C_ns.dot(skew_symmetric(imu_f.data[k - 1].reshape((3, 1)))))
#F_jac[3:6, 6:] = -skew_symmetric(C_ns @ imu_f.data[k-1]) * delta_t
Q_jac = np.eye(6)
Q_jac[:3, :3] = var_imu_f * delta_t**2 * np.eye(3)
Q_jac[3:, 3:] = var_imu_w * delta_t**2 * np.eye(3)
p_cov[k] = F_jac @ p_cov[k - 1] @ F_jac.T + l_jac @ Q_jac @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
# Check that we didn't reach end of LIDAR data and that current LIDAR timestamp is close to that of IMU
if lidar_i < lidar.t.shape[0] and lidar.t[lidar_i] <= imu_f.t[k]:
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(
var_lidar, p_cov[k], lidar.data[lidar_i], p_est[k], v_est[k],
q_est[k])
lidar_i += 1
if gnss_i < gnss.t.shape[0] and gnss.t[gnss_i] <= imu_f.t[k]:
return p_hat, v_hat, q_hat, p_cov_hat
#### 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
rotation_matrix = Quaternion(*q_est[k-1]).to_mat()
# 1.1 Linearize the motion model and compute Jacobians
p_est[k] = p_est[k-1] + delta_t*v_est[k-1] + (delta_t**2 / 2)*(rotation_matrix.dot(imu_f.data[k-1]) + g)
v_est[k] = v_est[k-1] + delta_t*(rotation_matrix.dot(imu_f.data[k-1]) + g)
q_est[k] = Quaternion(axis_angle=imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1])
# 2. Propagate uncertainty
F = np.identity(9)
Q = np.identity(6)
F[:3, 3:6] = delta_t * np.identity(3)
# F[3:6, 6:] = -skew_symmetric(rotation_matrix.dot(imu_f.data[k-1])) * delta_t
F[3:6, 6:] = -(rotation_matrix.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1)))))
# Q[:3, :3] = var_imu_f * delta_t**2 * np.identity(3)
# Q[3:, 3:] = var_imu_w * delta_t**2 * np.identity(3)
Q[:, :3] *= delta_t**2 * var_imu_f
Q[:, -3:] *= delta_t**2 * var_imu_w
p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)
#### 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]).normalize().to_mat()
p_check = p_est[k - 1] + delta_t * v_est[k - 1] + (
(delta_t**2) / 2) * (C_ns.dot(imu_f.data[k - 1] - g))
v_check = v_est[k - 1] + delta_t * (C_ns.dot(imu_f.data[k - 1] - g))
q_check = Quaternion(axis_angle=delta_t *
imu_w.data[k - 1]).normalize().quat_mult(q_est[k - 1])
q_check = Quaternion(*q_check).normalize().to_numpy()
# 1.1 Linearize Motion Model
Fk = np.eye(9) #1
Fk[0:3, 3:6] = np.eye(3) * delta_t #2
Fk[3:6, 6:9] = -skew_symmetric(C_ns.dot(
imu_f.data[k - 1])) * delta_t #3print(Fk)
Qk = np.zeros((6, 6))
Qk[0:3, 0:3] = (delta_t**2) * var_imu_f * np.identity(3)
Qk[3:6, 3:6] = (delta_t**2) * var_imu_w * np.identity(3)
# 2. Propagate uncertainty
): # 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()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + delta_t**2 / 2 * (
c_ns @ imu_f.data[k - 1] + g)
v_est[k] = v_est[k - 1] + delta_t * (c_ns @ imu_f.data[k - 1] + g)
q_est[k] = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right(
q_est[k - 1])
q_est[k] = Quaternion(*q_est[k]).normalize().to_numpy()
# 1.1 Linearize the motion model and compute Jacobians
f_jac = np.eye(9)
f_jac[0:3, 3:6] = np.eye(3) * delta_t
f_jac[3:6, 6:9] = -c_ns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t
# 2. Propagate uncertainty
p_cov[k] = f_jac @ p_cov[k - 1] @ f_jac.T + l_jac @ (Q * delta_t *
delta_t) @ l_jac.T
# 3. Check availability of GNSS and LIDAR measurements
# if 0:
if gnss_i < len(gnss.t):
if abs(gnss.t[gnss_i] - imu_f.t[k]) <= 1e3:
y_k = gnss.data[gnss_i]
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(
var_gnss, p_cov[k], y_k, p_est[k], v_est[k], q_est[k])
gnss_i += 1
if 0:
# if lidar_i < len(lidar.t):
#### 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()
p_check = p_est[k - 1] + v_est[k - 1] * delta_t + 0.5 * (
C_ns.dot(imu_f.data[k - 1]) +
g) * delta_t**2 #proveri je l ide plus ili minus g
v_check = v_est[k - 1] + (C_ns.dot(imu_f.data[k - 1]) +
g) * delta_t
tempq = Quaternion(euler=imu_w.data[k - 1] * delta_t)
q_check = Quaternion(*q_est[k - 1]).quat_mult_right(tempq)
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
F[0:3, 3:6] = np.eye(3) * delta_t
F[3:6, 6:9] = -(C_ns.dot(
skew_symmetric(imu_f.data[k - 1].reshape(
(3, 1))))) * delta_t
Q = np.eye(6)
# 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
Cns = Quaternion(*q_est[k-1]).to_mat() # orientation quaternion at k-1
p_check = p_est[k-1] + delta_t * v_est[k-1] + ((delta_t**2)/2) * (Cns @ imu_f.data[k-1] - g)
v_check = v_est[k-1] + delta_t * (Cns @ imu_f.data[k-1] - g)
q_check = Quaternion(axis_angle = (imu_w.data[k-1])*delta_t).quat_mult(q_est[k-1]) # orientation at k
# 1.1 Linearize Motion Model
F_k_1 = np.eye(9) #1
F_k_1[0:3,3:6] = np.eye(3) * delta_t #2
F_k_1[3:6,6:9] = -Cns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t
L_k_1 = l_jac
Q_k = np.eye(6)
Q_k[0:3, 0:3] = Q_k[0:3, 0:3]*var_imu_f
Q_k[3:6, 3:6] = Q_k[3:6, 3:6]*var_imu_w
Q_k = (delta_t**2)*Q_k
# 2. Propagate uncertainty
p_cov_check = F_k_1@p_cov[k-1]@F_k_1.T + L_k_1@Q_k@L_k_1.T
# 3. Check availability of GNSS and LIDAR measurements
GNSSavailable = is_avl(gnss.t,imu_f.t[k]) #To check if GNSS reading is available
Lidaravailable = is_avl(lidar.t,imu_f.t[k]) # To check if LIDAR reading is available
return p_check, v_check, q_check, p_cov_check
#### 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
Rotation_Mat = Quaternion(*q_est[k-1]).to_mat()
aux1=Rotation_Mat.dot(imu_f.data[k-1]) + g
p_est[k] = p_est[k-1] + delta_t * v_est[k-1] + delta_t * delta_t * aux1/ 2
v_est[k] = v_est[k-1] + delta_t * aux1
q_est[k] = Quaternion(euler = delta_t * imu_w.data[k-1]).quat_mult(q_est[k-1])
Rotation_Mat = Quaternion(*q_est[k - 1]).to_mat()
p_est[k] = p_est[k - 1] + delta_t * v_est[k - 1] + 0.5 * (delta_t ** 2) * (Rotation_Mat.dot(imu_f.data[k - 1]) + g)
v_est[k] = v_est[k - 1] + delta_t * (Rotation_Mat.dot(imu_f.data[k - 1]) + g)
q_est[k] = Quaternion(euler = delta_t * imu_w.data[k - 1]).quat_mult(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
imu = imu_f.data[k - 1].reshape((3, 1))
F[0:3,3:6]=np.eye(3)*delta_t
return p_hat, v_hat, q_hat, p_cov_hat
#### 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
rotation_matrix = Quaternion(*q_est[k-1]).to_mat()
p_est[k] = p_est[k-1] + delta_t * v_est[k-1] + (delta_t**2 / 2)*(rotation_matrix.dot(imu_f.data[k-1]) + g)
v_est[k] = v_est[k-1] + delta_t*(rotation_matrix.dot(imu_f.data[k-1]) + g)
q_est[k] = (Quaternion(axis_angle = imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1]))
# 1.1 Linearize the motion model and compute Jacobians
F = np.identity(9)
F[:3, 3:6] = delta_t * np.identity(3)
F[3:6, 6:] = -(rotation_matrix @ (skew_symmetric(imu_f.data[k-1].reshape((3,1))))) * delta_t
Q = np.identity(6)
Q[:, :3] *= delta_t**2 * var_imu_f
Q[:, -3:] *= delta_t**2 * var_imu_w
p_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T)
if lidar_i < lidar.t.shape[0] and abs(lidar.t[lidar_i] - imu_f.t[k]) < 0.02:
p_est[k], v_est[k], q_est[k], p_cov[k] = measurement_update(var_lidar, p_cov[k], lidar.data[lidar_i].T, p_est[k], v_est[k], q_est[k])
lidar_i += 1
return p_hat, v_hat, q_hat, p_cov_hat
#### 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]
Cns = Quaternion(*q_est[k - 1]).to_mat()
# 1. Update state with IMU inputs
p_check = p_est[k - 1] + v_est[k - 1] * delta_t + (
Cns.dot(imu_f.data[k - 1]) + g) * (delta_t**2) * 0.5
v_check = v_est[k - 1] + (Cns.dot(imu_f.data[k - 1]) + g) * delta_t
q_check = Quaternion(euler=imu_w.data[k - 1] * delta_t).quat_mult_right(
q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
rotation_part = np.eye(3)
rotation_part = -Cns.dot(skew_symmetric(imu_f.data[k - 1].reshape(
3, 1))) * delta_t
F = np.eye(9)
F[0:3, 3:6] = delta_t * np.eye(3)
F[3:6, 6:9] = rotation_part
# start taking in the sensor data and creating estimates
# for our state in a loop.
################################################################################
# start at 1 b/c we have initial prediction from gt
for k in range(1, imu_f.data.shape[0]):
delta_t = imu_f.t[k] - imu_f.t[k - 1]
# 1. Update state with IMU inputs
#the ortation matrix of the previous quaternion (3x3)
C_ns = Quaternion(*q_est[k-1]).to_mat()
#a. Position
p_check = p_est[k-1] + (delta_t*v_est[k-1]) +\
((delta_t**2/2)*((C_ns.dot(imu_f.data[k-1]))+g))
#b. Velocity
v_check = v_est[k-1] + (delta_t*((C_ns.dot(imu_f.data[k-1]))+g))
#c. Orientation
q_check = Quaternion(\
euler =(imu_w.data[k-1]*delta_t)).quat_mult_right(q_est[k-1])
# 1.1 Linearize the motion model and compute Jacobians
#a. F_(k-1) Jacobian size of (9x9)
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] = -delta_t*skew_symmetric (C_ns.dot(imu_f.data[k-1]))
#b. L_(k-1) Jacobian (9x6)
cns = Quaternion(*q_est[k - 1]).to_mat()
temp = np.dot(cns, imu_f.data[k - 1])
p_est[k] = p_est[k -
1] + v_est[k - 1] * delta_t + (temp - g) * delta_t**2 / 2
v_est[k] = v_est[k - 1] + (temp - g) * delta_t
q_est[k] = Quaternion(axis_angle=imu_w.data[k - 1] * delta_t).quat_mult(
q_est[k - 1])
# 1.1 Linearize Motion Model
Q = np.eye(6)
Q[:3, :3] = var_imu_f * delta_t**2 * I3
Q[3:, 3:] = var_imu_w * delta_t**2 * I3
F = np.eye(9)
F[:3, 3:6] = I3 * delta_t
#F[3:6, 6:] = -skew_symmetric(temp) * delta_t
F[3:6, 6:] = -cns.dot(skew_symmetric(imu_f.data[k - 1])) * delta_t
p_cov[k] = F @ p_cov[k - 1] @ F.T + l_jac @ Q @ l_jac.T
# 2. Propagate uncertainty
# 3. Check availability of GNSS and LIDAR measurements
lidar_found = False
while lidar_index < lidar.t.shape[0]:
if abs(lidar.t[lidar_index] - imu_f.t[k]) < threshold:
lidar_found = True
break
elif lidar.t[lidar_index] > imu_f.t[k] + threshold:
break
lidar_index += 1
if lidar_found:
#### 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]
# pre-definitions
c_ns = Quaternion(*q_est[k - 1]).to_mat()
# 1. Update state with IMU inputs
p_check = p_est[k - 1] + (delta_t * v_est[k - 1]) + (delta_t**2 / 2) * (
c_ns.dot(imu_f.data[k - 1]) + g)
v_check = v_est[k - 1] + (delta_t * (c_ns.dot(imu_f.data[k - 1]) + g))
q_check = Quaternion(axis_angle=imu_w.data[k - 1] *
delta_t).quat_mult_right(q_est[k - 1])
# 1.1 Linearize the motion model and compute Jacobians
F = np.eye(9)
F[0:3, 3:6] = np.eye(3) * delta_t
F[3:6, 6:9] = -(skew_symmetric(c_ns.dot(imu_f.data[k - 1])) * delta_t)
# 2. Propagate uncertainty
Q = np.diag([
var_imu_f, var_imu_f, var_imu_f, var_imu_w, var_imu_w, var_imu_w
]) * delta_t**2
p_cov_check = (F.dot(p_cov[k - 1].dot(F.T))) + (l_jac.dot(Q.dot(l_jac.T)))
