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()
Exemple #2
0
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()