IMPLEMENTATION OF KALMAN FILTER ON VISUAL TRACKING USING PID CONTROLLER

Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 41 I MPLEMENTATION OF K ALMAN F ILTER ON V ISUAL T RACKING U SING PID C ONTROLLER Abdurrahman,F.* 1 , Gunawan Sugiarta* 2 and Feriyonika* 3 * Department of Electrical Engineering, Bandung State of Polytechnic, Bandung, Indonesia A BSTRACT This paper explain the design of visual control system which equip Kalman filter as an additional subsystem to predict object movement. It is a method to overcome some weakness, such as low range view of camera and low FPS (Frame per Second). It alsoassist the system to track a fast moving object. The system is implemented to 2 motor servos, which are move on horizontal and vertical axis. Digital PID (Proportional, Integral, and Derivative) controller is used in the system, and bilinear transformation is used to approximate the value of derivative in transforming the analogue to digital controller on ztransform.In conclusion, we can get the value of system responses time from both motor servos. The rise time and settling time of motor servo in horizontal axis are 0.402s and 1.63s, and vertical axis’s responses are 0.38s and 1.34s. K EYWORDS Kalman filter, PID, bilinear transformation, z-transform 1. I NTRODUCTION In this digital era, robotic technologies are implementing vision-based control, such as object tracking [1], face tracking [2], and position-based control [3]. It was developed as one of the control method in robotic community. Researchers keep learning and involving the control system to transform itas close as human capability. Recent paper describes the vision-based control and computer vision as numerical theory [4], unfortunately there are lack of paper which describedand design the system in a more practical way. There are many add-on systems in vision-based control to improve accuracy, such as image filter and predictive filter. To implement such systems, which will be used onvarious environment with lots of noises, the method should therefore always be develop to reduce noises. So, there are many researchers in this field to improve the system’s capability. Gian Luca designedimage-based visual servo using geometry [5] in mobile robot and combined it with kinematic design.This paper will explain about a visual tracking system controlled by visual control using PID (Proportional, integral, and derivative) control law to move plants in horizontal and vertical axis. It also combined with Kalman filter to predict the object movement within camera frame. Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 42 The purpose of the system is to predict the object movement, so it can make the system work in a better performance to predict the object movement while it is not detected. The system will not search the object while it was outside the range of vision of the system without using kalman filter. The control law (PID), filter, and feature extraction will be designed in one processor. We will use 2-D in feature extraction, there are two axis which are being the feature for control system. The feature extraction method is contour in color detection, however it will not be discussed in this paper. This paper will concern to the design in development of visual control in robotics using the predictive filter. This paper proposedon how to implement and design the system in a more practicalway.According to the passages above, we canconclude that the advantages of the proposed system are: − The digital PID algorithm will be designed using z-transform which make it easier to understand and implement. − Kalman filter will be used to predict object movement, so it will help the system’s responsewhen tracking faster object. Kalman filter is useful on system arrangement using low resolution and low FPS camera. It will be useful if the camera frame which used in the system doesn’t have enough resolution and bad fps (frame per second) [6]. This paper is organized in 5 sections. Section 2 and 3 explain about the digital control system design and Kalman filter theory. Section 4 will discussed about the design from the control system perspective and the next section contains results and data analysis comparingdata between system with Kalman filter and nowithout Kalman filter. Finally, the last section is about the conclusion of all the system. 2. D IGITAL C ONTROL S YSTEM D ESIGN In this section, we will review the digital control system. Z-transform can be used to design the digital control system. To transform the analogue to digital system, the system must following these requirements [7]: − A stable analogue system must transform to a stable digital system. − The frequency response of the digital system must closely resemble the frequency response of the analogue system in the frequency range 0   2 ⁄ where   is the sampling frequency. There are many methods to transform the analogue to digital system, consist of: 2.1 Differencing Method The system can be represented by a transfer function or differential equation. Numerical analysis provides standard approximations of the derivative so as to obtain the solution to a differential equation. There are 2 types of the differencing method [7], such as: − Forward differencing In forward differencing the derivative approach will found through the basic equation 2.1: Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 43 =   [ + 1− ] (2.1) After that, the equation will be transformed into z-domain, so the equation 2.2 will be produced: →   [ − 1] (2.2) Based on this method, we could find the relation between Laplace and z-transform. The relation showed on the equation 2.3,with T is a time sampling  →   (2.3) − Backward differencing The other differencing method is backward differencing with the approximation of the derivative is showed on the equation 2.4 =   [−  − 1] (2.4) The equation will be transformed into z domain, so we can get the relation between Laplace and z domain in equation 2.5  →   (2.5) 2.2 Bilinear Transform Equation 2.6 is the relation between Laplace and z-transform [7] using bilinear transformation  =    (2.6) The relation can be found from the equality z =   using the first-order approximation on the equation 2.7 where the constant c = 2/T  =   ln=   [   ] (2.7) 3. D ISCRETE K ALMAN F ILTER Kalman filter is essentially a set of mathematical equations that implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the estimated error covariance. There are 2 general processes to implemented, such as time update and measurement update. The time update equations can be thought of as predictor equation, while the measurement update equations can be thought of as corrector equation. Indeed the final estimation algorithm resembles that of a predictor-corrector algorithm for solving numerical problems [8] as shown in Figure 1. Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 44 Figure 1. Process of Kalman Filter Kalman filter can be use in visual control to track the object. Normally, a tracking algorithm would be able to locate the object anywhere within the image at any point in time. However, only a limited region of the image is searched. Visual tracking will be failed if the object was moving to outside the region of camera frame and movetoo fast. Kalman filter can predict the object movement and solve these problems [6]. To implement discrete Kalman filter [8], we need to write down these equations into algorithm: − Time update First task is to determine the prediction value (xk), followed by error covariance (pk). There are other variables, such as A, B, uk, and Q. These variables can be assumed as constant variables.  = !" # + $% # & = !& ' ! ( + ) − Measurement update The process continued to determine Kalman gain (kk), error covariance (pk), and prediction value (xk). Xk will be the output of Kalman filter and used as feedback in control system. H can be determined as identity matrix, furthermore R is standard deviation. The method determine variable R is various, variable can be considered as a constant value or measured using the equation. * # = & + ( & + ( + ,   =  + * # - − +  & = . −* # +& 4. S YSTEM D ESIGN The system was designed to control 2 plants, consist of servo in horizontal axis and servo in vertical axis. These were controlled using PID controller and Kalman filter as feedback to move camera. The output of camera is an image frame which is proceed by computer vision technology before proceed to Kalman filter. Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 45 Figure 2. Block Diagram of Control System Figure 2 is the block diagram of the control system that explained on the proceeding section. Each block in the system has their own function. Set point has 2 values, there are x and y coordinate, which are proceed by their own PID block. Then, the output PID will be converted into degree and give instructions to servo to move to certain position. Servo has its own controller to activate the DC motor, so motor servo can determine its position. A Camera will be attached to a servo, therefore the servo’s position also shows the camera frame. The object will be recognized using image processing algorithm in feature extraction and release the x and y coordinate. Figure 3 is illustrating the hardware of the system using two servos (white) and the camera (green) so the system can move the camera in 2 axis, such as horizontal and vertical axis. Figure 3. Horizontal and Vertical of Motor Servos Kalman filter will proceed both coordinates in the same time, it means the coordinates will be merged. Kalman filter will be designed using the program. Finally, the output of the filter will be used as feedback in the whole system. Subsection below will explain 2 most important components in control system, such as digital PID (Proportional, Integral, and Derivative) controller and Kalman filter. PID controller (horizontal axis) Kalman Filter Feature Extraction +- Set Point (x,y) PWM converter (horizontal axis) +-Controller Motor DC Camera Servo (horizontal axis) Image Frame PID controller (vertical axis) PWM converter (vertical axis) +-Controller Motor DC Servo (vertical axis) Vertical position Horizontal position x coordinate y coordinate Coordinate (x,y) x coordinate y coordinate Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 46 4.1 Digital PID Controller Z-transform was implemented in the digital control system to design the algorithm. Equation 4.1 is the PID basic equation in Laplace. /= *0 + 12  + *3 (4.1) According to the equation, z-transform was applied using bilinear transform, then it will produced the equation 4.2. There is Ts variable in the equation, which is a time sampling. The time sampling has been measured with the value of 0.09second. 4 5 = *0 + 1 6  7    + *3.   7   (4.2) 9 :=*0 − 1 + 1+ 12.   + 1  + *3.;.2. − 1   − 1 + 1 →<0 + <1  + <2  >0 + >1  + >2  Equation 4.3 is the PID equation in z domain before simplified. We can get six variables, consist of b0, b1, b2, a0, a1, and a2. Each variable will be determined using the equation 4.3 until equation 4.8. <0 = *0 + 12.  + 2.*3.; (4.3) <1 = *?.; − 4.*3.; (4.4) <2 =−*0 + 12.  + 2.*3.; (4.5) >0 = 1 (4.6) >1 = 0 (4.7) >2 =−1 (4.8) Therefore, these equations can be substituted to be the final equation, which is equation 4.9 to implementPID (Proportional, Integral, and Derivative) algorithm in digital. >0.9+ >1.  .9+ >2.  .9 = <0.:+ <1.  .:+ <2.  .: 9=−>1.  .9 >0 −>2.  .9 >0 +<0.: >0 +<1.  .: >0 +<2.  .: >0 %= A AB %[ − 1] − A AB %[ − 2] + CB AB [] + C AB [ − 1] + C AB [ − 2] (4.9) The control system is a control loop, so Ziegler-Nichols type 2 was used to find the PID parameter. However, we need to tune it manually to get the best responses time. This method proposed as practical procedure to find the parameter properly. 4.2 Kalman Filter Kalman filter was designed to estimate the moving object which caught in camera frame. This filter is really beneficial in the system with a small visual angle of camera. Equations below Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 47 explain the Kalman filter process, there are 2 general process in the algorithm, such as prediction which is explained by the equation 4.10 until equation 4.11 and correction.  = !" # + $% # (4.10) & = !& ' ! ( + ) (4.11) * # = & + ( & + ( + ,  (4.12)  =  + * # - − +  (4.13) & = . −* # +& (4.14)  = prediction value & = error prediction % # = control input A = transition matrix B = system velocity There are 2 variables that can be changed to adjust the filter’s responses, i.e. Q and R. Best responses will be depend on plant and the capability of the algorithm. Q will be a random value depend on the environment, however R will be found using the basic equation of standard deviation, there is: "D = " B +"  +"  +" E +" F :5 I = J K LD M   (4.15) We take 5 samples to find the standard deviation. 5 samples were the most appropriate among all. If we sample more than 5, a minor change will not affect the system. 5. R ESULT AND A NALYSIS In this section, we present the simulation and analysed the data. There are 2 types of data which will be analysed, consist of filter data and PID parameters. MATLAB R2013a is used to monitoring the data in graphs. Kalman filter need to analyse before the control system. According to Figure 4, Kalman filter has fast responses, however Kalman filter can search the object whether the object is not detected. Kalman filter will search the object depend on the last velocity of the object. The filter is also used to reduce noises (simulated by shaking the object), nevertheless there are no noises in system, so it will not be used. Kk = Kalman gain H = transformation matrix R = standard deviation of noise measurement Q = proses noise - = input kalman Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 48 Figure 4. Kalman response (first tuning) We determine the Q equal to 5.0 and R equal to standard deviation for each 5 sampling data. If we increase Q to 20, we will get a faster response than before as shown inFigure 5.On actual process we implemented the dynamic camera (camera is moving), a slow responses Kalman will only helpful in the static camera. Fast responses are not good enough in dynamic camera, because plant will move the camera outside the moving range harshly if the object is not detected. Figure 5. Kalman response (second tuning) So, we can conclude that Q should equal to 5.0. After the value of Q has been fixed, the control system will be designed. According to the section 3, we will use a digital PID (Proportional, Integral, and Derivative) controller in bilinear transform. First of all, the controller will be designed using Ziegler-Nichols type 2. We need to find the ultimate gain using P controller in this method, we should made the system oscillating constantly, Input kalman Output kalman Object is not detected on camera frame Object is shaking Mechatronics and Applications: An International Journal (MECHATROJ), Vol. 1, No.1, January 2017 49 and then we got two variables, such as ultimate gain andPeriod of oscillation. After that, we will find the PID parameters using these equations: *0 = 0.6O*% (5.1) ;? = P% ∶ 2 (5.2) ;3 = P% ∶ 8 (5.3) According to the measurement using the equation 5.1 until equation 5.3, we got the PID parameter shown in Table 1 for horizontal axis and Table 2 for vertical axis. Table 1. PID (Horizontal Axis) Parameter using Z-N method K P T I T D K I K D 2.188 1.051923 0.075938 2.08 0.318026 Table 2. PID (Vertical Axis) Parameter using Z-N method K P T I T D K I K D 1.902 0.36413 0.100342 5.2234 0.37146 We control motor servo, which has its own controller. After some experiments, we conclude that PD control law should be used to control the position of motor servos, so Ki is changed to 0. Control system moves 2 motor servo, such as: - Motor servo in horizontal axis - Figure 5 (A) shows the system responses using Kalman filter. There are a lot of noises because the environment condition which affect the image processing, furthermore there is a state where the object moving outside the camera frame, then Kalman filter try to find the object. Table 3 is the result of system’s response in horizontal axis using Kalman filter. Figure 6. (A) PID Response (horizontal axis) (B) PID Response without Kalman Filter Set Point System response Object is moving Noises Set Point System Response Object is Moving Noise