def kalman_calc(event): global gyroAngVel, odoAngVel, x, P, dt, lastInterruptTime, gyroBelieve, odoBelieve gyroAngVel = drift_correction(gyroAngVel) temp_mes = [[gyroAngVel, odoAngVel]] update_Believes() dt = event.current_real.to_sec() - lastInterruptTime x, P = filter(x, P, temp_mes) # Publish Message pub_msg = kalman_output() pub_msg.header.stamp = rospy.get_rostime() pub_msg.yaw = x.value[0][0] % (pi * 2) pub_msg.ang_vel = x.value[1][0] pub_msg.ang_vel2 = x.value[2][0] global pub, odo2 if odo2: rotationQuaternion = tf.transformations.quaternion_from_euler(0, 0, -1 * ((x.value[0][0] - pi) % (pi * 2))) tfbr = tf.TransformBroadcaster() tfbr.sendTransform( (odo2.pose.pose.position.x, odo2.pose.pose.position.y, odo2.pose.pose.position.z), rotationQuaternion, rospy.Time.now(), "base_link", "odom", ) pub_msg = odo2 pub_msg.pose.pose.orientation.x = rotationQuaternion[0] pub_msg.pose.pose.orientation.y = rotationQuaternion[1] pub_msg.pose.pose.orientation.z = rotationQuaternion[2] pub_msg.pose.pose.orientation.w = rotationQuaternion[3] pub.publish(pub_msg) lastInterruptTime = event.current_real.to_sec()
def kalman_calc(event): global gyroAngVel, odoAngVel, x, P, dt, lastInterruptTime, gyroBelieve, odoBelieve gyroAngVel = drift_correction(gyroAngVel) temp_mes = [[gyroAngVel,odoAngVel]] update_Believes() dt = (event.current_real.to_sec()- lastInterruptTime) x, P = filter(x, P, temp_mes) #Publish Message pub_msg = kalman_output() pub_msg.header.stamp = rospy.get_rostime() pub_msg.yaw = x.value[0][0] % (pi * 2) pub_msg.ang_vel = x.value[1][0] pub_msg.ang_vel2 = x.value[2][0] global pub pub.publish(pub_msg) lastInterruptTime = event.current_real.to_sec()