[
  {
    "path": ".gitignore",
    "content": "# CMake Build Directory\n**/build"
  },
  {
    "path": "CMakeLists.txt",
    "content": "cmake_minimum_required(VERSION 3.11)\n\nproject(Madgwick_Filter)\n\n# Should be in a subdirectory \nadd_library(madgwick SHARED madgwickFilter.c)\n\ntarget_include_directories(madgwick PUBLIC ${PROJECT_SOURCE_DIR}/)\n\nadd_executable( ${PROJECT_NAME} main.c)\n\ntarget_link_libraries( ${PROJECT_NAME} \n        madgwick\n        m)"
  },
  {
    "path": "License.md",
    "content": "MIT License\n\nCopyright (c) 2020, 2022 Blake Johnson\n\nPermission 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:\n\nThe above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.\n\nTHE 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.\n"
  },
  {
    "path": "README.md",
    "content": "# Madgwick Filter\nThe paper can be found [here](https://courses.cs.washington.edu/courses/cse474/17wi/labs/l4/madgwick_internal_report.pdf).\nMy explanation of the filter can be found on my website [here](https://blakejohnsonuf.com/project/#IMU), just scroll down to the Madgwick Filter section.\n\n## Brief Description\nAn IMU is a sensor suite complete with an accelerometer and gyroscope. A MARG contains an IMU plus a Magnetometer.\nAll 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.\nThe fusion of sensors is critical for accuracy of orientation and for a complete transformation from the sensor's inertial frame to the Earth frame. \n\nThe 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.\n\nA 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.\n\n## Whats next\n\nThis 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.  \n\n## Build Instructions \nThis Project includes a basic CMakeList.txt. Run the following commands to build, make, and run the program.\n\n```bash\n# Clone the repo and cd to the root of the directory\n\n# Create a build directory \n mkdir build\n\n # Run CMake\n cmake -S . -B build\n\n # Build the program with the generated make files\n cmake --build build/\n\n # Run the program\n ./build/Madgwick_Filter\n ```\n\n ### CMake\n Use this [Modern CMake](https://cliutils.gitlab.io/modern-cmake/) guide as an introduction to CMake.\n \n"
  },
  {
    "path": "madgwickFilter.c",
    "content": "//\n//  madgwickFilter.c\n//  madgwickFilter\n//\n//  Created by Blake Johnson on 4/28/20.\n//\n\n#include \"madgwickFilter.h\"\n\nstruct quaternion q_est = { 1, 0, 0, 0};       // initialize with as unit vector with real component  = 1\n\nstruct quaternion quat_mult (struct quaternion L, struct quaternion R){\n    \n    \n    struct quaternion product;\n    product.q1 = (L.q1 * R.q1) - (L.q2 * R.q2) - (L.q3 * R.q3) - (L.q4 * R.q4);\n    product.q2 = (L.q1 * R.q2) + (L.q2 * R.q1) + (L.q3 * R.q4) - (L.q4 * R.q3);\n    product.q3 = (L.q1 * R.q3) - (L.q2 * R.q4) + (L.q3 * R.q1) + (L.q4 * R.q2);\n    product.q4 = (L.q1 * R.q4) + (L.q2 * R.q3) - (L.q3 * R.q2) + (L.q4 * R.q1);\n    \n    return product;\n}\n\n\n// The resulting quaternion is a global variable (q_est), so it is not returned or passed by reference/pointer\n// Gyroscope Angular Velocity components are in Radians per Second\n// Accelerometer componets will be normalized\nvoid imu_filter(float ax, float ay, float az, float gx, float gy, float gz){\n    \n    //Variables and constants\n    struct quaternion q_est_prev = q_est;\n    struct quaternion q_est_dot = {0};            // used as a place holder in equations 42 and 43\n    //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\n    struct quaternion q_a = {0, ax, ay, az};    // equation (24) raw acceleration values, needs to be normalized\n    \n    float F_g [3] = {0};                        // equation(15/21/25) objective function for gravity\n    float J_g [3][4] = {0};                     // jacobian matrix for gravity\n    \n    struct quaternion gradient = {0};\n    \n    /* Integrate angluar velocity to obtain position in angles. */\n    struct quaternion q_w;                   // equation (10), places gyroscope readings in a quaternion\n    q_w.q1 = 0;                              // the real component is zero, which the Madgwick uses to simplfy quat. mult.\n    q_w.q2 = gx;\n    q_w.q3 = gy;\n    q_w.q4 = gz;\n    \n    quat_scalar(&q_w, 0.5);                  // equation (12) dq/dt = (1/2)q*w\n    q_w = quat_mult(q_est_prev, q_w);        // equation (12)\n\n    /* NOTE\n    * Page 10 states equation (40) substitutes equation (13) into it. This seems false, as he actually\n    * substitutes equation (12), q_se_dot_w, not equation (13), q_se_w.\n    * \n    * // quat_scalar(&q_w, deltaT);               // equation (13) integrates the angles velocity to position\n    * // quat_add(&q_w, q_w, q_est_prev);         // addition part of equation (13)\n    */\n\n    /* Compute the gradient by multiplying the jacobian matrix by the objective function. This is equation 20.\n     The Jacobian matrix, J, is a 3x4 matrix of partial derivatives for each quaternion component in the x y z axes\n     The objective function, F, is a 3x1 matrix for x y and z.\n     To multiply these together, the inner dimensions must match, so use J'.\n     I calculated \"by hand\" the transpose of J, so I will be using \"hard coordinates\" to get those values from J.\n     The matrix multiplcation can also be done hard coded to reduce code.\n     \n     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\n     */\n    \n    quat_Normalization(&q_a);              // normalize the acceleration quaternion to be a unit quaternion\n    //Compute the objective function for gravity, equation(15), simplified to equation (25) due to the 0's in the acceleration reference quaternion\n    F_g[0] = 2*(q_est_prev.q2 * q_est_prev.q4 - q_est_prev.q1 * q_est_prev.q3) - q_a.q2;\n    F_g[1] = 2*(q_est_prev.q1 * q_est_prev.q2 + q_est_prev.q3* q_est_prev.q4) - q_a.q3;\n    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;\n    \n    //Compute the Jacobian matrix, equation (26), for gravity\n    J_g[0][0] = -2 * q_est_prev.q3;\n    J_g[0][1] =  2 * q_est_prev.q4;\n    J_g[0][2] = -2 * q_est_prev.q1;\n    J_g[0][3] =  2 * q_est_prev.q2;\n    \n    J_g[1][0] = 2 * q_est_prev.q2;\n    J_g[1][1] = 2 * q_est_prev.q1;\n    J_g[1][2] = 2 * q_est_prev.q4;\n    J_g[1][3] = 2 * q_est_prev.q3;\n    \n    J_g[2][0] = 0;\n    J_g[2][1] = -4 * q_est_prev.q2;\n    J_g[2][2] = -4 * q_est_prev.q3;\n    J_g[2][3] = 0;\n    \n    // now computer the gradient, equation (20), gradient = J_g'*F_g\n    gradient.q1 = J_g[0][0] * F_g[0] + J_g[1][0] * F_g[1] + J_g[2][0] * F_g[2];\n    gradient.q2 = J_g[0][1] * F_g[0] + J_g[1][1] * F_g[1] + J_g[2][1] * F_g[2];\n    gradient.q3 = J_g[0][2] * F_g[0] + J_g[1][2] * F_g[1] + J_g[2][2] * F_g[2];\n    gradient.q4 = J_g[0][3] * F_g[0] + J_g[1][3] * F_g[1] + J_g[2][3] * F_g[2];\n    \n    // Normalize the gradient, equation (44)\n    quat_Normalization(&gradient);\n  \n    /* This is the sensor fusion part of the algorithm.\n     Combining Gyroscope position angles calculated in the beginning, with the quaternion orienting the accelerometer to gravity created above.\n     Noticably this new quaternion has not be created yet, I have only calculated the gradient in equation (19).\n     Madgwick however uses assumptions with the step size and filter gains to optimize the gradient descent,\n        combining it with the sensor fusion in equations (42-44).\n     He says the step size has a var alpha, which he assumes to be very large.\n     This dominates the previous estimation in equation (19) to the point you can ignore it.\n     Eq. 36 has the filter gain Gamma, which is related to the step size and thus alpha. With alpha being very large,\n        you can make assumptions to simplify the fusion equatoin of eq.36.\n     Combining the simplification of the gradient descent equation with the simplification of the fusion equation gets you eq.\n     41 which can be subdivided into eqs 42-44.\n    */\n    quat_scalar(&gradient, BETA);             // multiply normalized gradient by beta\n    quat_sub(&q_est_dot, q_w, gradient);        // subtract above from q_w, the integrated gyro quaternion\n    quat_scalar(&q_est_dot, DELTA_T);\n    quat_add(&q_est, q_est_prev, q_est_dot);     // Integrate orientation rate to find position\n    quat_Normalization(&q_est);                 // normalize the orientation of the estimate\n                                                //(shown in diagram, plus always use unit quaternions for orientation)\n   \n}\n\n/*\n returns as pointers, roll pitch and yaw from the quaternion generated in imu_filter\n Assume right hand system\n Roll is about the x axis, represented as phi\n Pitch is about the y axis, represented as theta\n Yaw is about the z axis, represented as psi (trident looking greek symbol)\n */\nvoid eulerAngles(struct quaternion q, float* roll, float* pitch, float* yaw){\n    \n    *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)\n    *pitch = -asinf(2*q.q2*q.q4 + 2*q.q1*q.q3);                                  // equatino (8)\n    *roll  = atan2f((2*q.q3*q.q4 - 2*q.q1*q.q2), (2*q.q1*q.q1 + 2*q.q4*q.q4 -1));\n    \n    *yaw *= (180.0f / PI);\n    *pitch *= (180.0f / PI);\n    *roll *= (180.0f / PI);\n\n}\n\n\n"
  },
  {
    "path": "madgwickFilter.h",
    "content": "//\n//  madgwickFilter.h\n//  madgwickFilter\n//\n//  Created by Blake Johnson on 4/28/20.\n//\n\n#ifndef MADGWICK_FILTER_H\n#define MADGWICK_FILTER_H\n\n// Include a hardware specific header file to redefine these predetermined values\n#ifndef DELTA_T\n    #define DELTA_T 0.01f // 100Hz sampling frequency\n#endif\n\n#ifndef PI  \n    #define PI 3.14159265358979f\n#endif\n\n#ifndef GYRO_MEAN_ERROR\n    #define GYRO_MEAN_ERROR PI * (5.0f / 180.0f) // 5 deg/s gyroscope measurement error (in rad/s)  *from paper*\n#endif\n\n#ifndef BETA\n    #define BETA sqrt(3.0f/4.0f) * GYRO_MEAN_ERROR    //*from paper*\n#endif\n\n#include <math.h>\n#include <stdio.h>\n\nstruct quaternion{\n    float q1;\n    float q2;\n    float q3;\n    float q4;\n};\n\n// global variables\nextern struct quaternion q_est;\n\n// Multiply two quaternions and return a copy of the result, prod = L * R\nstruct quaternion quat_mult (struct quaternion q_L, struct quaternion q_R);\n\n// Multiply a reference of a quaternion by a scalar, q = s*q\nstatic inline void quat_scalar(struct quaternion * q, float scalar){\n    q -> q1 *= scalar;\n    q -> q2 *= scalar;\n    q -> q3 *= scalar;\n    q -> q4 *= scalar;\n}\n\n// Adds two quaternions together and the sum is the pointer to another quaternion, Sum = L + R\nstatic inline void quat_add(struct quaternion * Sum, struct quaternion L, struct quaternion R){\n    Sum -> q1 = L.q1 + R.q1;\n    Sum -> q2 = L.q2 + R.q2;\n    Sum -> q3 = L.q3 + R.q3;\n    Sum -> q4 = L.q4 + R.q4;\n}\n\n// Subtracts two quaternions together and the sum is the pointer to another quaternion, sum = L - R\nstatic inline void quat_sub(struct quaternion * Sum, struct quaternion L, struct quaternion R){\n    Sum -> q1 = L.q1 - R.q1;\n    Sum -> q2 = L.q2 - R.q2;\n    Sum -> q3 = L.q3 - R.q3;\n    Sum -> q4 = L.q4 - R.q4;\n}\n\n\n// the conjugate of a quaternion is it's imaginary component sign changed  q* = [s, -v] if q = [s, v]\nstatic inline struct quaternion quat_conjugate(struct quaternion q){\n    q.q2 = -q.q2;\n    q.q3 = -q.q3;\n    q.q4 = -q.q4;\n    return q;\n}\n\n// norm of a quaternion is the same as a complex number\n// sqrt( q1^2 + q2^2 + q3^2 + q4^2)\n// the norm is also the sqrt(q * conjugate(q)), but thats a lot of operations in the quaternion multiplication\nstatic inline float quat_Norm (struct quaternion q)\n{\n    return sqrt(q.q1*q.q1 + q.q2*q.q2 + q.q3*q.q3 +q.q4*q.q4);\n}\n\n// Normalizes pointer q by calling quat_Norm(q),\nstatic inline void quat_Normalization(struct quaternion * q){\n    float norm = quat_Norm(*q);\n    q -> q1 /= norm;\n    q -> q2 /= norm;\n    q -> q3 /= norm;\n    q -> q4 /= norm;\n}\n\nstatic inline void printQuaternion (struct quaternion q){\n    printf(\"%f %f %f %f\\n\", q.q1, q.q2, q.q3, q.q4);\n}\n\n\n// IMU consists of a Gyroscope plus Accelerometer sensor fusion\nvoid imu_filter(float ax, float ay, float az, float gx, float gy, float gz);\n\n// void marg_filter(void); for future\n\n\nvoid eulerAngles(struct quaternion q, float* roll, float* pitch, float* yaw);\n\n\n\n#endif /* MADGWICK_FILTER_H */\n"
  },
  {
    "path": "main.c",
    "content": "//\n//  main.c\n//  madgwickFilter\n//\n//  Created by Blake Johnson on 4/28/20.\n//\n\n#include <stdio.h>\n#include \"madgwickFilter.h\"\n\nint main(int argc, const char * argv[]) {\n    float roll = 0.0, pitch = 0.0, yaw = 0.0;\n\n    // flat upright position to resolve the graident descent problem\n    printf(\"\\n\\n Upright to solve gradient descent problem\\n\");\n    for(float i = 0; i<1000; i++){\n        imu_filter(0.05, 0.05, 0.9, 0, 0, 0);\n        eulerAngles(q_est, &roll, &pitch, &yaw);\n        printf(\"Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\\n\", i * DELTA_T, roll, pitch, yaw);\n    }\n\n    // Angular Rotation around Z axis (yaw) at 100 degrees per second\n    printf(\"\\n\\nYAW 100 Degrees per Second\\n\");\n    for(float i = 0; i<1000; i++){\n        imu_filter(0, 0, 1, 0, 0, -100 * PI / 180);\n        eulerAngles(q_est, &roll, &pitch, &yaw);\n        printf(\"Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\\n\", i * DELTA_T, roll, pitch, yaw);\n    }\n\n    // Angular Rotation around Z axis (yaw) at -20 degrees per second\n    printf(\"\\n\\nYAW -20 Degrees per Second\\n\");\n    for(float i = 0; i<1000; i++){\n        imu_filter(0, 0, 1, 0, 0, 20 * PI / 180);\n        eulerAngles(q_est, &roll, &pitch, &yaw);\n        printf(\"Time (s): %.3f, roll: %f, pitch: %f, yaw: %f\\n\", i * DELTA_T, roll, pitch, yaw);\n    }\n    \n    return 0;\n}\n"
  }
]