Sensor fusion with Kalman filter: what should go as "command" in case we have a choice?

Question

Let's say I have a robot with few sensors and dif. drive. And I want to perform sensor fusion using UKF.

Dif. drive can take commands, so my first thought was to use command (linear and angular speed) as filter's "control command". By "command" here I mean something that I send to dif. drive to make it move, like they do in ROS teleop utility. It means that when I run prediction, I use these parameters. But then I found few publications, like this: https://nitinjsanket.github.io/tutorials/attitudeest/kf

It merges gyro and accelerometer, and it runs "predict" every time gyro data received, and "update" every time accelerometer data received.

Which is understandable: 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"?

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

So my question is, are there any rules to choose what is used during "predict" and what - during "update"? And does it mater?

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?

As you can see, I am pretty much lost here, so any pointers are greatly appreciated. The objective is to use UKF to merge GPS/accel/gyro/magnetometer/... in a way that makes me understand what and why I am doing.

Thanks :)

Answer 1

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.

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:

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.