def stateVectorUpdate(stateVec,del_t):
transition_quat = getTransitionQuat(stateVec[10:13],del_t)
result_quat = trnsfrm.quaternion_multiply(stateVec[3:7],transition_quat)
return np.array([ [stateVec[0] + (stateVec[7]*del_t)], #0
[stateVec[1] + (stateVec[8]*del_t)], #1
[stateVec[2] + (stateVec[9]*del_t)], #2
[result_quat[0]], #3
[result_quat[1]], #4
[result_quat[2]], #5
[result_quat[3]], #6
[stateVec[7]], #7
[stateVec[8]], #8
[stateVec[9]], #9
[stateVec[10]], #10
[stateVec[11]], #11
[stateVec[12]] ]) #12
def stateVectorUpdate(stateVec, del_t):
transition_quat = getTransitionQuat(stateVec[10:13], del_t)
result_quat = trnsfrm.quaternion_multiply(stateVec[3:7], transition_quat)
return np.array([
[stateVec[0] + (stateVec[7] * del_t)], #0
[stateVec[1] + (stateVec[8] * del_t)], #1
[stateVec[2] + (stateVec[9] * del_t)], #2
[result_quat[0]], #3
[result_quat[1]], #4
[result_quat[2]], #5
[result_quat[3]], #6
[stateVec[7]], #7
[stateVec[8]], #8
[stateVec[9]], #9
[stateVec[10]], #10
[stateVec[11]], #11
[stateVec[12]]
]) #12
