def ins_ext_kfilter(imu_time, imu_accel, imu_gyro, accel_bias_std,
gyro_bias_std, attitude0, attitude0_std, gnss_time,
gnss_speed, gnss_dist, gnss_speed_std, gnss_dist_std):
# Output data
state_list = []
var_list = []
# IMU sampling period
imu_dt = imu_time[1] - imu_time[0]
dcm0 = utils.get_dcm(attitude0)
# State matrix
X = np.matrix([
# X position
[0.0],
# Y position
[0.0],
# Z position
[0.0],
# X speed
[0.0],
# Y speed
[0.0],
# Z speed
[0.0],
# Accel X bias
[0.0],
# Accel Y bias
[0.0],
# Accel Z bias
[0.0],
# Gyro X bias
[0.0],
# Gyro Y bias
[0.0],
# Gyro Z bias
[0.0],
# C11
[dcm0.item((0, 0))],
# C12
[dcm0.item((0, 1))],
# C13
[dcm0.item((0, 2))],
# C21
[dcm0.item((1, 0))],
# C22
[dcm0.item((1, 1))],
# C23
[dcm0.item((1, 2))],
# C31
[dcm0.item((2, 0))],
# C32
[dcm0.item((2, 1))],
# C33
[dcm0.item((2, 2))]
])
# Process noise matrix
Q = np.matrix([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
])
# Measurement noise matrix
R = np.matrix([[gnss_dist_std**2, 0, 0, 0], [0, gnss_dist_std**2, 0, 0],
[0, 0, gnss_dist_std**2, 0], [0, 0, 0, gnss_speed_std**2]])
'''
dcm_noise_0 = np.square(
utils.get_dcm( np.matrix([
# Psi
[ attitude0_std ],
# Theta
[ attitude0_std ],
# Gamma
[ attitude0_std ]
]) )
)
'''
'''
dcm_noise_0 = np.matrix([
[ 1.0, 1.0, 1.0 ],
[ 1.0, 1.0, 1.0 ],
[ 1.0, 1.0, 1.0 ]
])
'''
# State covariance matrix
'''
P = np.matrix([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 0, 0 ) ), 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 0, 1 ) ), 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 0, 2 ) ), 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 1, 0 ) ), 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 1, 1 ) ), 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 1, 2 ) ), 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 2, 0 ) ), 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 2, 1 ) ), 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_noise_0.item( ( 2, 2 ) ) ],
])
'''
dcm_c = 0.05
P = np.matrix([
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, 0, accel_bias_std**2, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, gyro_bias_std**2, 0, 0, 0, 0, 0,
0, 0, 0, 0
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, dcm_c, dcm_c, dcm_c, dcm_c,
dcm_c, dcm_c, dcm_c, dcm_c, dcm_c
],
])
gnss_i = 0
for t, accel, gyro in zip(imu_time, imu_accel, imu_gyro):
# ----- Kalman predict step
# Control vector matrix
U = np.matrix([
# X accel
[accel.item((0, 0))],
# Y accel
[accel.item((1, 0))],
# Z accel
[accel.item((2, 0))],
# X gyro
[gyro.item((0, 0))],
# Y gyro
[gyro.item((1, 0))],
# Z gyro
[gyro.item((2, 0))]
])
F = get_F_matrix(X, U, imu_dt)
X = exec_f_func(X, U, imu_dt)
P = F * P * F.transpose() + Q
# Gnss data is available
if (gnss_i < len(gnss_time) and t > gnss_time[gnss_i]):
# ----- Kalman update step
# Measurement matrix
Z = np.matrix([
# X position
[gnss_dist[gnss_i].item((0, 0))],
# Y position
[gnss_dist[gnss_i].item((1, 0))],
# Z position
[gnss_dist[gnss_i].item((2, 0))],
# Speed norm
[gnss_speed[gnss_i].item((0, 0))]
])
H = get_H_matrix(X, imu_dt)
# Calculate gain
K = P * H.transpose() * np.linalg.inv((H * P * H.transpose() + R))
# Estimate state mean
X = X + K * (Z - exec_h_func(X, imu_dt))
# Estimate state variance
P = P - K * H * P
gnss_i = gnss_i + 1
state_list.append(X.copy())
var_list.append(P.copy())
return [state_list, var_list]
def get_body_motion( psi0, theta0, gamma0, rot_changes_x, rot_changes_y, rot_changes_z, speed_changes, period ): # Speed norm global_speed_norm = [ np.matrix([ [ speed ] ]) for speed in param_from_changes( speed_changes, period ) ] # Body rotation angles body_attitude = [] for x,y,z in zip( param_from_changes( rot_changes_x, period ), param_from_changes( rot_changes_y, period ), param_from_changes( rot_changes_z, period ) ): body_attitude.append( np.matrix([ [ x ], [ y ], [ z ] ]) ) # Body rotation speed body_attitude_speed = rot_speed_from_angles( body_attitude, period ) # Total IMU samples count if ( len( global_speed_norm ) != len( body_attitude_speed ) ): print( "Angle changes time != speed changes time." ) # Global attitude in euler angles global_attitude_prev = np.matrix([ # Psi - around Y(up direction) axis [ psi0 ], # Theta - around Z(right wing direction) axis [ theta0 ], # Gamma - around X(nose cone direction) axis [ gamma0 ] ]) global_attitude = [] for body_w in body_attitude_speed: global_attitude_new = utils.attitude_euler_update( global_attitude_prev, body_w, period ) global_attitude.append( global_attitude_new ) global_attitude_prev = global_attitude_new # Global speed global_speed = [] for attitude, speed_norm in zip( global_attitude, global_speed_norm ): # Tangential speed of body speed_tang = np.matrix([ # X [ speed_norm.item( ( 0, 0 ) ) ], # Y [ 0 ], # Z [ 0 ] ]) global_speed.append( utils.get_dcm( attitude ) * speed_tang ) # Global acceleration global_accel = accel_from_speed( global_speed, period ) global_seem_accel = [ acc + np.matrix([ # X [ 0 ], # Y [ 9.81 ], # Z [ 0 ], ]) for acc in global_accel ] # Global distance global_dist = dist_from_speed( global_speed, period ) # Acceleration in body frame body_accel = [] for attitude, accel in zip( global_attitude, global_seem_accel ): body_accel.append( utils.get_inv_dcm( attitude ) * accel ) return [ body_attitude_speed, body_accel, global_attitude, global_seem_accel, global_speed, global_speed_norm, global_dist ]
[ imu_time, imu_accel, imu_gyro, gnss_time, gnss_speed, gnss_dist, real_accel_bias, real_gyro_bias, real_glob_attitude, real_glob_accel, real_glob_speed, real_glob_speed_norm, real_glob_dist ] = generate_signals( speed_changes, rot_changes_x, rot_changes_y, rot_changes_z, imu_period, accel_bias_std, accel_w_std, gyro_bias_std, gyro_w_std, gnss_period, gnss_speed_w_std, gnss_dist_w_std ) # Real signals euler_psi = [ np.rad2deg( v.item( ( 0, 0 ) ) ) for v in real_glob_attitude ] euler_theta = [ np.rad2deg( v.item( ( 1, 0 ) ) ) for v in real_glob_attitude ] euler_gamma = [ np.rad2deg( v.item( ( 2, 0 ) ) ) for v in real_glob_attitude ] attitude_dcm = utils.get_dcm( real_glob_attitude[ 0 ] ) dcm_psi = [ euler_psi[ 0 ] ] dcm_theta = [ euler_theta[ 0 ] ] dcm_gamma = [ euler_gamma[ 0 ] ] for gyro, gyro_bias in zip( imu_gyro[ 1 : ], real_gyro_bias[ 1 : ] ): wx = gyro.item( ( 0, 0 ) ) wy = gyro.item( ( 1, 0 ) ) wz = gyro.item( ( 2, 0 ) ) wx_b = gyro_bias.item( ( 0, 0 ) ) wy_b = gyro_bias.item( ( 1, 0 ) ) wz_b = gyro_bias.item( ( 2, 0 ) ) attitude_dcm = utils.attitude_dcm_update( attitude_dcm,
def get_F_matrix(x_vect, u_vect, period):
# Old state data
pos_gx = x_vect.item((0, 0))
pos_gy = x_vect.item((1, 0))
pos_gz = x_vect.item((2, 0))
speed_gx = x_vect.item((3, 0))
speed_gy = x_vect.item((4, 0))
speed_gz = x_vect.item((5, 0))
accel_bias_x = x_vect.item((6, 0))
accel_bias_y = x_vect.item((7, 0))
accel_bias_z = x_vect.item((8, 0))
w_bias_x = x_vect.item((9, 0))
w_bias_y = x_vect.item((10, 0))
w_bias_z = x_vect.item((11, 0))
psi = x_vect.item((12, 0))
theta = x_vect.item((13, 0))
gamma = x_vect.item((14, 0))
dcm = utils.get_dcm(np.matrix([[psi], [theta], [gamma]]))
cos_psi = math.cos(psi)
sin_psi = math.sin(psi)
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
cos_gamma = math.cos(gamma)
sin_gamma = math.sin(gamma)
# Calibrated IMU data
est_accel_ix = u_vect.item((0, 0)) - accel_bias_x
est_accel_iy = u_vect.item((1, 0)) - accel_bias_y
est_accel_iz = u_vect.item((2, 0)) - accel_bias_z
est_wx = u_vect.item((3, 0)) - w_bias_x
est_wy = u_vect.item((4, 0)) - w_bias_y
est_wz = u_vect.item((5, 0)) - w_bias_z
# d(C11)/d(psi)
d_c11_d_psi = cos_theta * (-sin_psi)
# d(C12)/d(psi)
d_c12_d_psi = -cos_gamma * (-sin_psi) * sin_theta + sin_gamma * cos_psi
# d(C13)/d(psi)
d_c13_d_psi = sin_gamma * (-sin_psi) * sin_theta + cos_gamma * cos_psi
# d(C21)/d(psi)
d_c21_d_psi = 0
# d(C22)/d(psi)
d_c22_d_psi = 0
# d(C23)/d(psi)
d_c23_d_psi = 0
# d(C31)/d(psi)
d_c31_d_psi = -cos_theta * cos_psi
# d(C32)/d(psi)
d_c32_d_psi = cos_gamma * cos_psi * sin_theta + sin_gamma * (-sin_psi)
# d(C33)/d(psi)
d_c33_d_psi = -sin_gamma * cos_psi * sin_theta + cos_gamma * (-sin_psi)
# d(C11)/d(theta)
d_c11_d_theta = (-sin_theta) * cos_psi
# d(C12)/d(theta)
d_c12_d_theta = -cos_gamma * cos_psi * cos_theta
# d(C13)/d(theta)
d_c13_d_theta = sin_gamma * cos_psi * cos_theta
# d(C21)/d(theta)
d_c21_d_theta = cos_theta
# d(C22)/d(theta)
d_c22_d_theta = cos_gamma * (-sin_theta)
# d(C23)/d(theta)
d_c23_d_theta = -sin_gamma * (-sin_theta)
# d(C31)/d(theta)
d_c31_d_theta = -(-sin_theta) * sin_psi
# d(C32)/d(theta)
d_c32_d_theta = cos_gamma * sin_psi * cos_theta
# d(C33)/d(theta)
d_c33_d_theta = -sin_gamma * sin_psi * cos_theta
# d(C11)/d(gamma)
d_c11_d_gamma = 0
# d(C12)/d(gamma)
d_c12_d_gamma = -(-sin_gamma) * cos_psi * sin_theta + cos_gamma * sin_psi
# d(C13)/d(gamma)
d_c13_d_gamma = cos_gamma * cos_psi * sin_theta + (-sin_gamma) * sin_psi
# d(C21)/d(gamma)
d_c21_d_gamma = 0
# d(C22)/d(gamma)
d_c22_d_gamma = (-sin_gamma) * cos_theta
# d(C23)/d(gamma)
d_c23_d_gamma = -cos_gamma * cos_theta
# d(C31)/d(gamma)
d_c31_d_gamma = 0
# d(C32)/d(gamma)
d_c32_d_gamma = (-sin_gamma) * sin_psi * sin_theta + cos_gamma * cos_psi
# d(C33)/d(gamma)
d_c33_d_gamma = -cos_gamma * sin_psi * sin_theta + (-sin_gamma) * cos_psi
# accel_gx = C11 * est_accel_ix + C12 * est_accel_iy + C13 * est_accel_iz
# accel_gy = C21 * est_accel_ix + C22 * est_accel_iy + C23 * est_accel_iz
# accel_gz = C31 * est_accel_ix + C32 * est_accel_iy + C33 * est_accel_iz
# d(accel_gx)/d(accel_bias_x) = -C11
d_agx_d_abx = -dcm.item((0, 0))
# d(accel_gx)/d(accel_bias_y) = -C12
d_agx_d_aby = -dcm.item((0, 1))
# d(accel_gx)/d(accel_bias_z) = -C13
d_agx_d_abz = -dcm.item((0, 2))
# d(accel_gx)/d(psi)
d_agx_d_psi = d_c11_d_psi * est_accel_ix + d_c12_d_psi * est_accel_iy + d_c13_d_psi * est_accel_iz
# d(accel_gx)/d(theta)
d_agx_d_theta = d_c11_d_theta * est_accel_ix + d_c12_d_theta * est_accel_iy + d_c13_d_theta * est_accel_iz
# d(accel_gx)/d(gamma)
d_agx_d_gamma = d_c11_d_gamma * est_accel_ix + d_c12_d_gamma * est_accel_iy + d_c13_d_gamma * est_accel_iz
# d(accel_gy)/d(accel_bias_x) = -C21
d_agy_d_abx = -dcm.item((1, 0))
# d(accel_gy)/d(accel_bias_y) = -C22
d_agy_d_aby = -dcm.item((1, 1))
# d(accel_gy)/d(accel_bias_z) = -C23
d_agy_d_abz = -dcm.item((1, 2))
# d(accel_gy)/d(psi)
d_agy_d_psi = d_c21_d_psi * est_accel_ix + d_c22_d_psi * est_accel_iy + d_c23_d_psi * est_accel_iz
# d(accel_gy)/d(theta)
d_agy_d_theta = d_c21_d_theta * est_accel_ix + d_c22_d_theta * est_accel_iy + d_c23_d_theta * est_accel_iz
# d(accel_gy)/d(gamma)
d_agy_d_gamma = d_c21_d_gamma * est_accel_ix + d_c22_d_gamma * est_accel_iy + d_c23_d_gamma * est_accel_iz
# d(accel_gz)/d(accel_bias_x) = -C31
d_agz_d_abx = -dcm.item((2, 0))
# d(accel_gz)/d(accel_bias_y) = -C32
d_agz_d_aby = -dcm.item((2, 1))
# d(accel_gz)/d(accel_bias_z) = -C33
d_agz_d_abz = -dcm.item((2, 2))
# d(accel_gz)/d(psi)
d_agz_d_psi = d_c31_d_psi * est_accel_ix + d_c32_d_psi * est_accel_iy + d_c33_d_psi * est_accel_iz
# d(accel_gz)/d(theta)
d_agz_d_theta = d_c31_d_theta * est_accel_ix + d_c32_d_theta * est_accel_iy + d_c33_d_theta * est_accel_iz
# d(accel_gz)/d(gamma)
d_agz_d_gamma = d_c31_d_gamma * est_accel_ix + d_c32_d_gamma * est_accel_iy + d_c33_d_gamma * est_accel_iz
# psi[i+1] = psi[i] + ( 1 / cos(theta) ) * ( wy * cos(gamma) - wz * sin(gamma) ) * period
# theta[i+1] = theta[i] + ( wy * sin(gamma) + wz * cos(gamma) ) * period
# gamma[i+1] = gamma[i] + ( wx - sin(theta) / cos(theta) * ( wy * cos(gamma) - wz * sin(gamma) ) ) * period
# d(wx)/d(wbx) = -1
# d(wy)/d(wby) = -1
# d(wz)/d(wbz) = -1
d_psi_d_wbx = 0
d_psi_d_wby = 1 / cos_theta * (-cos_gamma) * period
d_psi_d_wbz = 1 / cos_theta * (-(-sin_gamma)) * period
d_psi_d_psi = 1
d_psi_d_theta = sin_theta / (cos_theta**2) * (est_wy * cos_gamma -
est_wz * sin_gamma) * period
d_psi_d_gamma = 1 / cos_theta * (est_wy * (-sin_gamma) -
est_wz * cos_gamma) * period
d_theta_d_wbx = 0
d_theta_d_wby = -sin_gamma * period
d_theta_d_wbz = -cos_gamma * period
d_theta_d_psi = 0
d_theta_d_theta = 1
d_theta_d_gamma = (est_wy * cos_gamma + est_wz * (-sin_gamma)) * period
d_gamma_d_wbx = -period
d_gamma_d_wby = -sin_theta / cos_theta * (-cos_gamma) * period
d_gamma_d_wbz = -sin_theta / cos_theta * (-(-sin_gamma)) * period
d_gamma_d_psi = 0
d_gamma_d_theta = -1 / (cos_theta**2) * (est_wy * cos_gamma -
est_wz * sin_gamma) * period
d_gamma_d_gamma = 1 - sin_theta / cos_theta * (est_wy * (-sin_gamma) -
est_wz * cos_gamma) * period
dt2 = 0.5 * period**2
dt = period
F = np.matrix([
# rx ry rz vx vy vz abx aby abz wbx wby wbz psi theta gamma
[
1, 0, 0, dt, 0, 0, d_agx_d_abx * dt2, d_agx_d_aby * dt2,
d_agx_d_abz * dt2, 0, 0, 0, d_agx_d_psi * dt2, d_agx_d_theta * dt2,
d_agx_d_gamma * dt2
],
[
0, 1, 0, 0, dt, 0, d_agy_d_abx * dt2, d_agy_d_aby * dt2,
d_agy_d_abz * dt2, 0, 0, 0, d_agy_d_psi * dt2, d_agy_d_theta * dt2,
d_agy_d_gamma * dt2
],
[
0, 0, 1, 0, 0, dt, d_agz_d_abx * dt2, d_agz_d_aby * dt2,
d_agz_d_abz * dt2, 0, 0, 0, d_agz_d_psi * dt2, d_agz_d_theta * dt2,
d_agz_d_gamma * dt2
],
[
0, 0, 0, 1, 0, 0, d_agx_d_abx * dt, d_agx_d_aby * dt,
d_agx_d_abz * dt, 0, 0, 0, d_agx_d_psi * dt, d_agx_d_theta * dt,
d_agx_d_gamma * dt
],
[
0, 0, 0, 0, 1, 0, d_agy_d_abx * dt, d_agy_d_aby * dt,
d_agy_d_abz * dt, 0, 0, 0, d_agy_d_psi * dt, d_agy_d_theta * dt,
d_agy_d_gamma * dt
],
[
0, 0, 0, 0, 0, 1, d_agz_d_abx * dt, d_agz_d_aby * dt,
d_agz_d_abz * dt, 0, 0, 0, d_agz_d_psi * dt, d_agz_d_theta * dt,
d_agz_d_gamma * dt
],
[0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, d_psi_d_wbx, d_psi_d_wby, d_psi_d_wbz,
d_psi_d_psi, d_psi_d_theta, d_psi_d_gamma
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, d_theta_d_wbx, d_theta_d_wby,
d_theta_d_wbz, d_theta_d_psi, d_theta_d_theta, d_theta_d_gamma
],
[
0, 0, 0, 0, 0, 0, 0, 0, 0, d_gamma_d_wbx, d_gamma_d_wby,
d_gamma_d_wbz, d_gamma_d_psi, d_gamma_d_theta, d_gamma_d_gamma
],
])
return F
def exec_f_func(x_vect, u_vect, period):
# Old state data
pos_gx = x_vect.item((0, 0))
pos_gy = x_vect.item((1, 0))
pos_gz = x_vect.item((2, 0))
speed_gx = x_vect.item((3, 0))
speed_gy = x_vect.item((4, 0))
speed_gz = x_vect.item((5, 0))
accel_bias_x = x_vect.item((6, 0))
accel_bias_y = x_vect.item((7, 0))
accel_bias_z = x_vect.item((8, 0))
w_bias_x = x_vect.item((9, 0))
w_bias_y = x_vect.item((10, 0))
w_bias_z = x_vect.item((11, 0))
psi = x_vect.item((12, 0))
theta = x_vect.item((13, 0))
gamma = x_vect.item((14, 0))
# Calibrated IMU data
est_accel_ix = u_vect.item((0, 0)) - accel_bias_x
est_accel_iy = u_vect.item((1, 0)) - accel_bias_y
est_accel_iz = u_vect.item((2, 0)) - accel_bias_z
est_wx = u_vect.item((3, 0)) - w_bias_x
est_wy = u_vect.item((4, 0)) - w_bias_y
est_wz = u_vect.item((5, 0)) - w_bias_z
# Global accel
accel_g = utils.get_dcm(np.matrix([[psi], [theta], [gamma]])) * np.matrix(
[[est_accel_ix], [est_accel_iy], [est_accel_iz]])
# Substract gravity component
accel_g = accel_g - np.matrix([
# X
[0],
# Y
[9.81],
# Z
[0]
])
accel_gx = accel_g.item((0, 0))
accel_gy = accel_g.item((1, 0))
accel_gz = accel_g.item((2, 0))
# New attitude
attitude_new = utils.attitude_euler_update(
np.matrix([[psi], [theta], [gamma]]),
np.matrix([[est_wx], [est_wy], [est_wz]]), period)
dt2 = 0.5 * period**2
dt = period
# New state data
return np.matrix([
[pos_gx + speed_gx * dt + accel_gx * dt2],
[pos_gy + speed_gy * dt + accel_gy * dt2],
[pos_gz + speed_gz * dt + accel_gz * dt2],
[speed_gx + accel_gx * dt],
[speed_gy + accel_gy * dt],
[speed_gz + accel_gz * dt],
[accel_bias_x],
[accel_bias_y],
[accel_bias_z],
[w_bias_x],
[w_bias_y],
[w_bias_z],
# Psi
[attitude_new.item((0, 0))],
# Theta
[attitude_new.item((1, 0))],
# Gamma
[attitude_new.item((2, 0))]
])
real_glob_accel_y = [v.item((1, 0)) for v in real_glob_accel] real_glob_accel_z = [v.item((2, 0)) for v in real_glob_accel] real_glob_speed_x = [v.item((0, 0)) for v in real_glob_speed] real_glob_speed_y = [v.item((1, 0)) for v in real_glob_speed] real_glob_speed_z = [v.item((2, 0)) for v in real_glob_speed] real_glob_speed_norm = [v.item((0, 0)) for v in real_glob_speed_norm] real_glob_dist_x = [v.item((0, 0)) for v in real_glob_dist] real_glob_dist_y = [v.item((1, 0)) for v in real_glob_dist] real_glob_dist_z = [v.item((2, 0)) for v in real_glob_dist] ''' real_psi = [ np.rad2deg( v.item( (0, 0) ) ) for v in real_glob_attitude ] real_theta = [ np.rad2deg( v.item( (1, 0) ) ) for v in real_glob_attitude ] real_gamma = [ np.rad2deg( v.item( (2, 0) ) ) for v in real_glob_attitude ] ''' real_dcm = [utils.get_dcm(v) for v in real_glob_attitude] real_C11 = [v.item((0, 0)) for v in real_dcm] real_C12 = [v.item((0, 1)) for v in real_dcm] real_C13 = [v.item((0, 2)) for v in real_dcm] real_C21 = [v.item((1, 0)) for v in real_dcm] real_C22 = [v.item((1, 1)) for v in real_dcm] real_C23 = [v.item((1, 2)) for v in real_dcm] real_C31 = [v.item((2, 0)) for v in real_dcm] real_C32 = [v.item((2, 1)) for v in real_dcm] real_C33 = [v.item((2, 2)) for v in real_dcm] # Estimated signals ekf_glob_dist_x = [v.item((0, 0)) for v in ins_state] ekf_glob_dist_y = [v.item((1, 0)) for v in ins_state] ekf_glob_dist_z = [v.item((2, 0)) for v in ins_state] ekf_glob_speed_x = [v.item((3, 0)) for v in ins_state]