complete imu post

collections/projects/Arduino Libraries/_posts/2020-03-14-imu_sensor_fusion.md

@@ -4,22 +4,22 @@ title: "Review | Sensor fusion algorithm for an IMU based on a quaternion"
 ---
 
 # Introduction 
-In the process of developing an [autonomous tracked robot](/projects/robots/2020/05/10/autonomous_tracked_vehicle.html), it was deemed critical to create a system to track the vehicle's heading. This involved combining information from an inertial measurement unit (IMU) into a useful heading estimate. The process of combining this information is called sensor fusion, which can be accomplished through a properly designed algorithm. At the core of the algorithm is a mechanism to encode the sensor's orientation. This can be done using rotation matrices or quaternions. In this case, using a quaternion is preferable 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 using just four numbers. This simplicity makes it well-suited for an algorithm that needs to be executed continuously, as it is computationally inexpensive to repeat any calculations.
+In the process of developing an [autonomous tracked robot](/projects/robots/2020/05/10/autonomous_tracked_vehicle.html), it was deemed critical to create a system to track the vehicle's heading. This involved combining information from an inertial measurement unit (IMU) into a useful heading estimate. The process of combining this information is called [sensor fusion](https://en.wikipedia.org/wiki/Sensor_fusion), which can be accomplished through a properly designed algorithm. At the core of the algorithm is a mechanism to encode the sensor's orientation. This can be done using [rotation matrices](https://en.wikipedia.org/wiki/Rotation_matrix) or [quaternions](https://en.wikipedia.org/wiki/Quaternion). In this case, using a quaternion is preferable 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 using just four numbers. This simplicity makes it well-suited for an algorithm that needs to be executed continuously, as it is computationally inexpensive to repeat any calculations.
 
-A separate vector and quaternion library was developed to handle the mathematics involved in manipulating quaternions and any vectors that the quaternions would act upon. These data types were then used by the sensor fusion algorithm to perform heading computations. The fusion algorithm was extensively tested using an MPU-6050 inertial measurement unit. Another library was specifically developed to interface the MPU-6050 with an Arduino. Thus, the sensor fusion algorithm depended on two custom libraries to create a functioning system. We can consider this system to be a filter that acts on the raw data from the sensor. Therefore, the sensor fusion algorithm can also be referred to as an IMU filter, as it filters the information from the inertial measurement unit.
+A separate vector and quaternion library was developed to handle the mathematics involved in manipulating quaternions and any vectors that the quaternions would act upon. These data types were then used by the sensor fusion algorithm to perform heading computations. The fusion algorithm was extensively tested using an **MPU-6050** [inertial measurement unit](https://en.wikipedia.org/wiki/Inertial_measurement_unit). Another library was specifically developed to interface the MPU-6050 with an Arduino. Thus, the sensor fusion algorithm depended on two custom libraries to create a functioning system. We can consider this system to be a filter that acts on the raw data from the sensor. Therefore, the sensor fusion algorithm can also be referred to as an **IMU filter**, as it filters the information from the inertial measurement unit.
 
 **Note**: The auxiliary libraries developed for the IMU filter can be found here:
 - [MPU-6050 library](/projects/arduino libraries/2020/07/11/mpu6050.html)
 - [Vector and Quaternion library](/projects/arduino libraries/2020/09/20/3d_datatypes.html)
 
 # Initial PID-Based Filter 
-The initial form of the fusion algorithm was based on existing IMU filters. It was a **modified** version of the _Mahony filter_ that replaces the PI controller with something akin to a second-order low-pass filter. The proportional term was removed, and the integral term was forced to decay to dampen the system. For more information on the Mahony filter, see these references:
+The initial form of the fusion algorithm was based on existing IMU filters. It was a **modified** version of the _Mahony filter_ that replaces the PI controller with something akin to a [second-order low-pass filter](https://www.electronics-tutorials.ws/filter/second-order-filters.html). The proportional term was removed, and the integral term was forced to decay to dampen the system. For more information on the Mahony filter, see these references:
 - [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
 - [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)
 
 The correction steps for each filter are shown below:
 
-- **Mahony filter:** In this algorithm, the heading error is directly used to determine the heading correction. The error in heading is also integrated, and the result is included in the correction for the heading. As a result, the system behaves like a first-order low-pass filter, with a small amount of rebound due to the integral term winding up. However, because of the existence of a proportional term in the heading correction, the system responds immediately to the existence of any error. This rapid response reduces the filter's ability to suppress noise.
+- **Mahony filter:** In this algorithm, the heading error is directly used to determine the heading correction. The error in heading is also integrated, and the result is included in the correction for the heading. As a result, the system behaves like a [first-order low-pass filter](https://www.electronics-tutorials.ws/filter/filter_2.html), with a small amount of rebound due to the integral term [winding up](https://en.wikipedia.org/wiki/Integral_windup). However, because of the existence of a proportional term in the heading correction, the system responds immediately to the existence of any error. This rapid response reduces the filter's ability to suppress noise.
 
 {%include image.html src="/img/imu-filter/equations1/equation1.png" %}
 {%include image.html src="/img/imu-filter/equations1/equation2.png" %}
@@ -47,7 +47,7 @@ The behavior of the modified filter is analogous to a spring-mass system. (**Kp*
 Since a 6-axis IMU has no absolute reference for heading, a function was included to rotate the orientation estimate about the yaw axis. Basic vector operations were included to easily implement a heading correction algorithm if an additional sensor (such as a magnetometer or another absolute heading sensor) is available.
 
 # Updated Kalman-like Filter
-The sensor fusion algorithm was eventually rewritten with a completely different approach. Instead of developing an arbitrary system that mimicked a mass attached to a spring, it was more effective to consider the statistical noise of the gyroscope and accelerometer. This approach is taken by the analytically derived Kalman filter. In particular, I drew inspiration from the implementation of a [1D Kalman filter](https://github.com/denyssene/SimpleKalmanFilter) for Arduino.
+The sensor fusion algorithm was eventually rewritten with a completely different approach. Instead of developing an arbitrary system that mimicked a mass attached to a spring, it was more effective to consider the statistical noise of the gyroscope and accelerometer. This approach is taken by the analytically derived [Kalman filter](https://en.wikipedia.org/wiki/Kalman_filter). In particular, I drew inspiration from the implementation of a [1D Kalman filter](https://github.com/denyssene/SimpleKalmanFilter) for Arduino.
 
 #### Heading Estimate:
 The updated algorithm used a _Kalman-like_ filter to check the whether the acceleration was within a specific deviation from **(0,0,1)**g. If the acceleration was within this band, it will strongly correct the orientation. However, if the acceleration lied outside this band, it would have little effect on the orientation. To this end, the deviation from vertical was used to update the variance and Kalman gain: