Skip to main content
2 of 2
added 1315 characters in body
Scoeerg
  • 225
  • 6

First of all, usually the "command" to give to any robot is a publisher geometry_msgs/msg/Twist

geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

OR geometry_msgs/msg/TwistStamped

std_msgs/msg/Header header
geometry_msgs/msg/Twist twist

which essentially tells to robot to follow a reference velocity in all 6 degrees of freedom:

$\vec{v}_{linear} = \left(\dot{x}~\dot{y}~\dot{z} \right)^T$ $\vec{v}_{angular} = \left(\dot{roll}~\dot{pitch}~\dot{yaw} \right)^T$

any rigid body has. The ROS teleop keyboard for example publishes Twist-Messages. Convention(!) is the topic name cmd_vel for this Twist or StampedTwist message.

Now of course, you need to understand this is the reference(!) to a low-level controller, which will try and follow this reference velocity. So, the actual robot might move differently (due to control errors or kinematic constraints, especially non-holonomic constraints).

What is usually fancifully called Sensor Fusion is a simple State Observer of which there are a couple of well-known ones: The Luenberger/State Observer which is described in the linked Wiki above, and three Kalman Filter:

Standard Kalman
Extended Kalman (EKF)
Un-Scented Kalman (UKF)

which essentially all work by correcting prediction error. The scope of the answer is too small to go into details, but I invite you to read about those concepts in the provided links, as they will clear things up.

Now this is a scheme of any observer.

Observer Scheme

Here's a translation in "Robotic Terms":

Plant = Robot
Reference Signal = Twist/StampedTwist
Controller = Low-Level Controller
Output = Sensor Data (Gyro, IMU etc.)
Observer = Observer (Kalman, Luenberger etc.)

The output of your Observer is the State Estimate - which usually for robots means the estimated current position and velocity (relative to a fixed frame - usually called odom).

I sincerely hope this clears it all up. And here's what it looks like "inside" the Kalman Filter:

enter image description here

which - again - minimizes prediction error by correcting iteratively.

Let me try and answer some of your sub-questions:

they have gyro and they want accelerometer to comply (am I correct?) It can be done in case there is no "control command". But what if I have it? What if I have few sensors and "command"?

usually you can set a bool whether the Observer should use command or only sensor data to estimate states.

Even more, what if I have few sensors each of them can be used as input, for example, two different gyroscopes?

Standard Filters as used in localization package can fuse as many IMUs etc as you want. You only have to comply to localization package message type.

Second question: in Complimentary filter, I can choose alpha, so that accelerometer is, say, 1% and gyro 99%. How can I do it in UKF? I don't think that I can alter values in Q or P matrices, because a) they are related to properties of sensors and b) it makes no sense. Say, gyro has sigma 0.01 and accelerometer 0.01, yet one will drift, so sigma should grow?

I think(!) what you are asking is how to (fine) tune Kalman-Filters. You can set the process and (I think) sensor noise inside the parameters of the Observers as Covariance-Matrices. BUT(!) it's advised to do this carefully - it's not an easy task and standard-values are usually pretty good. This being said, the Covariance-Matrices (your "sigma") are updated periodically within the Kalman-Filter as it works - so they can and will drift.

Scoeerg
  • 225
  • 6