Repository: bjohnsonfl/Madgwick_Filter Branch: master Commit: e01de6ba4eb5 Files: 7 Total size: 14.9 KB Directory structure: gitextract_fyffrvus/ ├── .gitignore ├── CMakeLists.txt ├── License.md ├── README.md ├── madgwickFilter.c ├── madgwickFilter.h └── main.c ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ # CMake Build Directory **/build ================================================ FILE: CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.11) project(Madgwick_Filter) # Should be in a subdirectory add_library(madgwick SHARED madgwickFilter.c) target_include_directories(madgwick PUBLIC ${PROJECT_SOURCE_DIR}/) add_executable( ${PROJECT_NAME} main.c) target_link_libraries( ${PROJECT_NAME} madgwick m) ================================================ FILE: License.md ================================================ MIT License Copyright (c) 2020, 2022 Blake Johnson Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ # Madgwick Filter The paper can be found [here](https://courses.cs.washington.edu/courses/cse474/17wi/labs/l4/madgwick_internal_report.pdf). My explanation of the filter can be found on my website [here](https://blakejohnsonuf.com/project/#IMU), just scroll down to the Madgwick Filter section. ## Brief Description An IMU is a sensor suite complete with an accelerometer and gyroscope. A MARG contains an IMU plus a Magnetometer. All three of these sensors measure physical qualities of Earth's fields or orientation due to angular momentum. Alone, these sensors have faults thats that the other sensors can make up for. One problem is when a sensor has an axis aligned with Earth's field which prevents using trig functions to determine orientation due to tan(90) being undefined. The fusion of sensors is critical for accuracy of orientation and for a complete transformation from the sensor's inertial frame to the Earth frame. The Madgwick Filter fuses the IMU and optonally the MARG. It does this by using gradient descent to optimize a Quaternion that orients accelerometer data to a known reference of gravity. This quaternion is weighted and integrated with the gyroscope quaternion and previous orientation. This result is normalized and and converted to Euler angles. A Quaternion is a 4 dimensional number, an extension of the complex plane. It has a real component and 3 imaginary components. Rather than rotate about 3 axes, the quaternion rotates a certain amount of degrees about a vector. The math is ridiculously simple (in quaternion form) compared to euler rotation matrices. My website link above goes into quaternions to a deeper level. ## Whats next This filter will be used in my IMU project, also on github, and run on an embedded MCU. I also intend to implement the MARG filter. ## Build Instructions This Project includes a basic CMakeList.txt. Run the following commands to build, make, and run the program. ```bash # Clone the repo and cd to the root of the directory # Create a build directory mkdir build # Run CMake cmake -S . -B build # Build the program with the generated make files cmake --build build/ # Run the program ./build/Madgwick_Filter ``` ### CMake Use this [Modern CMake](https://cliutils.gitlab.io/modern-cmake/) guide as an introduction to CMake. ================================================ FILE: madgwickFilter.c ================================================ // // madgwickFilter.c // madgwickFilter // // Created by Blake Johnson on 4/28/20. // #include "madgwickFilter.h" struct quaternion q_est = { 1, 0, 0, 0}; // initialize with as unit vector with real component = 1 struct quaternion quat_mult (struct quaternion L, struct quaternion R){ struct quaternion product; product.q1 = (L.q1 * R.q1) - (L.q2 * R.q2) - (L.q3 * R.q3) - (L.q4 * R.q4); product.q2 = (L.q1 * R.q2) + (L.q2 * R.q1) + (L.q3 * R.q4) - (L.q4 * R.q3); product.q3 = (L.q1 * R.q3) - (L.q2 * R.q4) + (L.q3 * R.q1) + (L.q4 * R.q2); product.q4 = (L.q1 * R.q4) + (L.q2 * R.q3) - (L.q3 * R.q2) + (L.q4 * R.q1); return product; } // The resulting quaternion is a global variable (q_est), so it is not returned or passed by reference/pointer // Gyroscope Angular Velocity components are in Radians per Second // Accelerometer componets will be normalized void imu_filter(float ax, float ay, float az, float gx, float gy, float gz){ //Variables and constants struct quaternion q_est_prev = q_est; struct quaternion q_est_dot = {0}; // used as a place holder in equations 42 and 43 //const struct quaternion q_g_ref = {0, 0, 0, 1};// equation (23), reference to field of gravity for gradient descent optimization (not needed because I used eq 25 instead of eq 21 struct quaternion q_a = {0, ax, ay, az}; // equation (24) raw acceleration values, needs to be normalized float F_g [3] = {0}; // equation(15/21/25) objective function for gravity float J_g [3][4] = {0}; // jacobian matrix for gravity struct quaternion gradient = {0}; /* Integrate angluar velocity to obtain position in angles. */ struct quaternion q_w; // equation (10), places gyroscope readings in a quaternion q_w.q1 = 0; // the real component is zero, which the Madgwick uses to simplfy quat. mult. q_w.q2 = gx; q_w.q3 = gy; q_w.q4 = gz; quat_scalar(&q_w, 0.5); // equation (12) dq/dt = (1/2)q*w q_w = quat_mult(q_est_prev, q_w); // equation (12) /* NOTE * Page 10 states equation (40) substitutes equation (13) into it. This seems false, as he actually * substitutes equation (12), q_se_dot_w, not equation (13), q_se_w. * * // quat_scalar(&q_w, deltaT); // equation (13) integrates the angles velocity to position * // quat_add(&q_w, q_w, q_est_prev); // addition part of equation (13) */ /* Compute the gradient by multiplying the jacobian matrix by the objective function. This is equation 20. The Jacobian matrix, J, is a 3x4 matrix of partial derivatives for each quaternion component in the x y z axes The objective function, F, is a 3x1 matrix for x y and z. To multiply these together, the inner dimensions must match, so use J'. I calculated "by hand" the transpose of J, so I will be using "hard coordinates" to get those values from J. The matrix multiplcation can also be done hard coded to reduce code. Note: it is possible to compute the objective function with quaternion multiplcation functions, but it does not take into account the many zeros that cancel terms out and is not optimized like the paper shows */ quat_Normalization(&q_a); // normalize the acceleration quaternion to be a unit quaternion //Compute the objective function for gravity, equation(15), simplified to equation (25) due to the 0's in the acceleration reference quaternion F_g[0] = 2*(q_est_prev.q2 * q_est_prev.q4 - q_est_prev.q1 * q_est_prev.q3) - q_a.q2; F_g[1] = 2*(q_est_prev.q1 * q_est_prev.q2 + q_est_prev.q3* q_est_prev.q4) - q_a.q3; F_g[2] = 2*(0.5 - q_est_prev.q2 * q_est_prev.q2 - q_est_prev.q3 * q_est_prev.q3) - q_a.q4; //Compute the Jacobian matrix, equation (26), for gravity J_g[0][0] = -2 * q_est_prev.q3; J_g[0][1] = 2 * q_est_prev.q4; J_g[0][2] = -2 * q_est_prev.q1; J_g[0][3] = 2 * q_est_prev.q2; J_g[1][0] = 2 * q_est_prev.q2; J_g[1][1] = 2 * q_est_prev.q1; J_g[1][2] = 2 * q_est_prev.q4; J_g[1][3] = 2 * q_est_prev.q3; J_g[2][0] = 0; J_g[2][1] = -4 * q_est_prev.q2; J_g[2][2] = -4 * q_est_prev.q3; J_g[2][3] = 0; // now computer the gradient, equation (20), gradient = J_g'*F_g gradient.q1 = J_g[0][0] * F_g[0] + J_g[1][0] * F_g[1] + J_g[2][0] * F_g[2]; gradient.q2 = J_g[0][1] * F_g[0] + J_g[1][1] * F_g[1] + J_g[2][1] * F_g[2]; gradient.q3 = J_g[0][2] * F_g[0] + J_g[1][2] * F_g[1] + J_g[2][2] * F_g[2]; gradient.q4 = J_g[0][3] * F_g[0] + J_g[1][3] * F_g[1] + J_g[2][3] * F_g[2]; // Normalize the gradient, equation (44) quat_Normalization(&gradient); /* This is the sensor fusion part of the algorithm. Combining Gyroscope position angles calculated in the beginning, with the quaternion orienting the accelerometer to gravity created above. Noticably this new quaternion has not be created yet, I have only calculated the gradient in equation (19). Madgwick however uses assumptions with the step size and filter gains to optimize the gradient descent, combining it with the sensor fusion in equations (42-44). He says the step size has a var alpha, which he assumes to be very large. This dominates the previous estimation in equation (19) to the point you can ignore it. Eq. 36 has the filter gain Gamma, which is related to the step size and thus alpha. With alpha being very large, you can make assumptions to simplify the fusion equatoin of eq.36. Combining the simplification of the gradient descent equation with the simplification of the fusion equation gets you eq. 41 which can be subdivided into eqs 42-44. */ quat_scalar(&gradient, BETA); // multiply normalized gradient by beta quat_sub(&q_est_dot, q_w, gradient); // subtract above from q_w, the integrated gyro quaternion quat_scalar(&q_est_dot, DELTA_T); quat_add(&q_est, q_est_prev, q_est_dot); // Integrate orientation rate to find position quat_Normalization(&q_est); // normalize the orientation of the estimate //(shown in diagram, plus always use unit quaternions for orientation) } /* returns as pointers, roll pitch and yaw from the quaternion generated in imu_filter Assume right hand system Roll is about the x axis, represented as phi Pitch is about the y axis, represented as theta Yaw is about the z axis, represented as psi (trident looking greek symbol) */ void eulerAngles(struct quaternion q, float* roll, float* pitch, float* yaw){ *yaw = atan2f((2*q.q2*q.q3 - 2*q.q1*q.q4), (2*q.q1*q.q1 + 2*q.q2*q.q2 -1)); // equation (7) *pitch = -asinf(2*q.q2*q.q4 + 2*q.q1*q.q3); // equatino (8) *roll = atan2f((2*q.q3*q.q4 - 2*q.q1*q.q2), (2*q.q1*q.q1 + 2*q.q4*q.q4 -1)); *yaw *= (180.0f / PI); *pitch *= (180.0f / PI); *roll *= (180.0f / PI); } ================================================ FILE: madgwickFilter.h ================================================ // // madgwickFilter.h // madgwickFilter // // Created by Blake Johnson on 4/28/20. // #ifndef MADGWICK_FILTER_H #define MADGWICK_FILTER_H // Include a hardware specific header file to redefine these predetermined values #ifndef DELTA_T #define DELTA_T 0.01f // 100Hz sampling frequency #endif #ifndef PI #define PI 3.14159265358979f #endif #ifndef GYRO_MEAN_ERROR #define GYRO_MEAN_ERROR PI * (5.0f / 180.0f) // 5 deg/s gyroscope measurement error (in rad/s) *from paper* #endif #ifndef BETA #define BETA sqrt(3.0f/4.0f) * GYRO_MEAN_ERROR //*from paper* #endif #include #include struct quaternion{ float q1; float q2; float q3; float q4; }; // global variables extern struct quaternion q_est; // Multiply two quaternions and return a copy of the result, prod = L * R struct quaternion quat_mult (struct quaternion q_L, struct quaternion q_R); // Multiply a reference of a quaternion by a scalar, q = s*q static inline void quat_scalar(struct quaternion * q, float scalar){ q -> q1 *= scalar; q -> q2 *= scalar; q -> q3 *= scalar; q -> q4 *= scalar; } // Adds two quaternions together and the sum is the pointer to another quaternion, Sum = L + R static inline void quat_add(struct quaternion * Sum, struct quaternion L, struct quaternion R){ Sum -> q1 = L.q1 + R.q1; Sum -> q2 = L.q2 + R.q2; Sum -> q3 = L.q3 + R.q3; Sum -> q4 = L.q4 + R.q4; } // Subtracts two quaternions together and the sum is the pointer to another quaternion, sum = L - R static inline void quat_sub(struct quaternion * Sum, struct quaternion L, struct quaternion R){ Sum -> q1 = L.q1 - R.q1; Sum -> q2 = L.q2 - R.q2; Sum -> q3 = L.q3 - R.q3; Sum -> q4 = L.q4 - R.q4; } // the conjugate of a quaternion is it's imaginary component sign changed q* = [s, -v] if q = [s, v] static inline struct quaternion quat_conjugate(struct quaternion q){ q.q2 = -q.q2; q.q3 = -q.q3; q.q4 = -q.q4; return q; } // norm of a quaternion is the same as a complex number // sqrt( q1^2 + q2^2 + q3^2 + q4^2) // the norm is also the sqrt(q * conjugate(q)), but thats a lot of operations in the quaternion multiplication static inline float quat_Norm (struct quaternion q) { return sqrt(q.q1*q.q1 + q.q2*q.q2 + q.q3*q.q3 +q.q4*q.q4); } // Normalizes pointer q by calling quat_Norm(q), static inline void quat_Normalization(struct quaternion * q){ float norm = quat_Norm(*q); q -> q1 /= norm; q -> q2 /= norm; q -> q3 /= norm; q -> q4 /= norm; } static inline void printQuaternion (struct quaternion q){ printf("%f %f %f %f\n", q.q1, q.q2, q.q3, q.q4); } // IMU consists of a Gyroscope plus Accelerometer sensor fusion void imu_filter(float ax, float ay, float az, float gx, float gy, float gz); // void marg_filter(void); for future void eulerAngles(struct quaternion q, float* roll, float* pitch, float* yaw); #endif /* MADGWICK_FILTER_H */ ================================================ FILE: main.c ================================================ // // main.c // madgwickFilter // // Created by Blake Johnson on 4/28/20. // #include #include "madgwickFilter.h" int main(int argc, const char * argv[]) { float roll = 0.0, pitch = 0.0, yaw = 0.0; // flat upright position to resolve the graident descent problem printf("\n\n Upright to solve gradient descent problem\n"); for(float i = 0; i<1000; i++){ imu_filter(0.05, 0.05, 0.9, 0, 0, 0); eulerAngles(q_est, &roll, &pitch, &yaw); printf("Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\n", i * DELTA_T, roll, pitch, yaw); } // Angular Rotation around Z axis (yaw) at 100 degrees per second printf("\n\nYAW 100 Degrees per Second\n"); for(float i = 0; i<1000; i++){ imu_filter(0, 0, 1, 0, 0, -100 * PI / 180); eulerAngles(q_est, &roll, &pitch, &yaw); printf("Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\n", i * DELTA_T, roll, pitch, yaw); } // Angular Rotation around Z axis (yaw) at -20 degrees per second printf("\n\nYAW -20 Degrees per Second\n"); for(float i = 0; i<1000; i++){ imu_filter(0, 0, 1, 0, 0, 20 * PI / 180); eulerAngles(q_est, &roll, &pitch, &yaw); printf("Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\n", i * DELTA_T, roll, pitch, yaw); } return 0; }