ekf.py 文件源码

def ekf_pub(self, ranges, vel_data, yaw, alt):
        z = np.array([])
        new_pose = Odometry()
        ps_cov = PoseWithCovarianceStamped()
        for tag_name in self.tag_order:
            measurement = ranges[tag_name]
            z = np.append(z, measurement)

        if self.last_time is None:
            self.last_time = rospy.Time.now().to_sec()
        else:
            dt = rospy.Time.now().to_sec() - self.last_time
            self.predict(dt)
            self.update(z, vel_data, yaw, alt)
            self.last_time = rospy.Time.now().to_sec()

            new_pose.header.stamp = rospy.get_rostime()
            new_pose.header.frame_id = self.frame_id
            new_pose.pose.pose.position.x = self.x[0]
            new_pose.pose.pose.position.y = self.x[1]
            new_pose.pose.pose.position.z = self.x[2]
            cov = self.P.flatten().tolist()
            new_pose.pose.covariance = cov
            new_pose.twist.twist.linear.x = self.x[3]
            new_pose.twist.twist.linear.y = self.x[4]
            new_pose.twist.twist.linear.z = self.x[5]

        return new_pose

    # @n.publisher(EKF_COV_TOPIC, PoseWithCovarianceStamped)
    # def cov_pub(self, ps_cov):
    #     return ps_cov