Sensor Fusion
- Anthony Bertnyk
Software Explanation
Attitude
Attitude and airspeed are estimated from the fusion of IMU and airspeed sensor data. This is done using the Madgwick algorithm available here: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/.
Position
Altitude, latitude, and longitude are estimated from the fusion of altimeter, GPS, and IMU sensor data. This is done using a customized Kalman filter implementation based on the example available here: https://timdelbruegger.wordpress.com/2016/01/05/altitude-sensor-fusion/.
The Kalman filter iteratively estimates data by combining various measurements and the previous estimate. it is made up of two main steps: Time update and measurement update.
Time Update
The vector x lists values to estimate. In this case, there are 6 of these values.
This vector is declared below.
const int16_t DIM = 6;
arm_matrix_instance_f32 x;The vector of the previous estimations is also initialized.
arm_matrix_instance_f32 prevX = iterdata->prevX;For the first part of the time update, a matrix f is multiplied by x. f is defined so that this operation produces an updated vector incorporating any relationships between the elements of x. Due to the laws of physics, there is a relationship between the first 2 elements of x, altitude and vertical speed. Distance=speed*time, so the updated altitude's change should be equal to the vertical speed multiplied by the time since the last update. The same relationship exists between elements 3 and 4, as well as between 5 and 6.
To apply these changes, f is defined as the following matrix:
arm_matrix_instance_f32 f;
float32_t fData[DIM*DIM] =
{
1, dt, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, dt, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, dt,
0, 0, 0, 0, 0, 1
};
arm_mat_init_f32(&f, DIM, DIM, fData);Where dt (delta time) is the time since the previous iteration.
An updated vector is calculated using matrix-vector multiplication.
To obtain the first element, the altitude is multiplied by 1 and added to the product of the vertical speed and the time since the last update. This matches the physical relationship described earlier. All irrelevant elements are multiplied by zero before being added.
The second element, having no physical relationship to take advantage of, is kept the same. Vertical speed is multiplied by 1 while all other values are multiplied by zero. The rest of the elements are calculated similarly to the first 2.
The second part of the prediction step makes use of accelerometer data. A vector u lists acceleration values in three different axes.
A matrix b transforms u to produce a new vector that stores additional changes to x. Acceleration is related to both distance (d=a*t^2/2) and speed (s=a*t), so t can be used to update all six elements of x. To apply these changes, b is defined as the following matrix:
where dt is delta time and ddt is dt^2/2. Through matrix-vector multiplication, all of the predicted changes in position are calculated by multiplying the corresponding acceleration by ddt, while the changes in speed are calculated by multiplying the corresponding acceleration by dt.
The current iteration’s x is updated using the defined matrices according to the formula: x = f*prevX + b*u
Measurement Update
Sensor input that directly corresponds to values being estimated is stored in a vector z.
The Kalman filter uses a matrix h, which is defined to map elements of x to their corresponding elements in z. Multiplying h by x should produce a vector similar to z, but with the values based on the updated vector x from the time update, rather than the sensor measurements. h is defined as follows:
Each sensor measurement has only one element of x mapped to it. Altitude (the first element of x) is mapped to both the altimeter and GPS altitude readings. The three speed values do not directly correspond to a measurement, so they are multiplied by zero. Latitude and longitude are mapped to their corresponding GPS measurements.
These matrices are used by the Kalman filter to update x again with fused sensor data.