1. Gyroscopes & Accelerometers Sensor fusion Using MPU-6050

2. Inertial Measurement Unit (IMUs)
There are small devices indicating changing orientation in smart phones, video game remotes, quad-copters, etc.
These devices contains gyroscopes combined with accelerometers and/or compasses and are referred to as an Inertial Measurement Unit (IMU)
The number of sensor inputs in an IMU are referred to as “DOF” (Degrees of Freedom), so a chip with a 3-axis gyroscope and a 3-axis accelerometer would be a 6-DOF IMU.

3. MPU 6050
The MPU 6050 is a 6 DOF (Degrees of Freedom) or a six axis IMU sensor, which means that it gives six values as output.
Three values from the accelerometer and three from the gyroscope.
The MPU 6050 is a sensor based on MEMS (Micro Electro Mechanical Systems) technology.
Both the accelerometer and the gyroscope is embedded inside a single chip.
This chip uses I2C (Inter Integrated Circuit) protocol for communication.

4. MPU 6050
The MPU6050 has 6 built in 16 bit ADC channels, three for the gyroscope outputs and three for the accelerometer outputs.
It communicates with  the microcontroller using the I2C protocol.
The operating voltage range of MCU6050 is from 2.37v to 3.46V.
A low drop out regulator is provided on the MPU 6050 board for providing this voltage.
The full scale ranges of the acclelerometer and gyroscope are user programmable and they are +/- 2g, 4g, 8g and 16g for the accelerometer and +/- 250 °/S, 500 °/S, 1000 °/S and 2000 °/S.

5. How does an accelerometer work
An accelerometer works on the principle of piezo electric effect. Here, imagine a cuboidal box, having a small ball inside it, like in the picture below:

6. How does an accelerometer work
The walls of this box are made with piezo electric crystals.
Whenever you tilt the box, the ball is forced to move in the direction of the inclination, due to gravity.
The wall with which the ball collides, creates tiny piezo electric currents.
There are totally, three pairs of opposite walls in a cuboid. Each pair corresponds to an axis in 3D space: X, Y and Z axes.
Depending on the current produced from the piezo electric walls, we can determine the direction of inclination and its magnitude. 

7. Accelerometers
Each accelerometer has a zero-g voltage level, you can find it in spec
Accelerometers also have a sensitivity, usually expressed in mV/g
Divide the zero-g level corrected reading by the sensitivity to produce the final reading
Proof mass deflection is measured as a change in capacitance between the proof mass and sensing plates
Internal circuitry converts the tiny capacitance to a voltage signal which is digitized and output

8. How does a Gyroscope work?
A gyroscope measures angular velocity (the rate of change in orientation angle), not angular orientation itself .
Gyroscopes work on the principle of Coriolis acceleration i.e. mass flow meter, also known as an inertial flow meter which measures mass flow rate of a fluid travelling through a tube. 
Imagine that there is a fork like structure, that is in constant back and forth motion, as shown below:

9. How does a Gyroscope work?
It is held in place using piezo electric crystals.
Whenever, you try to tilt this arrangement, the crystals experience a force in the direction of inclination.
This is caused as a result of the inertia of the moving fork.
The crystals thus produce a current in consensus with the piezo electric effect, and this current is amplified. 

10. Gyroscope
It must first initialize the sensor position with a known value (possibly from the accelerometer), then measure the angular velocity (ω) around the X, Y and Z axes at measured intervals (Δt)  
ω × Δt = change in angle 
The new orientation angle is the original angle plus this change
This is integrating - adding up many small computed intervals - to find orientation  
Repeatedly adding up increments of  ω × Δt results in small systematic errors becoming magnified over time 
Gyroscopic drift---over long timescales the gyroscope data will become increasingly inaccurate

11. Gyroscope
Uses Coriolis effect to transform an angular velocity into a displacement
The Coriolis force acts perpendicular to the rotation axis and to the velocity of the body in the rotating frame
Fc= -2m Ω x v
The displacement induces a change in capacitance between the mass and the housing, thus transforming the angular rate input to the gyroscope into an electrical output

12. Gyroscopes Each gyroscope measures the rotation around one axis
Axz – is the angle between the Rxz (projection of R on XZ plane) and Z axis
Ayz – is the angle between the Ryz (projection of R on YZ plane) and Z axis
Gyroscopes measure the rate of change of these angles.

13. Computing Rotation Angles
Rotation from accelerometer data:
tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)
Rotation from gyroscope data:
Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) )
Axz(n) = Axz(n-1) + RateAxz(n) * T

14. Sensor Fusion
An accelerometer measures inertial force, such as gravity (and ideally only by gravity), but it might also be caused by acceleration (movement) of the device. Even if the accelerometer is relatively stable, it is very sensitive to vibration and mechanical noise.
A gyroscope is less sensitive to linear mechanical movements, the type of noise that accelerometer suffers from. Gyroscopes have other types of problems like drift (not coming back to zero-rate value when rotation stops).
Averaging the data that comes from accelerometers and gyroscopes can produce a better estimate of orientation than obtained using accelerometer data alone.

15. Fusion Algorithms
Several choices: Kalman Filter, Complementary Filter, …
Combine orientation estimated from Accelerometer readings with that estimated from the Gyroscope readings
Racc – current readings from accelerometer Rgyro – obtained from Rest(n-1) and current gyroscope readings
A weighted average:
Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)

16. Sensor Fusion

17. MPU-6050
The MPU-6050 is the world’s first integrated 6-axis Motion Tracking device/
It combines a 3-axis gyroscope, 3-axis accelerometer, and a Digital Motion Processor™ (DMP) all in a small 4x4x0.9mm package.
It uses a standard I2C bus for data transmission.
With it’s I2C bus, it can accepts inputs from an external 3-axis compass to provide a complete 9-axis Motion Fusion output.

18. A Read Example with MPU 6050 For MPU-6050: ACCEL_XOUT_H  register 3B
ACCEL_XOUT_L  register 3C
ACCEL_YOUT_H  register 3D
ACCEL_YOUT_L  register 3E
ACCEL_ZOUT_H  register 3F
ACCEL_ZOUT_L  register 40

19. MPU-6050

20. The Physical Connection
Connecting the MPU to the Pi
MPU Pi Pin ID
Pin ID VDD --> 5V on UNO
GND --> GND on UNO SCL > A5 on UNO
SDA --> A4 on UNO
XDA XCL ADO INT --> Digital Pin 2 on UNO

21. MPU 6050

22. Interfacing the MPU 6050 with Arduino
The MPU 6050 communicates with the Arduino through the I2C protocol. The MPU 6050 is connected to Arduino as shown in the following diagram:

23. Connections of MPU 6050 with Arduino
Here, if your MPU 6050 module has a 5V pin, then you can connect it to your arduino’s 5V pin. Else, you will have to connect it to the 3.3V pin. Next, the GND of the arduino is connected to the GND of the MPU 6050.
Connect arduino’s digital pin 2 (interrupt pin 0) to the pin labelled as INT on the MPU 6050.
Next, we need to set up the I2C lines. For this connect the pin labelled as SDA on the MPU 6050 to the arduino’s analog pin 4 (SDA). And the pin labelled as SCL on the MPU 6050 to the arduino’s analog pin 5 (SCL). And that’s it, you have finished wiring up the Arduino MPU 6050.

24. Uploading the code and testing the Arduino MPU 6050
To test the Arduino MPU 6050, first download the arduino library for MPU 6050, developed by Jeff Rowberg.
Next you have to unzip/extract this library. And then take the folder named “MPU6050” and paste it inside the arduino’s “library” folder.
That is, go to the location where you have installed arduino (Arduino –> libraries) and paste it inside the libraries folder.
You might also have to do the same thing to install the I2Cdev library as well, if you don’t already have it for your arduino.

25. If you have done this correctly, then when you open the arduino IDE, you can see “MPU6050” in File –> Examples. Next, open the example program from: File –> Examples –> MPU6050 –> Examples –> MPU6050_DMP6.

26. Now, you have to upload this code to arduino.
After uploading the code, open up the serial monitor and set the baud rate as
Next, check if you see stuff like “Initializing I2C devices…” on the serial monitor. If you don’t, just press the reset button.
Now, you’ll see a line saying  “Send any character to begin DMP programming and demo: “. So just type in any character on the serial monitor and send it.
And you will start seeing the yaw, pitch and roll values coming in from the MPU Like so:

28. DMP stands for Digital Motion Processing.
Here, the Invense’s MPU 6050 has an inbuilt motion processor. It processes the values from the accelerometer and gyroscope to give us accurate 3D values.
Also, you will need to wait about 10 secs before you get accurate values from the Arduino MPU After which, the values will begin to stabilize.