You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
In the process of developing an [autonomous tracked robot](/projects/robots/2020/05/10/autonomous_tracked_vehicle.html), it was deemed critical to develop a system to keep track of the heading of the vehicle.
8
-
This entailed using something to combine the information from an inertial measurement unit into a useful heading estimate.
9
-
The process of combining this information is called sensor fusion, and this could be accomplished through a properly designed algorithm.
10
-
At the core of the algorithm would have to lie some mechanism to encode the orientation of the sensor.
11
-
This can be through the use of rotation matrices, or alternatively, by using a quaternion.
12
-
In this particular case, it goes to use a quaternion because it is mathematically simpler than a rotation matrix.
13
-
It does not require extensive use of sine and cosine functions, and all of the heading can be encoded through four individual numbers.
14
-
This simplicity meant that it was well suited for an algorithm that had to be constantly executed, as it was computationally inexpensive to repeat any quaternion computations.
15
-
16
-
A separate [vector and quaternion library](/projects/arduino libraries/2020/09/20/3d_datatypes.html) was developed to handle the math behind manipulating quaternians and any vectors that the quaternians would act upon.
17
-
These data types were then used by the sensor fusion algorithm to perform its heading computations.
18
-
The fusion algorithm was then extensively tested through a MPU-6050 inertial measurement unit.
19
-
Yet another library was especially developed to interface the [MPU-6050](/projects/arduino libraries/2020/07/11/mpu6050.html) with an Arduino.
20
-
Therefore, the sensor fusion algorithm depended on two custom libraries to make a working system.
21
-
We can consider this system to be a filter that acts upon the raw data of the sensor. Therefore, the sensor fusion algorithm can also be called a IMU filter as it filters the information of the inertial measurement unit.
10
+
In the process of developing an autonomous tracked robot, it was deemed critical to develop a system to keep track of the heading of the vehicle. This entailed using something to combine the information from an inertial measurement unit into a useful heading estimate. The process of combining this information is called sensor fusion, and this could be accomplished through a properly designed algorithm. At the core of the algorithm would be a mechanism to encode the orientation of the sensor. This can be through the use of rotation matrices or through a quaternion. In this particular case, it is best to use a quaternion because it is mathematically simpler than a rotation matrix. It does not require extensive use of sine and cosine functions, and all of the heading can be encoded through four individual numbers. This simplicity meant that it was well suited for an algorithm that had to be constantly executed, as it was computationally inexpensive to repeat any computations.
11
+
12
+
A separate vector and quaternion library was developed to handle the math behind manipulating quaternians and any vectors that the quaternians would act upon. These data types were then used by the sensor fusion algorithm to perform its heading computations. The fusion algorithm was then extensively tested through a MPU-6050 inertial measurement unit. Yet another library was especially developed to interface the MPU-6050 with an Arduino. Therefore, the sensor fusion algorithm depended on two custom libraries to make a working system. We can consider this system to be a filter that acts upon the raw data of the sensor. Therefore, the sensor fusion algorithm can also be called a IMU filter as it filters the information of the inertial measurement unit.
22
13
23
14
24
15
# Initial PID-based filter
@@ -53,16 +44,14 @@ The behavior of the modified filter is analogous to spring-mass system. Kp (stif
53
44
- Get unit vectors of the X, Y and Z axes in the local or global frame.
54
45
55
46
<br>
56
-
Since a 6-axis IMU has no absolute reference for heading there is a function to rotate the orientation estimate about the yaw axis. Basic vector operations have been included to easily implement a heading correction algorithm should one have an additional sensor (such a magnetometer or some other absolute heading sensor).
47
+
Since a 6-axis IMU has no absolute reference for heading, there is a function to rotate the orientation estimate about the yaw axis. Basic vector operations were included to easily implement a heading correction algorithm should one have an additional sensor (such a magnetometer or some other absolute heading sensor).
57
48
58
49
59
50
# Updated Kalman-like filter
60
-
The sensor fusion algorithm was eventually rewritten by considering a completely different approach to the problem.
61
-
Rather than developing an arbitrary system that behaved similarly to a mass attached to a spring, it was better to consider the statistical noise in the gyroscope and accelerometer embedded in the IMU.
62
-
This is the approach taken by the analytically derived kalman filter. In particular, I took inspiration from the implementation of a [1D Kalman filter](https://github.com/denyssene/SimpleKalmanFilter) for arduino.
51
+
The sensor fusion algorithm was eventually rewritten by considering a completely different approach to the problem. Rather than developing an arbitrary system that behaved similarly to a mass attached to a spring, it was better to consider the statistical noise of the gyroscope and accelerometer. This is the approach taken by the analytically derived kalman filter. In particular, I took inspiration from the implementation of a [1D Kalman filter](https://github.com/denyssene/SimpleKalmanFilter) for arduino.
63
52
64
-
#### - Heading estimate
65
-
It uses a _kalman-like_ filter to check the acceleration and see if it lies within a deviation from (0,0,1)g. If the acceleration is within this band, it will strongly correct the orientation. However, if the acceleration lies outside of this band, it will barely affect the orientation. To this end, the deviation from vertical is used to update the variance and kalman gain:
53
+
#### Heading estimate:
54
+
The sensor fusion algorithm uses a _kalman-like_ filter to check the acceleration and see if it lies within a deviation from (0,0,1)g. If the acceleration is within this band, it will strongly correct the orientation. However, if the acceleration lies outside of this band, it will barely affect the orientation. To this end, the deviation from vertical is used to update the variance and kalman gain:
As the filter uses a quaternion to encode rotations, it's easy to perform coordinate transformations. The library has functions to:
78
-
- Transfor a vector to the local or global frame.
79
-
- Get the unit vectors of the X, Y and Z axes in the local or global frame.
66
+
This approach of using the acceleration's magnitude to determine when to use the vector to correct the heading was very effective.
67
+
It ensured that the heading would not be disturbed by sudden or prolonged rapid accelerations in any direction.
68
+
Only situations where the accelerometer was in the acceptable deadband, did the heading get corrected. These only occurred when the accelerometer was either at rest or in a state of constant linear motion.
69
+
This is a much better approach than the initial version of the IMU filter that depended upon an arbitrary spring mass system to combine the accelerometer and gyroscope rating. In this case, the accelerometer would always correct indifferent of whether it was true or not the heading of the heading of the estimate.
80
70
81
-
Moreover, since a 6-axis IMU (gyro-accelerometer) cannot measure an absolute heading, a function is included to rotate the orientation about the vertical (yaw) axis. One can use vector operations to correct the heading with an additional sensor like a magnetometer.
82
71
83
-
#### - Velocity estimate
72
+
#### Velocity estimate:
84
73
The library can integrate acceleration to obtain an estimate of velocity. This is accomplished by using a set of Kalman-like filters like the one shown above. The acceleration is checked against a rest condition of (0,0,0)g and any deviations that lie within a specified uncertainty band are suppressed. This eliminates much of the bias due to gravity or miscalibration:
This approach led to a fairly decent estimation of velocity for short periods of time.
91
+
The sensor would have to rapidly accelerate, then decelerate to get a accurate reading. If the operation was very prolonged or if the acceleration only occurred in one direction then did not reverse, the output of this algorithm would decay very rapidly to whatever target value was established.
92
+
Therefore, it was only well suited for short-term disturbances rather than large, long-term readings of velocity. For that type of information, another sensor would need to be used.
101
93
102
-
# Conclusions
103
-
The IMU filter was a very challenging project.
104
-
It required developing additional libraries which were projects in of their own.
105
-
Not only this, but developing a robust algorithm that could accurately determine heading, while not being distorted by unnecessary noise in the acceleration that was being measured was also challenging.
106
-
It required careful analysis of the readings of the accelerometer to understand when it was prudent to use its data and when it was not.
107
-
This complexity and difficulty was especially true when trying to convert the accelerometer values into useful estimates of velocity and position.
108
-
Just getting a stable reading of velocity was very difficult as integrating the acceleration directly leads to massive instability.
109
-
Therefore, whatever algorithm handled the integration of acceleration had to simultaneously rapidly suppress any deviations in the velocity estimate due to acceleration, but at the same time be responsive to any sudden changes in the acceleration to know when to integrate it.
110
94
95
+
# Conclusions
96
+
The IMU filter was a very challenging project. It required developing additional libraries which were projects in of their own. Not only this, but developing a robust algorithm that could accurately determine heading, while not being distorted by unnecessary noise in the acceleration that was being measured was also challenging. It required careful analysis of the readings of the accelerometer to understand when it was prudent to use its data and when it was not. This complexity and difficulty was especially true when trying to convert the accelerometer values into useful estimates of velocity and position. Just getting a stable reading of velocity was very difficult as integrating the acceleration directly leads to massive instability. Therefore, whatever algorithm handled the integration of acceleration had to simultaneously rapidly suppress any deviations in the velocity estimate due to acceleration, but at the same time be responsive to any sudden changes in the acceleration to know when to integrate it.
111
97
112
-
In the end, the Kalman-like system that was devised may have not been the best algorithm to obtain velocity estimates, but it worked, and it was simple. After a point, it became clear that there are limitations to what an algorithm can do if the sensor behind it is simply not very accurate. Evidently, some of this was the case with the MPU 60-50 since it's a very cheap and inexpensive sensor.
113
-
If a much more expensive IMU was used with the filter, it's very possible that the velocity estimates would be much better. Notwithstanding any limitations of inherent in the sensors, it's also possible to use completely different approaches to estimate velocity and orientation. For example, it's possible to use neural networks to provide very strong estimates based on the precise analysis of a lot of data correlating it with known displacements.
98
+
In the end, the Kalman-like system that was devised may have not been the best algorithm to obtain velocity estimates, but it worked, and it was simple. After a point, it became clear that there are limitations to what an algorithm can do if the sensor behind it is simply not very accurate. Evidently, some of this was the case with the MPU 60-50 since it's a very cheap and inexpensive sensor. If a much more expensive IMU was used with the filter, it's very possible that the velocity estimates would be much better. Notwithstanding any limitations of inherent in the sensors, it's also possible to use completely different approaches to estimate velocity and orientation. For example, it's possible to use neural networks to provide very strong estimates based on the precise analysis of a lot of data correlating it with known displacements.
0 commit comments