====== "Extended Kalman Filter ”====== =====Electric Kalman Figthers===== **Dannier Castro León B21594 \\ Ernesto Céspedes Montero A31355 \\ Felipe Moya Coto B24609 ** ===== Introduction ===== This proyect is intended to be a helpful guide to those people who are planning on writing a Kalman Filter algorithm to estimate their robot's current state, given some noisy measurements in the environment and sensors that might affect the robot's localization. The intention of this work is to develop an EKF algorithm written in Python programming language, which will run on a Raspberry Pi. ===== Objectives ===== ==== General Objective ==== * Implement a program written in Python programming language that makes use of the EKF to estimate the copter's board current state. ==== Specific Objectives ==== * Explain the EKF as a probabilistic estimation filter and its application in robotics. * Estimate the statistic variables that describe the EKF, such as the standard deviation and the mean of the measurements of the copter's state. * Incorporate an EKF algorithm in a python program that considers the measurements given by the board and its noise, so the copter may be able to estimate its position using the probabilistic nature of the EKF. * Consider a simulated extern trusted measurement (visual odometry) to fix the calculations made by the EKF. ===== Implementation ===== ==== Getting the Pi ready to run the code ==== === Flashing your board === First off, we need to flash our board so the Raspberry Pi can recognize when it's connected. We have to add our user to the dialout group. $sudo adduser dialout After doing this, we need to install the minicom software to enable the serial communication with the board. $sudo apt-get install minicom Once the downloading process it's over, execute minicom. $minicom -s Now go to Serial port setup. Once there, set the serial device option to /dev/ttyUSB0 and set Bps/Par/Bits to 9600 8N1. Then return to minicom's main menu and save setup as dfl. Now your board is ready to flash. === MultiWiiConf === If you want to test your board, it might be necessary to install the MultiWiiConf software, that way you'll be able to calibrate the sensors of your board. In your system's console, download the software as follows: $ wget http://code.google.com/p/multiwii/downloads/detail?name=MultiWii_2_2.zip&can=2&q= After doing this, unzip and go to the application.linux32 or application.linux64 directory, and execute MultiWiiConf, according to your operating system's architecture. To do this, on your current directory type (we are assuming a 32 bit OS since we are doing this on a Raspberry Pi): $unzip -p MultiWii_2_2.zip $cd MultiWiiConf/application.linux32/ $chmod +x MultiWiiConf $./MultiWiiConf Once you are running MultiWiiConf, you can select the board's port and start testing your board, as well as you can calibrate the magnetometer with the CALIB_MAG option and the accelerometer with the CALIB_ACC option. === Some programs and dependencies === We'll mention some software that your Raspberry Pi will need to run the EKF. To begin with, YARP will be necessary to set the control variable of the Extended Kalman Filter algorithm. Since we don't have a real camera that estimates the state of the board, we'll simulate it by using YARP, which allows real-time communication with the board. To install YARP properly, we need to have installed cmake and xstow. In addition, we need some dependencies that will be needed to download and run the MultiWii software that comes with the board. $sudo apt-get install python cmake xstow libeigen3-dev git Since YARP's installation is a little bit tedious we'll just follow the steps of a guide previously made. So once you've done all this, do as the following link states in order to install YARP, [[http://www.raspberrypi.org/wp-content/uploads/2012/12/quick-start-guide-v1.1.pdf|Installing YARP in Debian]]. Another program that is needed is Orocos-kdl. To install it, write in your source directory's terminal: $git clone https://github.com/arcoslab/orocos-kdl.git $cd orocos-kdl $make install-python Another dependency that the code written will need is arcospyu. Then again, in your source directory: $git clone https://github.com/arcoslab/arcospyu.git $cd arcospyu $make xstow_install ==== Description of the Extended Kalman Filter ==== The Extended Kalman Filter is a linear quadratic estimation, it's an algorithm that uses a series of measurements observed over time, that contain noise (random variations) and other inaccuracies, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone. The algorithm that we ought to implement is shown in figure 1. {{ :ie0117_proyectos:final_2013:algoritmo.png?600 |Kalman Filter Algorithm}} As one might see in figure 1, the EKF consists of two main parts, the time update and the measurement update. The time update is a prediction of what the state vector will probably be, while the measurement update is a correction of the state vector (in this case, a position and velocity vector) given some noisy measurements that the sensors give. This algorithm receives two inputs, the control vector (considered as the visual odometry, given by the camera) and the measurement vector (which is given by the sensors). Since we have a greater confidence in the state given the camera, this will be in the correction stage as our control vector. On the other hand, the sensors of the board are considered to be noisy, so the measurements given by the board are to be in the prediction stage. So this means that:Uk=data given by the board. Zk=data given by the camera. The rest of the variables shown in figure 1 are matrices and vectors that are initial conditions for the state of the board and its respective covariances, or intermediary variables that are used by the filter. These variables and its values will be explained in the source code. However, there are some matrices that are not so obvious to estimate, so we’ll explain them furthermore. This is the case of matrices Q and R. === Covariance noise matrices Q and R === Because of the probabilistic nature of the EKF, it uses the noise covariance of the process and measurements, so it incorporates it to the algorithm. The covariance is just a matrix that includes all the variances of some dependent/independent events. If the events are independent, then the covariance is a diagonal matrix. The bigger the covariance, the bigger the uncertainty. On the opposite, the smaller the covariance, the smaller the uncertainty. To estimate these matrices Q and R, it was necessary to measure the noise and then calculate the covariance in those noises. So we created a method within the multiwii code of the ARCOSLab’s Github [[https://github.com/arcoslab/python-multiwii|python-multiwii]] (which contains all the code necessary to establish serial communication with the board), that estimates the noise in the measurements. This was done by using a method that takes the maximum and minimum value of the measurements, and it calculates its difference. The code for this method is shown down below. def filter_noise(self): xmax=array([0.]*3) xmin=array([0.]*3) angmax=array([0.]*3) angmin=array([0.]*3) print "WAIT... Calculating Noise..." a=50 #a=raw_input() for i in xrange(a): self.update_delta_time() #print "delta", self.delta_time error, resp=self.copter_serial.control(self.cmd) b=resp xmax=array(resp[1][3:6]) xmin=array(resp[1][3:6]) angmax=array(resp[1][:3]) angmin=array(resp[1][:3]) for i in xrange(500): self.update_delta_time() #print "delta", self.delta_time error, resp=self.copter_serial.control(self.cmd) if error==0: if resp[1][-1]>0: self.copter_control_freq=1000000./resp[1][-1] if resp[1][0] < angmin[0]: angmin[0] = resp[1][0] if resp[1][1] < angmin[1]: angmin[1] = resp[1][1] if resp[1][2] < angmin[2]: angmin[2] = resp[1][2] if resp[1][4] < xmin[0]: xmin[0] = resp[1][3] if resp[1][3] < xmin[1]: xmin[1] = resp[1][4] if resp[1][5] < xmin[2]: xmin[2] = resp[1][5] if resp[1][0] > angmax[0]: angmax[0] = resp[1][0] if resp[1][1] > angmax[1]: angmax[1] = resp[1][1] if resp[1][2] > angmax[2]: angmax[2] = resp[1][2] if resp[1][4] > xmax[0]: xmax[0] = resp[1][3] if resp[1][3] > xmax[1]: xmax[1] = resp[1][4] if resp[1][5] > xmax[2]: xmax[2] = resp[1][5] #self.averageang[0] += resp[1][0] #self.averageang[1] += resp[1][1] #print i print "Noise: " print angmax print angmin print xmax print xmin self.deltamag= angmax[2]-angmin[2] self.deltapos= xmax-xmin self.deltaang[0] = angmax[0]-angmin[0] self.deltaang[1] = angmax[1]-angmin[1] self.averageang[0] = (angmax[0]+angmin[0])/2 self.averageang[1] = (angmax[1]+angmin[1])/2 self.averagepos[0] = (xmax[0]+xmin[0])/2 self.averagepos[1] = (xmax[1]+xmin[1])/2 self.averagepos[2] = (xmax[2]+xmin[2])/2 print self.deltamag print self.deltapos print self.averageang self.posbefore[0]=0 self.posbefore[1]=0 self.posbefore[2]=norm(array(resp[1][3:6])) self.magbefore[0]=resp[1][2] self.angbefore[0]=resp[1][0] self.angbefore[1]=resp[1][1] Once the noise is measured, its covariance was estimated using another method covariance_calc_matrix(). Knowing that the variance is the square of the standard deviation, this method was implemented. def covariance_calc_matrix(self): measure_cov = array([[0.]*20,[0.]*20,[0.]*20]) xaverage = 0 yaverage = 0 zaverage = 0 print "Calculate Covariance Matrix, Press Enter: " raw_input() for i in xrange(20): error, resp=self.copter_serial.control(self.cmd) self.update_sensors() measure_cov[0][i] = resp[1][4] #*self.grav_cal_const*self.delta_time*self.delta_time/2 measure_cov[1][i] = resp[1][3] #*self.grav_cal_const*self.delta_time*self.delta_time/2 measure_cov[2][i] = resp[1][5] #*self.grav_cal_const*self.delta_time*self.delta_time/2 xaverage = xaverage*(float(i)/(i+1.))+(resp[1][4])*(1./(i+1.)) yaverage = yaverage*(float(i)/(i+1.))+(resp[1][3])*(1./(i+1.)) zaverage = zaverage*(float(i)/(i+1.))+(resp[1][5])*(1./(i+1.)) sum1=0 sum2=0 sum3=0 for i in xrange(20): sum1 = sum1+(measure_cov[0][i]-xaverage)*(measure_cov[0][i]-xaverage) sum2 = sum2+(measure_cov[1][i]-yaverage)*(measure_cov[1][i]-yaverage) sum3 = sum3+(measure_cov[2][i]-zaverage)*(measure_cov[2][i]-zaverage) self._R[0][0]= m.fabs(self._R[0][0]*sum1/20) self._R[1][1]= m.fabs(self._R[1][1]*sum2/20) self._R[2][2]= m.fabs(self._R[2][2]*sum3/20) print "Covariance Matrix" print self._R print "Click Enter to Calulate Position: " raw_input() === Initial assumptions of the EKF === There are two main state assumptions in the EKF. One of them, is the Xk-1 variable, which is the previous state vector, which is continuously updated in the algorithm. At time zero, this is assumed to be whatever you consider reasonable, in our case we set it to zero. The other assumption, is the Pk-1 variable, which is the previous covariance state matrix. You can set it to whatever you consider reasonable, this variable will be updated and fixed through time. We set it to an identity matrix. self._Xk=array([[0.],[0.],[0.]]) #Assume first position to be zero, it'll be fixed by the filter anyways self._Pk=array([[1., 0., 0.], #Assume first state covariance to be an identity matrix, it'll be fixed by the filter as well [0., 1., 0.], [0., 0., 1.]]) ==== Incorporation of the EKF ==== To make our code functional as it was intended, a class named Kalman_filter was created, so a constructor could be made. In this constructor, the state and transformation matrices are initialized. In addition to this, a method called kf_alg was made in the Kalman_filter class, which receives two inputs: the control and measurement variables. E. Kalman filter class: class Kalman_filter: def __init__(self,first_covariance,_A,_B,_Q,_H, _CR, _Xk, _Pk, _BC): self.Xk= _Xk self.R = first_covariance self.Pk= _Pk self.A=_A self.B=_B self.Q=_Q self.H=_H . . . E.Kalman filter algorithm def kf_alg(self,mv,cv): print "Xk: ", self.Xk print "Pk: ", self.Pk print "Current :" ,self.Q print "R, first: ", self.R #This is the prediction step, begin with state prediction predicted_state=dot(self.A,self.Xk)+dot(self.B,mv) #Continue with covariance prediction predicted_covariance=dot(dot(self.A,self.Pk),numpy.transpose(self.A))+self.Q #Proceed with innovation error innovation_error=cv-dot(self.H,predicted_state) #Innovation covariance innovation_cov=dot((dot(self.H,predicted_covariance)),numpy.transpose(self.H))+(self.R) #Calculate the Kalman Gain which will correct the state update estimate kalman_gain=dot((dot(predicted_covariance,numpy.transpose(self.H))),numpy.linalg.inv(innovation_cov)) #Update,state update and covariance update self.Xk=predicted_state+dot(kalman_gain,innovation_error) size=self.Pk.shape[0] self.Pk=dot((numpy.eye(size)-dot(kalman_gain,self.H)),predicted_covariance) #end kalman filter algorithm for some k state return self.Xk, self.Pk State and transition matrices values #Since the inputs and outputs are position vectors, then the state and transition matrices can be set to identity matrices # A - Previous state self._A=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # B - Control Matrix self._B=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) # H - No change self._H=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]) ===Camera Input implementation=== The filter requires a control variable (visual odometry) so it can fix the predicted state in the first part of the algorithm. This variable appears in the fourth equation as Zk. This is considered to be one of the two input to the filter's recursive algorithm. This control input, in our case, is considered as a camera, which gives the position of the board and it has a low covariance in its measurements (this is why we assume that it's the control variable). The real implementation of the visual odometry is out of our interests for this project, we just want to show how the filter works and its behaviour. So we chose to simulate the camera, which sends position data to the filter through YARP bottles, while the program is running. The code for the camera simulation is shown below. self.bottle=self.port.read(False) if not self.bottle == None: self.b=self.bottle.get(0).toString() a = self.b.split() self.cv = array([[float(a[0])], [float(a[1])], [float(a[2])]]) print self.cv ===== Results ===== Once the class Kalman_filter was implemented in the code, the code was ready to run. This is what the program looks like just after being run. Notice that an error linked to multiwii appears, however this can be fixed by launching the MultiWiiConf software before executing the filter code incorporated to the [[https://github.com/arcoslab/python-multiwii|python-multiwii]] program. {{ :ie0117_proyectos:final_2013:inicio.png?900 |}} To demonstrate how the Kalman Filter works, we set covariances matrices R and Q to some particular values, as well as we set the control variable to zero. In the first case, we wanted to show that a null measurement covariance R would make the filter trust the measurements given by the board. The process covariance matrix Q was set to an identity matrix, because the intention is to make the process covariance larger than the measurement covariance. Setting the covariance matrices R to a null matrix and Q to an identity matrix will cause the filter to consider the measurements as if they were trusted inputs. {{ :ie0117_proyectos:final_2013:fotocov1.png?300 |Figure 2}} As the picture shows below, the position given by the board is equal to the position estimated by the filter. {{ :ie0117_proyectos:final_2013:cov1.png?450 |Figure 3}} The second example is an intermediate case in which the process and measurement covariances are the same. We set both of them to identity matrices, you can set it to whatever you like as long as they´re equal. {{ :ie0117_proyectos:final_2013:fotocov2.png?300 |covariance second example}} In this case, because the covariance of the control input is the same as the measurement input, the filter will determine the mean of both values, that occurs when the two inputs are equally trusted. {{ :ie0117_proyectos:final_2013:cov2.png?500 | segundo ejemplo}} As a final demonstration, we set the covariance matrices so the filter would considere the control variable to be a more trusted input (with a variation of 0.01 mts) than the measurement variable (with a variation of 5 mts). {{ :ie0117_proyectos:final_2013:fotocovreal.png?300 |tercer matriz covarianza}} And this is how the filter should work given the covariances set in this third case, since the control input is not perfect and it has a covariance matrix larger than a zero matrix, the filter would still consider some of the data of the odometry not to be fully trusted. That´s why the filter throws position values in the X axis of -4 mts, while the board says that it´s located in -24 mts. The filter still believes that the control input has a better estimate about the position than the measurement variable. In our case, this is true because the sensor of the board that we are using is noisy compared to the visual odometry. {{ :ie0117_proyectos:final_2013:cov3.png?500 |tercer ejemplo}} ====== Conclusions ====== * It was possible to incorporate the Kalman Filter algorithm within the [[https://github.com/arcoslab/python-multiwii|python-multiwii]] code given by the ARCOSLab-Github. * Measurements on the covariance (standard deviation) could be made by using multiple methods created in the [[https://github.com/arcoslab/python-multiwii|python-multiwii]] code. * It is necessary to comprehend what each matrix in the EKF represents, so better approximations on the state of the board can be made. * Running the code with the EKF incorporated proved that EKF considers the control variable to be reliable data according to its respective covariance, while the measurement variable is considered to be noisy data due to its high covariance.