Robotics: ROS2

Differential Drive Robot

ROS2 Navigation: Basics

Navigation: Sensors

Mapping and Localization

Navigation: Improved project

Navigation: Examples

Waypoint Follower

Navigating with Keepout Zones

Navigating along given path

Go to battery charger

Dock to battery charger - 1

Dock to battery charger - 2

Dock to battery charger - 3

Multiple Robots

Multiple Robots: Traditional approach

Custom Nav Algorithms

Localization and Custom Path

Lines and Markers

Map Editing Utility

Building 2.5d world

Textured 3d mesh from topo map

Adding roads to 3d mesh

First Person View 2.5D world navigation

Navigation in 2.5D world

Kalman Filter

Using Kalman Filter for ROS2 Localization

Simulation with slippage and drift

Robotic Arm

6 DOF Manipulator and Controllers

Gripper

Fixing Gazebo Gripper

Robotics: Hardware

ESP8266

In this tutorial I am going to fuse all sensors introduced so far, except for odometry, and explain why odometry is an exception.

All theory is based on navigation example, but it can be extended to non-navigation situations - the logic stays the same.

**Why do we need sensor fusion** on the first place?

Because (see earlier section) two sensors can give higher accuracy than one. But this is
not the only reason. Sensors are different, and we can use fusion to compensate
weakness of one type of sensor with strength of another. Also sometimes sensors are not
available: your robot enters a tunnel and GPS is no longer an option. So we need to
adjust.

Traditionally, this is when authors of tutorials mention fusing accelerometer and gyroscope, so that's what I am going to do. Note however, that my primary task is to find coordinates, while orientation is nice but not that important. So I will talk about accel+gyro, but it will not have that much of influence on the later code. It is something one has to know when working with fusion, that's all.

Just keep in mind that this theory I will mostly... not ignore, no. I will work my way around it, so we kind of use it but in a non-obvious way.

Accelerometer is noisy. If you think about its design, it is nothing but a weight on some kind of a spring. You move (accelerate) the sensor, the spring bends, as weight has inertia. Then you stop accelerating it, and spring goes back... passes the equilibrium point... goes further... then back again... And if by that time you accelerate again... it can still move... Also it has internal friction, and so on. And as the result you get noise, a lot of it.

But accelerometer always go to neutral position when not accelerated, which means it has no drift. In other words, it has high frequency noise but no bias.

**Gyroscope** has no high frequency noise. It returns angular velocity, and it has bias, and it
deviates from neutral position... Or should I say it has no neutral position... in theory.
In reality, there are limits to deviation, so do not expect gyro at rest to spin like crazy.

And then we integrate it. And whatever small error gyro had, integration adds it to result.
10 times a second. That means drift (image credits: https://mwrona.com/posts/gyro-noise-analysis/).

**Accelerometer** model is a vector with 3 accelerations:

**(ax, ay, az).T - dv/dt + Wb*v - R(0, 0, g).T + beta(t) + gamma(t)**

Where Wb*v is translation + rotation, R(0, 0, g) is for gravity (gravity is down),
beta is time-varying bias and gamma is zero mean Gaussian noise. In other words, accelerometer
shows linear or rotational acceleration, gravity, random walt bias and zero mean noise.

Axes are usually called yaw, pitch and roll:
**roll(ɸ rotation along X-axis)
pitch(θ rotation along Y-axis)
Yaw(Ѱ rotation along Z-axis)**

If accelerometer is at rest,
**pitch arcsin(ax/g) = arcsin(ax/(sqrt(ax^2 + ay^2 + az^2)))
roll = arctan(ay/az)**

And we should not forget about noise. Initial bias cancellation can be done using calibration,
but it will change with time, so we need to keep making adjustments. One of possible "tricks"
is to make sure sensor is at rest and get few readings to zero the bias, a so-called
'zero velocity update' (ZUPT). For example, if a robot updates its velocity to zero
every time it "knows for sure" that it doesn't move, , the position estimation will remain
precise for a much longer time.

Unfortunately, sometimes (long car ride, sattelite, plane flying across the sea) we simply can not stop.

**Gyro** measures angular rate of rotations around the axes. Our gyro model is
simpler than one for accelerometer:
**w = w_true + beta(t) + gamma(t)**

where beta(t) is time-varying bias and y(t) is zero-mean Gaussian noise.

Let ω_x, ω_y, and ω_z represent the angular speeds measured by the gyroscope along its X, Y, and Z axes. We need the gyroscope's orientation in the world frame to interpret the measured angular speeds. This orientation can be represented by a rotation matrix, denoted as R.

We need to transform rates from body reference frame (p, q, r) to Euler rates in a global frame.
To get the angular speed around roll and pitch, we need to transform the gyroscope measurements
(ω_x, ω_y, ω_z) from the sensor frame to the world frame. This transformation is achieved by
multiplying the element-wise product of the gyroscope data and the rotation matrix:
**ω_world = R * [ω_x, ω_y, ω_z].T**

If you're using Euler angles (roll, pitch, yaw), you can use the following relationships
(assuming a Z-Y-X convention) to extract roll (p) and pitch (q) rates from ω_world:
**p = ω_world[0] + sin(roll) * tan(pitch) * ω_world[1] + cos(roll) * tan(pitch) * ω_world[2]
q = cos(roll) * ω_world[1] - sin(roll) * ω_world[2]**

Now, to perform these transformations, we need to know angles to get p, q, r into inertial frame. Can we simply integrate Euler rates?

Unfortunetely, imperfection of our sensor makes it impossible. It produces strong noise and leads to gyro drift.

An elegant and computationaly easy solution is using Complementary filter. The complementary filter is one of the simplest ways to fuse sensor data from multiple sensors. It is based on the idea that the errors from one sensor will be compensated by the other sensor, and vice versa.

Let's take another look at what we have.

**Accelerometer:** only provides valid measurements when at rest. Has a lot of high-frequency
noise.

**Gyroscope:** only provides valid data over short period of time.

Can we combine these sensors so that their negative sides are compensated?

Let's look at the benefits of each sensor.

**Accelerometer:** good for a long-term compensation (compensates for bias). We can use it to
nullify the gyro drift.

**Gyroscope:** accurate noise-free measurements over short period.

So we use accelerometer while at rest (only sensing gravity). Gather some measurements,
apply digital low-pass filter and end up with final measurement vector
**[ax, ay, az]
φ_acc = arctan(ay/az)
θ_acc = arcsin(ax/g)**

So we have gyro readings in a body frame, and we have accelerometer data;
we transform them to inertial frame and combine with accelerometer:
**φ[n+1] = φ_acc * alpha + (1 - alpha) * (φ[n] + dt * φ_gyro[n])**

and same for θ[n + 1]

Here **alpha** states how much do we trust the accelerometer data. As accelerometer
is noisy, it is here just to compensate the drift of a gyroscope, so alpha is
typically between 1 and 5 percent.

Let me explain the work of Complimentary filter in simple terms. Since gyroscopes are nearly perfect,
**but havs slight drift**, by inputing a little bit of the accelerometer data - just a bit, so
the noise doesn't increase much - into the gyro output, the gyroscope's drift can be compensated.

In a Complimentary Filter above, we chose alpha "just" to make sure it compensate the drift but does not make data too noisy, in other words, we had no justification for a number we selected. Alternatively, we can use Kalman filter to perform same sensor fusion, but this time sensors will be "mixed" in an optimal way.

Let's take a look at a Kalman filter, as an "observer" from a control theory:
**x - Ax + Bv + K(y-Cx)**

where Ax + Bv is a model and K(y-Cx) is a correction term. Here K is chosen at
each step, in an optimal way, so it is a dynamic observer. The alpha of a Complimentary
filter is now replaced with a matrix called **Kalman gain**.

I am not going to provide all Kalman theory here, as it is done in countless sources available online. For those I consider the best, see "links" section below. Instead, let me focus on aspects that are important and usually not obvious.

First of all, Kalman filter works by repeating "predict - update" cycle. Let's say we have accelerometer and gyroscope. Then we can call "predict" every time we receive gyro data and "update" every time we receive accelerometer data. Now, what if we have other sensors, gor example, GPS? And what if we have "input" commands that tell our robot where to go?

"Input" is something that provides info on a state of a system. Angular velocities from gyro sensor can be used by a model to predict state. Or we may have a command sent from ROS2 "teleop" utility to robot's dif. drive. Such command contains linear and angular velocity and can be used to predict robot's state.

The problem arises when we have both gyro data and command: which one should go to "predict" and which one should be used in "update"?

As I understand, there is no universal solution, people do whatever they believe is right, including chaining Kalman filters (one uses one input, another uses another input, and then we combine their outputs). I think this is not a good idea, but we should always keep in mind that there is more than one way to feed data to a filter.

Another non-obvious thing is related to a state - in Kalman sense of this word. If the sensor has drift, then we can not just use its data in a system state vector, because the error (sigma) will become time-dependent. Kalman filter, just to remind you, is based on Markov process model, expecting the current state to depend on the previous state ONLY. No history.

To overcome this problem, bias can be included into the state vector.

The original state vector likely contained the roll (φ), pitch (θ), and yaw (ψ) angles.

We'll augment it with a new element representing the gyro bias (bias_gyro).
The new state vector becomes a 4x1 vector:
**[roll (φ)]
[pitch (θ)]
[yaw (ψ)]
[gyro_bias (b)]**

Update for roll, pitch, and yaw (assuming small angles):
**roll (new) = roll (old) + (unbiased_gyro_x * dt)
pitch (new) = pitch (old) + (unbiased_gyro_y * dt)
yaw (new) = yaw (old) + (unbiased_gyro_z * dt)**

unbiased_gyro_x, unbiased_gyro_y, and unbiased_gyro_z are calculated by subtracting the
estimated bias from the measured gyroscope data:
**unbiased_gyro_x = measured_gyro_x - bias_gyro
unbiased_gyro_y = measured_gyro_y - bias_gyro
unbiased_gyro_z = measured_gyro_z - bias_gyro**

Update for gyro bias (assuming random walk model):
**bias_gyro (new) = bias_gyro (old) + process_noise**

process_noise represents the uncertainty in the bias change and is
typically modeled as zero-mean Gaussian noise.

Measurement Model Update: The accelerometer measurement model relates the predicted
orientation to the measured acceleration. We can use a simplified model for small angles
(just an example):
**accelerometer_x = g * (sin(pitch) + cos(roll) * tan(pitch))
accelerometer_y = g * (-cos(roll) * sin(pitch) + cos(pitch))
accelerometer_z = g * (cos(roll) * cos(pitch))**

By incorporating the gyro bias into the UKF and adjusting the process and measurement models, you can compensate for the bias.

Gravity points down, therefore we can turn our robot around Z axe, and accelerometer will not notice. Sorry, it will notice, while rotation happens, but will not be able to provide us with x, y orientation in an unbiased time-stable way.

Ok, that was unclear. Let's ask ourselves a simple question: what do we want the accelerometer to show when robot spins in place?

Nothing!

Because for rotation info, we have gyroscope, and it would be much nicer if accelerometer data are not contaminated with acceleration resulting from rotation.

This is why the gyro and accelerometer should be mounted as close as possible to the center
of the robot, since it measures rotation around the axis perpendicular to its board (gyro)
and *linear* acceleration (accel).

Back to the topic. We need another external reference, which is not aligned with Z axe, Earth's magnetic field is a good candidate.

Of course, magnetometer has both advantages and disadvantages. An advantage is, the direction to a North pole is not affected by acceleration, so we don't have to stop to measure it. A disadvantage is, unlike with gravity, magnetic field can be altered by both natural and artifficial magnetic sources.

There are two types of inference: hard iron abd soft iron ones. Hard iron source
generates its own magnetic field. So if we rotate our robot 360 degrees in absence of any
sources of magnetic fields except for Earth's magnetism, we will get chart of magnetic
field strength as a circle, with center being at 0,0.

If we add a hard iron source of a magnetic field and perform the same measurement,
we will get a circle, shifhed from 0, 0.

As for soft source, it does not generate a field of its own, but it can pull in existing magnetic lines. If we rotate a magnetometer in presence of such a source, we will gett an ellipse, instead of a circle.

Good news is, if these sources are not moving around, we can measure them in advance and compensate for them.
It does not matter if the source is part of our robot or if it is an external one (of course, as we move
away from the source, its influence will drop, but it still can be calibrated).
**x_corrected = (x-b_hard) * A_soft**

here b_hard is a 3x1 vector and A_soft is a 3x3 matrix.

By the way, this is exactly what your phone does when it asks you to turn the phone in different directions: it calibrates itself to compensate for magnetic fields of your car, house and keys in your pocket (the last one will probably fail).

Also, it is a good idea to spend few seconds at startup to perform calibration. When the robot starts up the gyro runs a bias measurement and calculation. During this time the robot must be absolutely still.

(C) snowcron.com, all rights reserved