def mahoneyEstimator():
global maxIt
rospy.init_node('mahoneyEstimator', anonymous=True)
i=0
j=0
acc=np.zeros(3)
mag=np.zeros(3)
ang_vel=np.zeros(3)
accel_avg=np.zeros((maxIt,3))
angvel_avg=np.zeros((maxIt,3))
mag_avg=np.zeros((maxIt,3))
args_imu=[acc,ang_vel,i,accel_avg,angvel_avg]
args_mag=[mag,j,mag_avg]
rospy.loginfo("Running, with max it: %d", maxIt)
rospy.Subscriber("/imu/data_raw",Imu,imu_measurement,args_imu)
rospy.Subscriber("/imu/mag",MagneticField,mag_measurement,args_mag)
pub=rospy.Publisher('mahoneyAtt',rpy, queue_size=10)
r=rospy.Rate(1000)
Cea=np.eye(3)
bhat=np.matrix('0;0;0')
now=rospy.get_time()
while not rospy.is_shutdown():
if args_imu[2]% maxIt ==0:
acc=args_imu[3].mean(axis=0)
w_y_a=args_imu[4].mean(axis=0)
mag=args_mag[2].mean(axis=0)
if np.linalg.norm(mag,2) != 0 and np.linalg.norm(acc,2) != 0 and np.linalg.norm(w_y_a,2) !=0:
Cba=rm.triad(acc,mag)
derivatives=rm.mahoneyPoisson(Cea, Cba, bhat, w_y_a)
before=now
now=rospy.get_time()
dT=now-before
Cea=Cea+derivatives.Cdot*dT
bhat=bhat+derivatives.bdot*dT
rllptchyw=rm.RPYfromC(Cea)
for i in range(3):
rllptchyw[i]=rllptchyw[i]*180./m.pi
pub.publish(roll=rllptchyw[0],pitch=rllptchyw[1],yaw=rllptchyw[2])
r.sleep()
def triadEstimation():
global maxIt
rospy.init_node('triadEstimation', anonymous=True)
i=0
j=0
acc=np.zeros(3)
mag=np.zeros(3)
accel_avg=np.zeros((maxIt,3))
mag_avg=np.zeros((maxIt,3))
args_accel=[acc,i,accel_avg]
args_mag=[mag,j,mag_avg]
rospy.loginfo("Running, with max it: %d", maxIt)
rospy.Subscriber("/imu/data_raw",Imu,accel_measurement,args_accel)
rospy.Subscriber("/imu/mag",MagneticField,mag_measurement,args_mag)
pub=rospy.Publisher('triadAtt',rpy, queue_size=10)
r=rospy.Rate(1000)
while not rospy.is_shutdown():
if args_accel[1]% maxIt ==0:
acc=args_accel[2].mean(axis=0)
mag=args_mag[2].mean(axis=0)
if np.linalg.norm(mag,2) != 0 and np.linalg.norm(acc,2) != 0:
Cinst=rm.triad(acc,mag)
rllptchyw=rm.RPYfromC(Cinst)
for i in range(3):
rllptchyw[i]=rllptchyw[i]*180./m.pi
pub.publish(roll=rllptchyw[0],pitch=rllptchyw[1],yaw=rllptchyw[2])
r.sleep()
