pow(3.0e-3, 2)
])
P[6:9,
6:9] = np.diag([1e-4, 1e-4,
1e-4]) #P for err_state, use euler not quaternion.
filter_init = 1
x_nom[3:6] = v[i]
x_nom[0:3] = gps[i, 3:6]
#end fi filter_init
dt = (timestamp[i] - timestamp[i - 1]) / 1e6
if predictState == 1:
#predict nominate state
delVel = (acc[i, :].transpose() - x_nom[10:13]) * dt
delAng = (gyro[i, :].transpose() - x_nom[13:16]) * dt
Cbn = q2R(x_nom[6:10])
delVel = np.dot(Cbn, delVel) - np.array([0, 0, 9.80665 * dt])
dq = from_axis_angle(delAng)
x_nom[6:10] = [
x_nom[6] * dq[0] - x_nom[7] * dq[1] - x_nom[8] * dq[2] -
x_nom[9] * dq[3], x_nom[6] * dq[1] + x_nom[7] * dq[0] +
x_nom[8] * dq[3] - x_nom[9] * dq[2], x_nom[6] * dq[2] -
x_nom[7] * dq[3] + x_nom[8] * dq[0] + x_nom[9] * dq[1],
x_nom[6] * dq[3] + x_nom[7] * dq[2] - x_nom[8] * dq[1] +
x_nom[9] * dq[0]
]
x_nom[6:10] = x_nom[6:10] / np.linalg.norm(x_nom[6:10])
Vel_pre = x_nom[3:6]
x_nom[3:6] = x_nom[3:6] + delVel
x_nom[0:3] = x_nom[0:3] + (x_nom[3:6] + Vel_pre) * dt / 2
pow(3.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0 * DEG_TO_RAD_DOUBLE, 2),
pow(3.0 * DEG_TO_RAD_DOUBLE, 2)
]
p[0:4, 0:4] = np.diag([1e-4, 1e-4, 1e-4, 1e-4])
#p[0:4,0:4] = initialize_quat_covariance(x[0:4], rotation_vector_variance)
filter_init = 1
x[4:7] = v[i]
x[7:10] = gps[i, 3:6]
#end fi filter_init
dt = (timestamp[i] - timestamp[i - 1]) / 1e6
if predictState == 1:
delAng = (gyro[i, :].transpose() - x[10:13]) * dt
delVel = (acc[i, :].transpose() - x[13:16]) * dt
Cbn = q2R(x[0:4])
delVel = np.dot(Cbn, delVel) - np.array([0, 0, 9.80665 * dt])
dq = from_axis_angle(delAng)
x[0:4] = [
x[0] * dq[0] - x[1] * dq[1] - x[2] * dq[2] - x[3] * dq[3],
x[0] * dq[1] + x[1] * dq[0] + x[2] * dq[3] - x[3] * dq[2],
x[0] * dq[2] - x[1] * dq[3] + x[2] * dq[0] + x[3] * dq[1],
x[0] * dq[3] + x[1] * dq[2] - x[2] * dq[1] + x[3] * dq[0]
]
x[0:4] = x[0:4] / np.linalg.norm(x[0:4])
Vel_pre = x[4:7].copy()
x[4:7] = x[4:7] + delVel
x[7:10] = x[7:10] + (x[4:7] + Vel_pre) * dt / 2
#end predict, then need to update P and Q
FF = [[
])
P[6:9,
6:9] = np.diag([1e-4, 1e-4,
1e-4]) #P for err_state, use euler not quaternion.
filter_init = 1
x_nom[3:6] = v[i]
x_nom[0:3] = gps[i, 3:6]
#a_b and w_b is 0, so do not do init
#end fi filter_init
dt = (timestamp[i] - timestamp[i - 1]) / 1e6
if predictState == 1:
#predict nominate state
delVel = (acc[i, :].transpose() - x_nom[10:13]) * dt
delAng = (gyro[i, :].transpose() - x_nom[13:16]) * dt
Cbn = q2R(x_nom[6:10])
delVel = np.dot(Cbn, delVel) - np.array([0, 0, 9.80665 * dt])
dq = from_axis_angle(delAng)
x_nom[6:10] = [
x_nom[6] * dq[0] - x_nom[7] * dq[1] - x_nom[8] * dq[2] -
x_nom[9] * dq[3], x_nom[6] * dq[1] + x_nom[7] * dq[0] +
x_nom[8] * dq[3] - x_nom[9] * dq[2], x_nom[6] * dq[2] -
x_nom[7] * dq[3] + x_nom[8] * dq[0] + x_nom[9] * dq[1],
x_nom[6] * dq[3] + x_nom[7] * dq[2] - x_nom[8] * dq[1] +
x_nom[9] * dq[0]
]
x_nom[6:10] = x_nom[6:10] / np.linalg.norm(x_nom[6:10])
Vel_pre = x_nom[3:6].copy()
x_nom[3:6] = x_nom[3:6] + delVel
x_nom[0:3] = x_nom[0:3] + (x_nom[3:6] + Vel_pre) * dt / 2