15 | Combine a gyroscope and accelerometer to measure angles - precisely

welcome to the next video in our series explaining how to build your own quadcopter drone in this video I will explain how to program a one-dimensional Kalman filter to calculate the absolute angle of a quadcopter in part 14 we learned that you could calculate the angle with two methods gyroscope rotation rate integration and accelerometer trigonometry we also found out that both methods have disadvantages with rotation rate integration we have an ever increasing error on the angle because you keep dragging all past errors with you this is clearly visible on the red graph with accelerometer trigonometry vibrations have a huge impact on the angle as you can see on the orange graph fortunately there exists a mathematical method that allows you to combine both measurements without their visual disadvantages the Kalman filter let's start with the integration of the rotation rates which we derived in part 14 in order to derive the common filter let's change the notation of the angle and call it the Kalman angle remember that the rotation rate in degrees per second is measured with a gyroscope now let's assume that the resulting angle is not its final value but merely a prediction because it is a prediction it has an Associated uncertainty which is equal to the uncertainty of the previous iteration and the iteration length DS squared multiplied by the variance on the rotation rate the variance on the rotation rate is nothing more than an estimation of the error on the gyroscope measurement let's make an educated guess and characterize this error with the standard deviation of 4 degrees per seconds knowing that the variance is equal to the squared deviation now we can calculate a new prediction of the angle which will be equal to the sum of the prediction of the angle with the so-called Kalman gain multiplied by the difference between the measured angle and the prediction of the angle the measured angle is in this case equal to the angle measured with accelerometer trigonometry now how can you determine the common gain itself well the common gain is defined as a relative ratio of the uncertainty on the predicted angle to the uncertainty on the measured angle with the accelerometer we will assume here that the standard deviation of the accelerometer measurement error is equal to 3 degrees a final equation is necessary to update the uncertainty on the new angle prediction once again using the Kalman gain let's take the opportunity to write the command filter in a more General way the first equation predicts the current state of the system in our one-dimensional case the state Vector is equal to the kelman angle the state transition Matrix is equal to one the control Matrix is the iteration length and the input variable is the rotation rate measurement the second equation is used to calculate the uncertainty of the prediction while the common gain K is calculated with a third and fourth equation from the uncertainties on the prediction and measurements next you need to update the predicted state of the system with the measurements of the state which are the accelerometer measurements in our case in the final equation you update the uncertainty of the predicted State now let's proceed by programming the Kalman filter first declare all gyroscope and accelerometer values as seen in part 14 Define the rule and the pitch angles coming from our Kalman filter your initial guess for the angle values is zero because the quadcopter will generally take off from a level surface of course a surface can never be exactly level well so you take the uncertainty on the initial guess for the angles to be 2 degrees Define the output from the Kalman filter these are two variables the common prediction for the state which is the angle in our case and the uncertainty on this prediction both variables are updated during each iteration let's continue with the function for the Kalman filter the function contains the equations to predict the state with rotation rate integration calculate the uncertainty on the prediction calculate the common gain updates the predicted state with the accelerometer measurements and finally update the uncertainty on the predicted State the output of the Kalman filter function consists of a prediction for the state which is the angle and the corresponding uncertainty remember that the Kalman input contains the rotation rate measurement the common measurement and the accelerometer angle measurement and that common state contains the angle calculated with the Kalman filter the other parts of the code are the same as in Project 14. use a function to read the rotation rate and angles from the mpu6050 and do not forget to put your own accelerometer calibration values for the numbers highlighted in yellow before calculating the accelerometer roll and Pitch angles the setup Parts where you communicate with the gyroscope and calibrate it come straight from Parts 4 and 5 and the setup part and start the loop part with the calculation of the rotation rate now that the rotation reads from the gyro and the angles from the accelerometer are measured you can start the Kalman filter function the first Common filter will calculate the roll angle and needs the uncertainty on the Kalman angle together with the measured rotation rate and angle the output of the filter will give you the updated common angle and its Associated uncertainty repeat the same lines for the pitch angle and print the predicted angle values now upload a code to your TNC and open the serial plotter to visualize the rule and Pitch angles you can observe that both the roll and Pitch angles calculated by the command filter behave very smoothly and are therefore suitable for a flight controller now pitch the mpu6050 in the positive and negative Direction the measured angle of the Kalman filter follows smoothly repeat the same movement in the role Direction the behavior is smooth and accurate as well notice how there is no accumulation of Errors because the common filter compares the rotation rate with the accelerometer values if you apply some vibrations to the mpu-6050 you will notice that the angle does not change as much as when you would use the accelerometer values this means that thanks to the Kalman filter we are now able to calculate angles that can be used in a flight controller let's compare the values for the angles calculated by the kalman's filter in blue with the angles calculated through accelerometer trigonometry for an mpu 6050 sensor mounted on a stationary quadcopter when the motors of a quadcopter are started you notice that the angles calculated with the accelerometer reflect the vibrations and that the vibrations increase with higher motor speeds the vibrations disappear when the motors are stopped the angles calculated with the common filter are not impacted by the vibrations showing the quality of the applied filter one question Still Remains to be answered what is the meaning of the Kalman gain well let's go back to the equation in which you calculate the kelman angle prediction from the gain the gain has a value close to 1 if the measurement of the angle through the accelerometer has the biggest influence on the Kalman angle prediction on the other hand the gain will have a value close to zero if the prediction of the angle from the gyroscope rotation rate integration has the biggest influence on the carbon angle when looking at the evolution of the kelman gain for our application it indicates that during the first few milliseconds the angle will be calculated with the help of the accelerometer however this effect disappears soon and the rotation rate integration will become more important leading to a common gain that is very close to zero in steady state and that's it in the next video we will use the angle to develop a stabilized mode flight controller thank you for watching this video don't forget to subscribe if you like the series and remember that you can find all tutorials on YouTube and the full codes on GitHub the manual which contains all applications is available as well on GitHub if you need some more information thanks for watching and see you next time

Channel: Carbon Aeronautics

Views: 153,871

Keywords: Arduino, Teensy, Electronics, Quadcopter, Drone, MPU-6050

Id: 5HuN9iL-zxU

Length: 9min 48sec (588 seconds)

Published: Fri Dec 30 2022