How to get geometry_msgs/Pose from position(x,y,z) like geometry_msgs/Point
Hi, all. Now I got the position (x,y,z) and velocity (vx,vy,vz) of an agent. Now both the positon and velocity are represented as geometry_msg/Point. Now I want to convert thme to geometry_msgs/Pose and geometry_msgs/Twist, respectively. That is, How to get quaternion of Pose from (x,y,z)? How to get linear and angular parts of Twist from (vx,vy,vz)?
Additionally, if Kalman filter or Particle filter are used for tracking the position and velocity of an agent, how to get the covariance of postion and velocity, respectively? With theirs covariance, both geometry_msgs/PoseWithCovariance and geometry_msgs/TwistWithCovariance can be calculated, respectively.
Thank you!
EDIT1: If kalman filter is used to track the movement of an agent, the position and velocity of the agent can be obtained from kalman tracker. Then how to get the covariance of the position from kalman filter? i.e. how to get the value of geometry_msgs/PoseWithCovariance.msg consisting of pose and covariance from the tracked position?
EDIT2: Thank you! From this wiki of kalman filter, is the P_(k/k-1) the covariance of kalman fitler? However, I still wonder how to get the covariance of particle filter.
The covariance is just tracked with a Kalman filter, so just fill a message with that after each step in your Kalman filter.