def _rhs_strapdown_ins(t, y):
q = y[0:4]
v = y[4:7]
p = y[7:10]
bg = y[10:13]
qn = np.sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2)
if qn > 0:
q = q / qn
q0, q1, q2, q3 = q
omega_b = np.array([
0.02 * np.sin(0.5 * t),
0.01 * np.cos(0.3 * t),
0.005 * np.sin(0.1 * t),
]) - bg
omega_ie_n = np.array([
_OMEGA_E * np.cos(_LAT0),
0.0,
-_OMEGA_E * np.sin(_LAT0),
])
C_bn = np.array([
[1 - 2*(q2**2 + q3**2), 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
[2*(q1*q2 + q0*q3), 1 - 2*(q1**2 + q3**2), 2*(q2*q3 - q0*q1)],
[2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*(q1**2 + q2**2)],
])
omega_nb = omega_b - C_bn.T @ omega_ie_n
dq = 0.5 * np.array([
-q1*omega_nb[0] - q2*omega_nb[1] - q3*omega_nb[2],
q0*omega_nb[0] + q2*omega_nb[2] - q3*omega_nb[1],
q0*omega_nb[1] + q3*omega_nb[0] - q1*omega_nb[2],
q0*omega_nb[2] + q1*omega_nb[1] - q2*omega_nb[0],
])
f_b = np.array([
2.0 * np.sin(0.2 * t),
1.0 * np.cos(0.15 * t),
-_G + 0.5 * np.sin(0.1 * t),
])
f_n = C_bn @ f_b
g_n = np.array([0.0, 0.0, _G])
omega_en_n = np.array([v[1] / _R_EARTH, -v[0] / _R_EARTH, 0.0])
dv = f_n + g_n - np.cross(2.0 * omega_ie_n + omega_en_n, v)
dp = np.array([
v[0],
v[1],
v[2],
])
dbg = -bg / _GYRO_BIAS_TAU
return np.concatenate([dq, dv, dp, dbg])