Python quatMultiplyNorm Examples

Example #1

def update_quaternion(delta_x_k1k, q_kk):
    delta_q = np.hstack((delta_x_k1k[0:3] / 2, np.array([0])))
    #print("delta_x_kk=")
    #print(delta_x_k1k)
    #print("q_kk=")
    #print(q_kk)
    return qnv.quatMultiplyNorm(delta_q, q_kk)

Example #2

def qBO2qBI(v_q_BO, v_pos_i, v_vel_i):
    #input: Unit quaternion which rotates orbit frame vector to body frame, position and velocity in ecif
    #output: Unit quaternion which rotates ecif vector to body frame
    #Orbit frame def:	z_o = nadir(opposite to satellite position vector) y_o: cross(v,r) x_o: cross(y,z)
    z = -v_pos_i / np.linalg.norm(v_pos_i)
    y = np.cross(v_vel_i, v_pos_i) / np.linalg.norm(np.cross(v_vel_i, v_pos_i))
    x = np.cross(y, z) / np.linalg.norm(np.cross(y, z))
    m_DCM_OI = np.array([x, y, z])

    v_q_OI = qnv.rotm2quat(m_DCM_OI)
    v_q_BI = qnv.quatMultiplyNorm(v_q_BO, v_q_OI)
    if v_q_BI[3] < 0.:
        v_q_BI = -1. * v_q_BI.copy()
    v_qBI = v_q_BI / np.linalg.norm(v_q_BI.copy())
    return v_q_BI

Example #3

File: test_default_blocks.py Project: rsriya/Advitiy-ADCS-CLS

	def test_magmeter(self,value): 
		qBI = np.asarray(value[0])
		v_B_i=np.asarray(value[1])
		v_pos_i = np.array([1e6,-2.03,-3.0])
		v_vel_i = np.array([2.0e3,2.8,-73.2])
		qBO = frames.qBI2qBO(qBI,v_pos_i,v_vel_i)
		state = np.hstack((qBO,np.zeros(3)))
		mySat = satellite.Satellite(state,12.0)
		mySat.setMag_i(v_B_i)
		mySat.setPos(v_pos_i)
		mySat.setVel(v_vel_i)
		qOI = qnv.quatMultiplyNorm(qnv.quatInv(qBO),qBI)
		result = defblock.magnetometer(mySat)
		v_expected = value[2];
		self.assertTrue(np.allclose(result,v_expected))

Example #4

File: frames.py Project: rsriya/Advitiy-ADCS-CLS

def qBO2qBI(v_q_BO,v_pos_i,v_vel_i):
	#input: Quaternion corresponding to transformation of components of vector in orbit frame to components in body frame, 
	#		position and velocity in ecif
	#output: Quaternion corresponding to transformation of components of vector in ecif to components in body frame
	#Orbit frame def:	z_o = nadir(opposite to satellite position vector) y_o: cross(v,r) x_o: cross(y,z)
	z = -v_pos_i/np.linalg.norm(v_pos_i)
	y = np.cross(v_vel_i,v_pos_i)/np.linalg.norm(np.cross(v_vel_i,v_pos_i))
	x = np.cross(y,z)/np.linalg.norm(np.cross(y,z))
	m_DCM_OI = np.array([x,y,z])

	v_q_OI = qnv.rotm2quat(m_DCM_OI)
	v_q_BI = qnv.quatMultiplyNorm(v_q_BO,v_q_OI)
	if v_q_BI[3] < 0. :
		v_q_BI = -1.*v_q_BI.copy()
	v_qBI = v_q_BI/np.linalg.norm(v_q_BI.copy())	
	return v_q_BI

Example #5

File: test_qnv.py Project: sumitPhD/Advitiy-Control-Model

	print ("error for quatInv")
 
flag=1 # testing for quatMultiplynorm
m1 = np.array([1,-1,-1,1])
m2 = np.array([-1,1,-1,-1])
m3 = np.array([0,1,1,-1])
m4 = np.array([0,1,-1,0])
m5 = np.array([0,0,-1,1])
m6 = np.array([0,4,0,0])
m7 = np.array([-1,-3,-1,-1])
m8 = np.array([-1,-1,-1,-1])
m9 = m6/np.linalg.norm(m6)
m10 = m7/np.linalg.norm(m7)
m11 = m8/np.linalg.norm(m8)

G=qnv.quatMultiplyNorm(m1,m2)
if (G == m9).all() == 0:
	flag=0

G=qnv.quatMultiplyNorm(m3,m2)
if (G == m10).all() == 0:
	flag=0

G=qnv.quatMultiplyNorm(m4,m5)
if (G == m11).all() == 0:
	flag=0

if flag == 1:
	print ("all cases passed for quatMultiplynorm")
else:
	print ("error for quatMultiplynorm")

Example #6

File: MEKF2019.py Project: Djay1364/Advitiy_estimator

def update_quaternion(phi, delta_x_k1k, q_kk):
    delta_q = (delta_x_k1k[0:3], 0)
    return qnv.quatMultiplyNorm(delta_q, q_kk)