TELKOM UNIVERSITY E-PROCEEDING OF ENGINEERING : VOL. 2, NO. 1 APRIL 2015 1

IMPLEMENTATION OF KALMAN FILTERAND PID CONTROLLER FOR

INVERTED PENDULUM ROBOTMuhammad Royyan, Telkom University, Angga Rusdinar, Telkom University, and Susanto, Telkom University

Abstract—The development on inverted pendulum robot hasgained momentum due to their reliability when completingcertain tasks. Therefore variation of technique in dynamic envi-ronments movement is increasing. We present implementation ofKalman filter and PID on inverted pendulum robot.We design control system with Kalman filter and PID which willbe implemented and analyze its performance. The results clearlyshow how the adjustable parameters on the control system di-rectly affected the overall system performance. Because Kalmancan remove sensor noise, the almost reliable sensor data increasesPID controller performance to drive the robotic platform tovertical equilibrium. From the experimental result obtained theoptimal parameters. The optimal parameters of Kalman filterare Rmeasurement = 0.03, Qaccelerometer = 0.001, Qgyroscope =0.003. Whereas the optimal parameter of PID are KP = 9.0 ,KI = 2.0 , KD = 3.0.

Index Terms—Kalman Filter, PID , Inverted Pendulum.

I. INTRODUCTION

RECENTLY , researches on a combined system of amobile robot and an inverted pendulum system, the

mobile pendulum robot system are quite actively conducted.The mobile pendulum robot has not only a certain level ofdifficulty to control, but also practicality of the real worldapplications. Segway is the popular commercial product ofhuman carriers that carries humans to the desired destination.Segway is suitable for the place where human traffic isfrequent so that narrow turn is necessary such ac airportlobbies or where to move short distance [1].While Segway may have been a well-known commercialproduct, research into the control of such a mechanical systemhas been diverse. A two-wheel self-balancing robot is verysimilar to the inverted pendulum, which is an important testbedin control education and research; see, for example [2], [3].Besides the development of Segway, studies of two-wheel self-balancing robots have been widely reported. For example, JOE[4] and nBot [5] are both early versions complete with inertiasensors, motor encoders and on-vehicle microcontrollers.To make the mobile inverted pendulum robot system workwell, accurate sensing of current state of the system becomesmore important and has to be done before control. The balanc-ing angle of the pendulum can be measured by a gyroscopeor a accelerometer. In [6], an expensive commercial gyroscopehas been used to measure the pendulum angle and controlledsince the gyro sensor has the fast response.To replace with a low cost gyroscope, a drift problem ofthe gyroscope has to be compensated with the help of the

accelerometer. The gyroscope shows the fast response, butsuffers from drift due to accumulated errors by integrationto obtain angle information from acceleration. Meanwhile, theaccelerometer has a good steady state response, but has a slowresponse and suffers from noise in high frequency response.Therefore, combining the gyroscope and the accelerometertogether by considering different frequency responses is re-quired. The kalman filter compensates for those defects andhas been used for different frequencies to obtain accurate angleinformation .This paper is organized as follows: Section 2 describes thebasic theory that support the system which is used in thisstudy; Section 3 details the design of the system, such asgeneral overview of system, physical form of robot, Kalmanfilter process model; Section 4 present the experimental results,followed by some concluding remarks in Section 5.

II. BASIC THEORY

A. Inertial Measurement Unit

Inertial measurement unit (IMU) consist of accelerometersand gyroscopes. We use MPU 6050 that has 6-axis.

1) Accelerometer [7], [8]: The accelerometer is a sensorwill be used to measure both dynamic and static accelerations.To understand how it works, try to imagine a box in shape ofa cube with a ball inside it. Figure 1 represents no gravitationfields that might affect the ball’s position. Symbol on the wall

Fig. 1. Flying ball inside non-gravitational cube

of the cube represent of 3 dimensional axes. If cube is placedin place where gravity’s applied, the ball’s going to fall downand acceleration is red 1g which could be seen in figure 2. Ifthe cube is tilted, ball will touch two walls. This condition isshown in figure 3.

TELKOM UNIVERSITY E-PROCEEDING OF ENGINEERING : VOL. 2, NO. 1 APRIL 2015 2

Fig. 2. Gravity is applied

Fig. 3. Gravity is applied in tilted cube

2) Gyroscope [7], [9]: The gyroscope is another importantinertial sensor that is needed to achieve successful balancingcontrol of inverted pendulum robot. The gyroscope measuresangular rate and determines how fast the inverted pendulumrobot is falling in either side.

Fig. 4. Construction of gyroscope

B. Discrete Kalman Filter [10]

The Kalman filter addresses the general problem of trying toestimate the state x ∈ <n of a discrete-time controlled processthat is governed by the linear stochastic difference equation

# »xk = A # »xk−1 +B # »uk−1 +# »wk−1 (1)

with a measurement z ∈ <m that is#»zk = H # »xk + #»vk (2)

# »wk and #»vk represent the process and measurement noise(respectively).The Kalman filter estimates a process by using a form offeedback control: the filter estimates the process state atsome time and then obtains feedback in the form of (noisy)measurements.Kalman filter fall into two groups: time update equations andmeasurement update equations. The time update equations areresponsible for projecting forward (in time) the current stateand error covariance estimates to obtain the a priori estimatesfor the next time step. The measurement update equations areresponsible for the feedback.Final estimation algorithm resembles that of a predictor-corrector algorithm for solving numerical problems as shownin Figure 5.

Fig. 5. The ongoing discrete Kalman filter cycle

C. PID

The control algorithm that was used to maintain it balanceon the inverted pendulum robot is the PID controller. Theproportional, integral, and derivative (PID) controller, is wellknown as a three term controller. Each term performs adifferent task in the control process. The controller terms alsohave different effects on the system output response [11].Figure 6 shows the integration of the PID controller on a

Fig. 6. Closed Loop PID

general close loop control system which is implemented oninverted pendulum robot.

TELKOM UNIVERSITY E-PROCEEDING OF ENGINEERING : VOL. 2, NO. 1 APRIL 2015 3

III. SYSTEM DESIGN

A. General Overview of System

Figure 7 shows a block diagram that details the data flowbetween the electrical components. The block diagram alsodisplays the actual inverted pendulum robot system implemen-tation.

Fig. 7. Closed Loop PID

The system that has been designed and implemented hastwo input, those are tilt angle which is read by accelerom-eter and angle speed which is read by gyroscope. Becauseaccelerometer has noises and slow respone to changes in tilt,and gyroscope has a bias value. Kalman filter will be used tosolve this problem.Once reliable angle value is obtained, the next step is tocontrol the position of the dc motor to adjust the balanceof the robot. Angle value which is obtained by kalman filterbecome the input of PID in order to increase the stability andperformance. Speed readings become feedback of PID system.PID parameters derived from the method of trial and error.

B. Physical form of Robot

Robot consists of three iron plates measuring 7 × 17 cm.Three iron plate is associated with an adjustable spacer. Thespace between the plates provides the freedom to place theelectrical components and other hardware. The overall heightis 21 cm. Two DC motors are embedded in the bottom plate.

C. Kalman Filter Process Model

In order to make Kalman filter be implemented successfullyonto the inverted pendulum robot, an accurate model needs tobe developed. The process also needs to be modeled as a linearsystem . Kalman filter will use the data from the accelerometerto eliminate the drift problem from the gyroscope outputsignal. In the process of using the filter, unwanted noise fromthe accelerometer will either be minimized or completelyeliminated [12] [?]. It will be modeled as a single dimen-sional inertial measurement unit. A two state Kalman filteris implemented to track the angle of the inverted pendulumrobot and gyroscope bias value. The simple process model

Fig. 8. The ongoing discrete Kalman filter cycle

using the gyroscope input data can be modeled in state spacerepresentation as shown on equation 3.

# »xk = A # »xk−1 +B # »uk−1 +# »wk−1

# »xk =

(1 −4 t0 1

)# »xk−1 +

(4t0

)# »uk−1 +

# »wk−1(θ

θ̇b

)k

=

(1 −4 t0 1

)(θ

θ̇b

)k−1

+

(4t0

)θ̇k−1 +

# »wk−1(θ

θ̇b

)k

=

(θ + (θ̇ − θ̇b)4 t

θ̇b

)k−1

+ # »wk−1 (3)

The output of kalman filter will be shown in equation 4.

#»zk = H # »xk + #»vk

#»zk =(1 0

)# »xk + #»vk

#»zk =(1 0

)( θθ̇b

)k

+ #»vk

#»zk = θk + #»vk (4)

θ is the output of accelerometer and θ̇b is bias value ofgyroscope (drift). The input of kalman model is the output ofgyroscope ( # »uk = θ̇k).

IV. RESULT

This section talks about the results and the performanceevaluation of the inverted pendulum robot system. The resultswere logged by the serial data logger and plotted usingMicrosoft Excel.

A. Kalman Filter Results

As previously described, the main objective of using theKalman filter in an application is to eliminate most of mea-surement noise from the accelerometer sensor. The filter also

TELKOM UNIVERSITY E-PROCEEDING OF ENGINEERING : VOL. 2, NO. 1 APRIL 2015 4

helps to reduce the drift problem that the gyroscope sensorexperiences.For the filter to work properly, a manual tuning method needsto be carried out. The tuning filter parameters that needed tobe tuned are the covariance matrices Qw and Rv . The Qw

matrix consists of the process noise and the Rv matrix is themeasurement noise. Both matrices take the form as shown onequation 5 and equation 6.

Q =

(Qaccelerometer 0

0 Qgyroscope

)(5)

R =(Rmeasurement

)(6)

Values of Qaccelerometer and Qgyroscope are set dependingon how much confidence one puts on the particular sensor.A higher value indicates that the particular sensor is trustedless as compared to the other sensor. The value of theRmeasurement matrix indicates the amount of noise expectedfrom the measurement, a low value indicates that the measure-ment is less corrupted with noise.The filter tuning method used on this project was done bytrial and error. The simple test platform is used for theKalman filter experimentation. Both the gyroscope and theaccelerometer were rotated at angles between +1- 10 degrees.Before the Kalman filter can be implemented and tested, theaccelerometer and gyroscope raw sensor data has to be scaled.

From Figure 9 it is clearly shown that the filter output

Fig. 9. Not properly tuned Kalman filter.

response is not tuned properly. The filter resembles a sawtooth waveform. The filter doesnt track the true accelerometerangle in real time. The output of the Kalman filter has avery slow response to changes from the accelerometer data.This large delay in Kalman filter can be a major problemwhen the estimated accelerometer angle is fed into the PIDcontroller. To solve the slow output response of the Kalmanfilter, the Qaccelerometer was set to have a lower value thanthe Qgyroscope. In this case the accelerometer sensor data willbe trusted more than the gyroscope data.From Figure 10, it is observed that the filter output response

has improved. The Kalman filter tracks the accelerometersensor data in real time. A problem was observed in Figure 10,

Fig. 10. Kalman filter output response varying Q matrix.

when the filter is initialized; the filter tends to have a dramaticovershoot. After approximately 5 seconds, the filter recoversfrom the overshoot. The other problem that was observed, fromthe figure, is that every time there is a change in angle there areovershoots that follows. The Kalman filter does an excellentjob eliminating the overshoots when the filter is operating.These overshoots can become an issue if the estimated Kalmanangle gets fed into the PID controller. The solution to the

Fig. 11. Optimal Kalman filter Q matrix values.

overshoot problem can be solved by adjusting the processnoise covariance matrix parameter Qgyroscope. After numeroustrials and errors, the optimal covariance matrix Qw valueswere set as Qaccelerometer = 0.001 and Qgyroscope = 0.003.These process noise covariance matrix values offer the bestfilter output response. Figure 11 shows the results for thesematrix values.In Figure 12, the Rmeasurement was set to 0.000003. The

waveform results clearly show that the Kalman filter has anoutput response that is similar to the accelerometer waveform.This type of Kalman filter response has the same amount ofnoise as the raw accelerometer data. The noisy filter datacannot be used in the balancing robot system. To solve thenoisy Kalman filter data, the measurement noise covariancematrix was set to 0.03. Figure 11 displays the waveform when

TELKOM UNIVERSITY E-PROCEEDING OF ENGINEERING : VOL. 2, NO. 1 APRIL 2015 5

Fig. 12. Kalman filter output response varying R matrix.

the measurement noise covariance matrix is set to 0.03.When using the Kalman filter, numerous trial and error needto be carried out to find the optimal values. The Kalman filtercovariance matrix values that were chosen to give the bestresults were hard coded into software. The measurement noiseand the process noise covariance matrix values are listed asfollows:

• Rmeasurement = 0.03• Qaccelerometer = 0.001• Qgyroscope = 0.003.

These filter parameters offered the best performance andresults. Figure 11 shows the Kalman filter results when usingthe parameters listed.

B. PID Controller Results

The objective of the PID controller is to provide controland stability. The PID consists of three gain terms that affectthe output response of the system. The proportional KP gaindetermines the rise time of the system. The integral KI gainaffects the steady state error. The derivative KD gain controlsthe system response overshoot and helps with its stability.Figure 13 displays the output response result captured for theinverted pendulum robot.

Fig. 13. Measured system output response.

Results from Figure 11 show the estimated Kalman filterangle measurement. This plot also displays the closed loopoutput response for the robot system. Figure 11 displays howrobot tried to maintain a stable state by hovering around thezero radian value. The measured angle hovered around the zeroradian value. The figure also shows that the first few secondsthe system response had an overshoot. This initial overshootis caused by the initial Kalman angle estimate, but the systemsoon recovers from the overshoot. The output response of thesystem still suffers from the noise generated by the sensors.All efforts were made to minimize as much of the signal noiseas possible.The PID gain values that were used for the PID controller are:

• KP = 9.0• KI = 2.0• KD = 3.0

V. CONCLUSION

Using lost cost components in this type of project cansometimes lead to drawbacks, in this case from both theaccelerometer and the gyroscope. Both sensors experienceinitial problems that made them difficult to use in this typeof project. The accelerometer data was highly corrupted withunwanted noise. The gyroscope suffered from a problem thatmade its bias value drift away with time. A solution to theproblems associated with these sensors outputs can be foundin the use of a Kalman filter. This filter is used to minimize andtotally eliminate the unwanted output results from the sensors.The PID controller relies on the input signal to be almost freeof noise.

REFERENCES

[1] Segway. Available: http://www.segway.com[2] R. Fierro, F. Lewis, and A. Lowe, Hybrid control for a class of

underactuated mechanical systems, IEEE Transactions on Systems, Manand Cybernetics, Part A: Systems and Humans, vol. 29, no. 6, pp. 6494,nov 1999.

[3] K. Xu and X.-D. Duan, Comparative study of control methods of single-rotational inverted pendulum, in Proceedings of the First InternationalConference on Machine Learning and Cybernetics, vol. 2, 2002, pp. 7768.

[4] F. Grasser, A. DArrrigo, S. Colombi, and A. C. Rufer, JOE: A mobile,inverted pendulum, IEEE Transactions on Industrial Electronics, vol. 49,no. 1, pp. 10714, 2002.

[5] D. P. Anderson. (2003, Aug.) nBot balancing robot.[Online].Available: http://www.geology.smu.edu/dpa-www/robo/nbot

[6] Sungsu Kim and Seul Jung, Control experiment of a wheel-driven mobileinverted pendulum using neural network,IEEE Trans. On Control SystemsTechnology, vol. 16, no. 2, pp. 297-303, 2008

[7] Permadi,T.A.,2014. Balancing System of Tray on Waiter Robot usingComplementary Filter and Fuzzy Logic, Bali, IAICT 2014.

[8] Baluta, Sergiu (2009, Dec). A Guide to Using IMU (Accelerometer andGyroscope Devices) in Embedded Applications..[Online].Available: http://www.starlino.com/imu guide.html

[9] Wikipedia (2014, May). Gyroscope. [Online].Available: http://en.wikipedia.org/wiki/Gyroscope

[10] Welch,Bishop,2010. An Introduction to the Kalman Filter.Available: http://www.cs.unc.edu/welch/kalman/kalmanIntro.html

[11] Johnson,Michael A.; Moradi,Mohammad H. (2005). PID Control: NewIdentification andDesign Methods. London.

[12] Colton, Shane (2007, Jun). The Balance Filter, A Simple Solution forIntegrating Accelerometer and Gyroscope Measurement for a BalancingPlatform..[Online].Available: http://www.chiefdelphi.com/media/papers/download/2059