Sensor Fusion Theory

Accelerometer vs Gyroscope

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.

For example, if, in terrestrial use, the inertially tracked velocity is intermittently updated to zero by stopping, the position will remain precise for a much longer time, a so-called ''zero velocity update'' (ZUPT).

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.

Complimentary filter

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.

Kalman Filter

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.

Magnetometer and calibration

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.