|
|
@ -191,9 +191,9 @@ class LocKalman(): |
|
|
|
|
|
|
|
|
|
|
|
# Observation matrix modifier |
|
|
|
# Observation matrix modifier |
|
|
|
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) |
|
|
|
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): |
|
|
|
H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) |
|
|
|
H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): |
|
|
|
H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:] |
|
|
|
H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:] |
|
|
|
|
|
|
|
|
|
|
|
# these error functions are defined so that say there |
|
|
|
# these error functions are defined so that say there |
|
|
@ -205,17 +205,17 @@ class LocKalman(): |
|
|
|
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) |
|
|
|
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) |
|
|
|
|
|
|
|
|
|
|
|
err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) |
|
|
|
err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): |
|
|
|
delta_quat = sp.Matrix(np.ones(4)) |
|
|
|
delta_quat = sp.Matrix(np.ones(4)) |
|
|
|
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :]) |
|
|
|
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :]) |
|
|
|
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat |
|
|
|
err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): |
|
|
|
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) |
|
|
|
err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) |
|
|
|
|
|
|
|
|
|
|
|
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) |
|
|
|
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |
|
|
|
for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): |
|
|
|
inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) |
|
|
|
inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |
|
|
|
for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): |
|
|
|
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] |
|
|
|
delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] |
|
|
|
inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) |
|
|
|
inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) |
|
|
|
|
|
|
|
|
|
|
|