1. Graduate School of Electrical Engineering
Southern Taiwan University
Graduate School of Electrical Engineering
Improving Accuracy of Orientation Determination In Auto Balance Systems Using Mems IMU By Kalman
Student : Trinh Dinh Quan
　Advisor：Tsung-Fu Chien   
December, 2014

2. Outline Introduction Optimal in orientation determination
PID Control Algorithm
The Balancing Camera System
Experiment
Conlusion

3. Outline Introduction Literature Review
Proposal auto balancing platform
Optimal in orientation determination
PID Control Algorithm
The Balancing Camera System
Experiment
Conlusion

4. Literature Review
To keep titling the camera of an Unmanned Aviation Vehicle (UAV) mobile mapping system, an auto-balancing is proposed.
A proposed auto-balancing photogrammetric platform includes an Inertial Measurement Unit (IMU) for measuring three angles of Roll, Pitch, and Yaw.
With this proposed platform, we can easily take a photo from camera with small disturbance by auto adjusting three angles of Roll, Pitch, and Yaw to get desired direction

5. Proposal auto balancing platform
A proposed platform includes a 4-link mechanism for motions around three directions corresponding to three angles of Roll, Pitch, and Roll which are measured by an Inertial Measurement Unit (IMU).
Three servo motors are used as actuator for rotating around three directions. This proposed platform will be mounted at UAV.
Direction of platform can be set and controlled by a microcontroller via PID algorithm. Schematic of a proposed platform was shown in Figure .

6. Outline Introduction Optimal in orientation determination
Reference frame
Inertial frame
Local lever frame
Body frame
Optimal INS mechanization
Attitude estimation with kalman filter
Model design for kalman filter
Model design for EKF

7. Inertial Frame
The origin of the i-frame at the center of mass of the earth.
z-axis apparel to the rotation axis (north polar axis) of the earth.
x-axis in the equatorial plane pointing toward the vernal equinox (The direction of the intersection of the earth with the plan of the earth’s orbit around the sun).
y-axis completes a right-hand system.
Figure : Inertial frame

8. Local Level Frame Its origin coincides with that of the body frame.
Its xn-axis points toward to the geodetic north.
Its yn-axis points toward to the geodetic east, and its zn-axis is orthogonal to the reference ellipsoid pointing up (with NEU) or down (with NED)
Figure 2.2: Earth-centered, Earth-fixed frame (blue),
geodetic frame (yellow), and navigation frame (green).

9. Body frame The origin of the body frame is the center of the IMU
For rotation representation, roll (), pitch (θ), and yaw () are rotation angles around the x-, y-, and z-axes, respectively
Figure 2.3 : Describes the body frame

10. Optimal INS mechanization
INS mechanization is a set of mathematical equations that calculate the navigation solutions, including position, velocity, and attitude, from the output of an IMU
This study introduces INS mechanization to derive attitude (roll, pitch, heading) in the l-frame from gyroscope only.
In principle, the attitude is update from the output of gyroscope by below equation:
𝐶 𝑏 𝑙 = 𝐶 𝑏 𝑙 𝛺 𝑖𝑏 𝑏 − 𝛺 𝑖𝑙 𝑏
𝐶 𝑏 𝑙 is the time derivative of the attitude.
𝐶 𝑏 𝑙 is the transformation matrix from the body frame to the local level frame.
𝛺 𝑖𝑏 𝑏 is the angular velocity of the body frame relative to the inertial frame parameterized in the body frame.
𝛺 𝑖𝑙 𝑏 is the rotation rate of the Inertial-frame with respect to the local-level frame.

11. Figure : INS mechanization for orientation determination
Optimal INS mechanization
To avoid singularity problem in INS mechanization , a quaternion based method is applied in this research as shown in the Figure
Figure : INS mechanization for orientation determination

12. Figure 2.6 IMU/Magnetometer integration Integration architecture
Attitude estimation with Kalman filter
The purpose of the KF is to estimate the posterior probability distribution of a designed state vector based on principle of minimum corresponding covariance matrix [citation]
Figure 2.6 IMU/Magnetometer integration Integration architecture

13. Attitude estimation with Kalman filter
The system model is built based on INS error model
Where 𝑥= 𝛿𝜓 𝑏 𝑔 𝑠 𝑔 9×1 𝑇 is state vector, its components include position, velocity, attitude errors, biases and scale factor of accelerometers and gyroscopes.
𝛷 𝑘−1;𝑘 is the state transition matrix from epoch k − 1 to k.
wk is system noise.
The measurement model is built based on GPS measurement:
Where zk is measurement vector.
Hk is mapping matrix.
nk is measurement noises at time k, respectively.

14. Attitude estimation with Kalman filter
Estimation with Extended Kalman filter

15. Outline Introduction Optimal in orientation determination
PID Control Algorithm
PID Control Algorithm in an Embedded System
PID control for Auto-balancing System
The Balancing Camera System
Experiment
Conlusion

16. PID Control Algorithm in an Embedded System
The combination of the proportional, integral, and derivative actions can be done in different ways. In the so-called ideal or non-interacting form, the PID controller is described by the following transfer function :
𝐶 𝑠 = 𝐾 𝑃 𝐾 𝐼 𝑠 + 𝐾 𝐷 𝑠
𝐾 𝑃 is the proportional gain.
𝐾 𝐼 is the integral time constant.
𝐾 𝐷 is the derivative time constant
The PID controller representations can be shown in Fig :
where, e(t) is controller input (system error), 𝑢 𝑐 (𝑡) is PID control signal

17. PID Control Algorithm in an Embedded System
The typical industrial control system is represented in Fig :
Figure 3.2: The typical industrial control system

18. PID Control Algorithm in an Embedded System
A general methodology used in designing an embedded system is shown in Table.
Design phase
Design phase details
Requirements
Functional requirements and non-functional requirements (size, weight, power consumption and cost)
User specifications
User interface details along with operations needed to satisfy user request
Architecture
Hardware components (processor, peripherals, programmable logic and ASSPs), software components (major programs and their operations)
Component design
Pre-designed components, modified components and new components
System integration (hardware and software)
Verification scheme to uncover bugs quickly

19. PID Control Algorithm in an Embedded System
Structure of a general-purpose embedded system is shown in following Fig

20. PID control for Auto-balancing System
In this research, a PID controller was applied to control the angle rotation of each link of frame.
A mathematical description of the PID controller in time-domain is as follows
𝑢 𝑡 = 𝐾 𝑝 𝑒 𝑡 + 𝐾 𝑖 𝑒 𝑡 𝑑𝑡 + 𝐾 𝑑 𝑑𝑒 𝑑𝑡
u(t) which was the PMW output of the microcontroller
e(t) The variable represents the tracking error which was the difference between the set input value r(t) and the actual output y(t).
e(t) The error signal was used to generate the proportional, integral, and derivative actions, with the resulting signals weighted and summed to form the control signal u(t) applied to the needle heating system
K­p, Ki, and Kd are proportional gain, integral gain, and derivative gain, respectively

21. PID control for Auto-balancing System
In this paper, three parameters of the PID controller were adjusted by the Ziegler-Nichols method
Actual pitch, raw, roll
Desired angles pitch, raw, roll
PWM signal +
e(t)
PID controller
Auto-balancing system _
r(t) as reference input signal was the set angles of links including pitch, raw, and roll angle.
y(t)-output signal was the actual angle of the link

22. Outline Introduction Optimal in orientation determination
PID Control Algorithm
The Balancing Camera System
Auto balance camera system
Controller
Experiment
Conlusion

23. Auto balance camera system
Axis Brushless Gimbal Camera

24. Auto balance camera system
Model: BGM
Turns: 130 turns
Cooper Wire(mm): 0.15
Camera Range: 1600g~2000g
magnet: 18
Slots:24
Shaft : 4.0
Weight:110g
Motor Size (mm):46*25 
Ri: 17.0ohm
Brushless Gimbal Motor BGM

25. Controller
The Arduino Mega 2560 is a microcontroller board based on the ATmega2560
Summary
Microcontroller ATmega2560
Operating Voltage 5V
Input Voltage (recommended) 7-12V
Input Voltage (limits) 6-20V
Digital I/O Pins 54 (of which 14 provide PWM output)
Analog Input Pins 16
DC Current per I/O Pin 40 mA
DC Current for 3.3V Pin 50 mA
Flash Memory 256 KB of which 8 KB used by bootloader
SRAM 8 KB
EEPROM 4 KB
Clock Speed 16 MHz
Microcontroller – Arduino Mega 2560

26. Controller
The InvenSense MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single chip.
It is very accurate, as it contains 16-bits analog to digital conversion hardware for each channel
MPU-6050 QFN Package & Axis Orientation Accelerometer Specifications
Top side of the MPU-6050

27. Controller
IMU MPU6050 specification

28. Controller
Arduino Software

29. Physical characteristics
Experiment
To evaluate the performance of the proposed algorithms and system, two tests were implemented in this research
In the first test a high-end grated IMU, SPAN-LCI with fiber-optical gyro (Novatel) was used as the reference system. The specification if the LCI-CPT is shown on Table
Physical characteristics
IMU performance
Output rate (Hz)
200
In run Gyro bias (degrees/h)
0.1
Gyro scale factor (ppm)
150
Accelerometer bias (mg)
1.0
Accelerometer scale factor (ppm)
300
Physical characteristics
IMU performance
Output rate (Hz) 50
Gyro bias (degrees/h)
Gyro scale factor (ppm)
5,0 00
Accelerometer bias (mg)
6.0
Accelerometer scale factor (ppm)
19, 700
The specifications of the SPAN-LCI (Novatel)
MIDG-II specifications

30. Experiment IMU-gyroscopes output IMU-accelerometers output
Calculated orientation

31. Experiment
Calculated orientation

32. Experiment
Attitude output

33. Experiment
Attitude errors

34. Experiment
accelerometer bias estimated by EKF

35. Experiment
gyro bias estimated by EKF

36. Conclusion
For the research, an auto balance system is design composed of a body, a low cost IMU, a microcontroller unit (MCU), and four motors.
The attitude determination and control system with PID is fed to the MCU to control the system to the expected attitude.
A quaternion-based method is applied to avoid singularity in calculation. To improve the heading accuracy, the integration of IMU and magnetometer is implemented using EKF for data fusion
To evaluate the performance of the proposed data fusion technique in attitude determination, two tests was implemented.
The test result indicated that with the proposed algorithm and the data fusion technique, the accuracy of the attitude determination improves significantly (about 30-50%) compared to the common algorithms

37. Thank You !
Dinh Quan , Trinh