Tuning a GPS/IMU Kalman Filter for a Robot Driver

Transcription

1 Tuning a GPS/IMU Kalman Filter for a Robot Driver Jamie Bell, Karl A. Stol Deartment of Mechanical ngineering The University of Aucland Private Bag Aucland 1142 jbel060@ec.aucland.ac.nz Abstract This research tested different ways of tuning the rocess noise covariance matrix of a GPS/IMU extended Kalman filter. This Kalman filter was develoed for a retrofit robot driver. Because of the constraints of this alication, the Kalman filter had no rocess model for the oututs of the robot s controller. In this situation, a significant roortion of the rocess noise would be caused by the robot s lanned manoeuvres. Since the robot driver controls these manoeuvres in a somewhat redictable manner, the otimisation of the rocess noise covariances was carried out offline. valuating the erformance of a given rocess noise covariance matrix was achieved by comaring the true values of states to the state estimates gained while running the Kalman filter with simulated GPS and IMU data. The rocess noise covariance matrix was otimised using a genetic algorithm. Three hyotheses were tested using this otimisation rocedure and all were found to be true. The hyotheses were that multiobjective otimisation should be used, that the vehicle manoeuvres were the ey source of rocess noise for the robot driver and that different manoeuvres require different rocess noise covariance matrices. The roosed tuning rocedure for a robot driver s GPS/IMU Kalman filter was successfully field tested. 1 Introduction Kalman filters are emloyed extensively for sensor fusion. These Kalman filters roduce estimates of the states of a system by combining the data from a variety of different sensors. The resulting state estimates may be more accurate than those that would be roduced without sensor fusion. There is well established theory for develoing Kalman filters for the integration of data from Global Positioning System (GPS) and Inertial Measurement Unit (IMU) sensors [Farrell and Barth, 1998; Grewal et al., 2001]. evertheless, one of the ey challenges of roducing a reliable Kalman filter is adequately tuning its arameters [Powell, 2002]. When educated guesses are used, trial and can result in satisfactory Kalman filter erformance in some alications; however, this aroach can be time-consuming and unreliable [Bolognani et al., 2003]. More conventional techniques that roduce deendable arameters include adative Kalman filtering and offline stochastic metaheuristic searches. Adative Kalman filters tune arameters online. Using this method, arameters are altered according to how accurately the internal equations of the filter can redict the measurements of the system [Loebis et al., 2004]. Fuzzy adative Kalman filters have been found to erform well in exeriments [Hu et al., 2003; Xiong et al., 2005]. Offline otimisation of the arameters may be carried out using metaheuristic techniques. For examle, the genetic algorithm may be used to otimise arameters by searching the feasible solution sace for a combination of values that gives the best erformance of the Kalman filter [Chan et al., 2001; Gueye et al., 2005]. Metaheuristic methods are the subject of this aer because they have been found to suit the unique alication for which the Kalman filter is being created. The alication is roducing localisation and heading data for a robot driver. What is unique about this alication is that the robot driver will only erform a limited number of set manoeuvres. Hence, given some sensor data for the robot driver erforming these manoeuvres, the GPS/IMU Kalman filter can be tuned offline using metaheuristics. 2 The xtended Kalman Filter The extended Kalman filter, which is used in this research, consists of five recursive equations [Welch and Bisho, 2004]. The first of these is given here: where ( x u ) x = f 1, 1, (1) x holds the redictions of the system states, x 1 holds state estimates from the last time ste, u 1 holds the inuts to the system and f x u is a nonlinear function. ( ) 1, 1 This first equation is a rocess model for the system. It gives a rediction for the states at the current time ste based on the revious estimate of the states and the inuts at the last time ste. The Jacobian of this nonlinear function, with resect to the states is given by: f A ( x u ). (2) [ i] [ i, j] = 1, 1 x [ j] 1

2 The second equation of the Kalman filter is then: T P A P 1A + W Q 1 T = W, where P is the covariance matrix for x, P 1 is the covariance for x 1, W is the vector of system disturbance, is the rocess noise covariance matrix. Q 1 The next three equations of the Kalman filter correct the redictions made by the first two equations. The third equation calculates the Kalman filter gain: where K H V R T T ( H P H + V R V ) 1 T K = P H, is the Kalman gain, is the linearised measurement model, is the vector of measurement disturbance, is the measurement noise covariance matrix. The fourth equation of the Kalman filter calculates the state estimates. These estimates are the ey oututs of the Kalman filter. The equation is: where ( z h( x ) = x + K x, is the vector of measurements, h is the measurement model for the system. z ( ) x Finally, the corrected covariance matrix is found by: = I K H P P ( ). Once initial estimates for the corrected state estimates and covariance have been established, the Kalman filter equations form a continuous loo. The three corrector equations follow the two redictor equations, with state estimates being outut every time ste. 3 The Robot Driver Alication In this research an extended Kalman filter has been develoed to combine data from GPS and IMU sensors. This Kalman filter is a art of the navigation module of a retrofit robot driver, which has been develoed for the DARPA Grand Challenge race by the Grand Challenge Z team. This robot is straed into the driver s seat of a vehicle. It has actuators for a steering wheel, accelerator edal, brae edal and an automatic gear shift. The robot driver is designed to be transferable from one vehicle to another. There are some unique features about driving with this robot that influence the creation of the Kalman filter. Firstly, there is no reliable model for how the oututs of the robot s high level control affect the states of the system. Ideally, such a model would be included in the Kalman filter, with the oututs of the robot s controller being fed into the first equation of the filter (equation 1) as the inuts. However, since there is no set vehicle for which the robot has been designed and because the actuators on the robot are continuously evolving, the machinery is always (3) (4) (5) (6) changing. Hence, the relationshi between the controller and the system states is unredictable. The absence of such a relationshi affects the Kalman filter because it introduces significant rocess noise. This rocess noise would have otherwise been accounted for as inuts to the system in the first equation of the Kalman filter. Another unique feature of the retrofit robot driver is that some additional information about the rocess noise of the system is nown. Because there is no relationshi between the controller oututs and the system states, the rocess noise is to some degree deendant on the dynamic behaviour of the vehicle. Because the dynamic behaviour of the vehicle is lanned by the higher level control of the robot driver, the rocess noise is somewhat redictable and can be anticiated. There are two imortant results of such redictability: the oututs of the higher level control may be used to influence the rocess noise arameters in the Kalman filter online and offline otimisation of these same arameters can be emloyed. The final ey unique feature of the robot driver alication is the controllability of the driving. The robot driver has two ey tyes of aths that it has to follow: bends and straights. ach time the robot follows one of these tyes of aths, the behaviour of the robot is not accurately reeatable but is, nevertheless, to some extent redictable. The fact that the robot only follows two tyes of aths means that the comonents of the rocess noise that are created by these manoeuvres can be anticiated. Limiting the number of tyes of manoeuvres also decreases the variation in rocess noise. When the robot drives along a straight, it controls the vehicle to maintain a set constant seed and a set angle. Therefore there is very little rocess noise in terms of lanned manoeuvres when the vehicle travels along a straight ath. In contrast, when the robot drives around a corner, it controls the vehicle to decelerate at a set rate, to maintain a constant seed through a constant radius turning circle and to turn through 90 degrees before accelerating again at a set rate. Therefore, there is considerable rocess noise due to the lanned manoeuvres when the robot driver turns a corner. However, this noise is quite redictable because the manoeuvre is lanned by the robot. An adative Kalman filter in this context would mae use of historic data, which would be measured online. Offline otimisation of the rocess noise covariance matrix is aroriate in this alication because some rior nowledge about the rocess noise is nown. It may also be ossible to use different rocess noise covariance matrices for different lanned manoeuvres. 4 Research Hyotheses This research investigates three hyotheses. Firstly, that multiobjective otimisation is needed for offline otimisation of Kalman filter arameters. Secondly, that the lanned aths of the vehicle are the dominant sources of rocess noise and hence using these is sufficient for tuning the Kalman filter. Thirdly, that different rocess noise covariances are beneficial for the different lanned manoeuvres of the robot driver. These hyotheses are tested individually in simulation and the resulting rinciles are demonstrated in field tests. 2

3 5 Research Tools In this research a genetic algorithm was used to find an otimal rocess noise covariance matrix for a GPS/IMU Kalman filter. To do this, simle aths, made u of the robot driver s two manoeuvres, were created. Then, deending on the exeriment, these aths may have been followed by a vehicle model, which was simulated to behave as though the robot driver were in control. Simulated sensor measurements for the aths travelled were created. These measurements were reeatedly run in a Kalman filter, while the genetic algorithm altered the rocess noise covariance matrix from reetition to reetition. By comaring the states outut by the Kalman filter with the true values of the states, the erformance of each rocess noise covariance matrix in the Kalman filter was evaluated. Using these evaluations, the genetic algorithm directed the search for the otimal set of arameters. The way in which these research tools fitted together in the otimisation rocess is shown in Figure 1. 1 Create Simulation Data 1.1 Plan a ath with bends and straights 1.2 Simulate the robot driving the lanned ath (otional, deending on the exeriment) 1.3 Simulate sensor measurements for the ath considered (either the lanned ath or the ath travelled by the robot) 2 Run Genetic Algorithm create five sets of data five sets of data reeat 4000 times 2.1 valuate rocess noise covariance matrices reeat for five sets of data Run the Kalman filter for the rocess noise covariance matrix Comare the Kalman filter state estimates with the true values at each time ste averaged results 2.2 Create new oulation of rocess noise covariance matrices Figure 1: Research tools in the otimisation rocess. There were five ey tools used in this research: a simle vehicle ath lanner, a robot driver simulator, a simulated sensor data generator, an extended Kalman filter and a genetic algorithm. The three simulators were used to create data for the Kalman filter. The genetic algorithm was used to find otimal arameters for the Kalman filter. Then a coy of the Kalman filter and sets of searately created test data were used to finally trial the otimised arameter values. A diagram of how the research tools were used in the testing stage is given in Figure 2. This rocedure was very similar to the otimisation rocess, though without the genetic algorithm. ote that all testing data was et the same throughout all of the exeriments. All five tools used in this research are described in more detail in the subsections that follow. 1 Create Testing Data create five sets of data 1.1 Plot a long ath with bends and straights 1.2 Simulate the robot driving the lanned ath 1.3 Simulate sensor measurements for the ath considered five sets of data 2 Test Otimised Process oise Covariance Matrix reeat for five sets of data Run the Kalman filter for the rocess noise covariance matrix Comare the Kalman filter state estimates with the true values at each time ste averaged results Figure 2: Research tools in the testing stage. 5.1 The Path Planner The vehicle ath lanner rogram lotted oints for ideal movements of a vehicle. Three lanned aths were defined at a samling frequency of 1 Hz (Figure 3). Two shorter aths and one longer ath were created. The aths were defined not just in terms of the ositions in a orthing asting coordinate system; the accelerations in the body coordinate system, the angular velocity at every time ste and the heading of the vehicle were also calculated. The aths generated began at a constant seed in a straight line, which was followed by a eriod of constant deceleration, then turning around a corner at a constant radius and seed, which was followed by a eriod of constant acceleration. The aths roduced by the ath lanner were used to create data for the otimisation and testing stages of the exeriments in this research. 3

4 Paths Created by the Path Planner orthing (m) Long Short 1 Short asting (m) Figure 3: The three aths generated by the ath lanner. The aths were defined so as to mimic real aths that the robot might have to traverse. It is thought that, since all ossible aths could be constructed from aths Short 1 and Short 2 or their mirror images, these should be reresentative of the robot driver aths. The values of the seeds, accelerations and the turning circle radius were chosen to be realistic. For the aths lotted by the ath lanner, the seed along the straights was made to be 40 m/h. The rate of deceleration into a corner was taen to be 1 ms -2 and the same rate of acceleration was used out of a corner. The turning circle at the corners had a radius of 6.4 m. The seed around the corners was set at 7.2 m/h. The two shorter aths were only used to create data for otimisation. The test data and the otimisation data for exeriment 2 was created from the longer ath. 5.2 The Robot Driver Simulator The robot driver simulator generated an outut of the states of the system as though the robot was driving a vehicle. The inuts to the robot driver simulator came from the ath lanner. The simulator roduced state oututs that would be exected if the robot were following the ath from the ath lanner. The states outut were osition, velocity, accelerations, heading and angular velocity. The urose of the robot driver simulator is to generate realistic driving data for each lanned ath. The comonents of the simulator created in MATLAB/ Simulin are: a vehicle dynamics model, a trajectory tracing controller, and first-order actuator dynamics models. The vehicle is modelled as a lanar rigid body with non-holonomic inematic constraints imosed by standard Acermann steering. An engine torque ma, aerodynamic drag and rolling resistance are included. The trajectory tracing controller uses PID control for seed tracing and ure ursuit for ath tracing [Shin et al., 1992] The Sensor Simulator The sensor simulation rogram created realistic GPS and IMU data from the oututs of either the ath lanner or the robot driver simulator, deending on the exeriment. These oututs had to include ositions, accelerations and angular velocities. GPS data (GPS) was made by adding noise (n) to vehicle ositions () in a orthing (denoted by subscrit ) and asting (denoted by subscrit ) coordinate system: GPS = + n GPS = + n T he noise was modelled based on GPS measurement noi se, collected in field tests. The IMU data (IMU) was generated by first adding noise and a linearly time varying bias to vehicle accelerations (a) and angular velocities (ω). The result was then multilied by a linearly time varying scaling factor. The noise (n), biases (b) and scaling factors (f) were modelled based on lab tests of the sensors. The equations for the IMU measurements in the roll (r), itch () and yaw (y) directions were: IMU r = f r ( ar + nr + br ) IMU = f ( a + n + b ) IMU = f ω + n + b ) Y Y ( y y (7) (8) (9) (10) (11) The outut of the sensor simulator was sets of otimisation and testing data. In addition to the sensor data described here, the true values of states were also et at this stage, so that they could be comared with the state estimates of the Kalman filter. Within a set of otimisation aths, the noise created for each ath was different. However, the same otimisation aths were recycled from exeriment to exeriment to ee the results comarable. Only one set of test aths was created and the noise in this did not change.

5 5.4 The Kalman Filter The extended Kalman filter was designed for a vehicle travelling in two dimensions. The sensor measurements taen as inuts were GPS ositions, in terms of orthings and astings relative to the starting oint, and IMU measurements. The IMU sensor included an accelerometer (which was aligned with the roll direction of the vehicle), another accelerometer (which was aligned with the itch direction of the vehicle) and a gyroscoe, measuring the yaw rate. The ey states of the vehicle in the Kalman filter were the ositions, velocities and accelerations in the orth and ast coordinate system, the heading of the vehicle and the angular velocity. The other states were the biases and scaling factors in the IMU measurements Coordinate System Definition The definition of the coordinate systems for the Kalman filter and the data going into the Kalman filter is given in Figure 4. The dynamics of the vehicle were defined in both the global orth ast coordinate system and the local body coordinate system. The global coordinate system is the domain of the GPS data and the robot controller. The IMU sensors give data for the local body coordinate system. Only three degrees of freedom were considered so for the local body coordinate system the motion is described in the roll (r) direction, the itch () direction and by the angle theta (θ), which is defined as clocwise rotation of the local body axes relative to the global axes. Figure 4: The Coordinate System Definitions Kalman Filter Derivation To formulate a Kalman filter, the most imortant equations are the state dynamics model and the measurement model. All of the equations of the Kalman filter were derived from these two models. The general state dynamics equation is: where θ ( x u) x & = f,, x is the system states, u is the system inuts, f ( x, u ) is a nonlinear function of the states and inuts. r (12) 5 For the robot driver s Kalman filter, inuts are not considered. As a result, this equation becomes: where v v v ω v v ω v 0 a 0 a ω x& = f( x) = 0 θ x =, (13) ω, 0 0 br b 0 b y 0 0 f r 0 f 0 f y v is the vehicle velocity. The measurement model of the Kalman filter relates the sensor measurements to the states of the system. It is given in equation 14. h (x) = f ( ) r a cos( θ ) + a sin( θ ) + b (14) r f ( a cos( θ ) a sin( θ ) + b ) ( ) f y ω + by The matrix exonential of f(x) in the state dynamics equation (equation 13) is the nonlinear function of the state estimates from the last time ste in the first equation of the Kalman filter (equation 1). The Jacobian of equation 14 gives the linearised measurement model, which is used in the third equation of the Kalman filter (equation 4). For more information on how to derive the Kalman filter equations from the state dynamics equation and measurement model see [Welch and Bisho, 2004] Two Dimensional Assumtion The Kalman filter and hence the sensor simulation and ath lanners have all been created for two dimensions. If imlemented in three dimensions, this same Kalman filter would roduce s due to three-dimensional effects. For examle, it is commonly recognised that biases are the major cause of inaccuracy in IMU measurements [Suarieh et al., 1999]. In a real life imlementation, gravity would contribute to these biases and other forms of noise. However, gravity is not considered in the two dimensional model. evertheless, the two dimensional Kalman filter has wored well in field tests, as will be demonstrated in section 9. In any case, the two dimensional Kalman filter simly serves as a latform to test the various hyotheses, concerning rocess noise covariance matrix otimisation, resented in this research.

6 5.5 The Genetic Algorithm A simle genetic algorithm was made to otimise the Kalman filter arameters. The genetic algorithm begins with the creation of an initial grou of solution guesses [Charbonneau, 2002]. In this case, each guess was a randomly generated vector of arameter values. The next ste may be called evaluation and involves evaluating the erformance of each vector of arameters. In this research, the erformance of each set of arameter values was evaluated by running the Kalman filter for those arameter values and comaring the true values of states with the state estimates calculated by the Kalman filter. Selection follows evaluation and consists of raning the arameter vectors. The success of a vector of arameter values during evaluation determines its raning and hence how much that solution will contribute during reroduction. Reroduction is the creation of new sets of arameters from the members of the current generation. A rocess called crossover is used, where the arameters from existing individuals are recombined to form new oulation members. Parameters in one individual are swaed with equivalent arameters in the other individual. Crossover oints mar the boundaries for swaing and are randomly selected. The result of crossover is two new solutions, which are new combinations of the original solution elements. Mutation also occurs during reroduction. Mutation occurs on a arameter by arameter basis and simly involves the relacement of a vector element with a randomly generated value. Mutation should occur with a low robability if the set of arameter values are to converge on the otimum. After crossover and mutation has taen lace, reroduction is comlete and a new generation of vectors, containing arameter values, will have been formed. This new generation will be subjected to evaluation and selection. This new generation will also reroduce to form another generation and so on the genetic algorithm roceeds as shown in Figure 5. In this research, the genetic algorithms were run for 4000 generations. Covariance matrix guesses are randomly generated ach covariance matrix is evaluated in terms of the between the state estimates and the true values ach individual s contribution during reroduction is determined based on their erformance evaluation The current generation reroduces using crossover and mutation to form the next generation Stoing criterion met? Yes Best erforming covariance matrix is oututted Figure 5: The genetic algorithm summarised. o 6 6 xeriment 1 The first exeriment was conducted to investigate how the erformance of a set of arameter values should be evaluated. It was hyothesised that the Kalman filter arameters should be otimised in terms of more than just one state of the system. This is multiobjective otimisation. It was thought that otimising the rocess noise covariance matrix in terms of just one objective would lead to the degradation of the erformance of the Kalman filter state estimates in terms of the other objectives. So if the covariance was otimised in terms of minimum osition, the Kalman filter might roduce large angular s. It is hyothesised that multiobjective otimisation would be a good solution to this roblem. 6.1 Method To test this hyothesis, firstly, simulated data was created. Searate data was created for otimisation and testing. The otimisation data was created by utting the ath lanner data for the two shorter aths through the sensor simulation rogram. One ath ( short 2 ) was ut through the sensor simulation rogram two times; the other ( short 1 ) was ut through it three times. The result was sensor measurements for five vehicles aths. These sensor measurements were et constant throughout all of the subsequent testing. The testing data was created by utting the long ath from the ath lanner through the robot driver simulation and then through the sensor simulation rogram 100 times. The result of this was realistic sensor measurements for 100 distinct tris. The erformance of a single set of arameter values was evaluated by feeding the sensor measurements for the five otimisation vehicle aths into the Kalman filter one at a time. During each time ste, states generated by the Kalman filter were comared with the true values, which were the ath lanner data values, in this articular exeriment. The s between the true values and the Kalman filter estimates were set to ositive and averaged out over all of the five otimisation aths. What combination of these mean absolute s should be used in the evaluation ste of the genetic algorithm was the question behind this exeriment. The mean absolute s chosen for evaluation included the osition in metres, angular in degrees and a combination of the osition and angular : osition = ( ositionestimate true osition) (15) angular = ( filter angle estimate true angle) (16) mixed = 2 2 ( angular ) + ( osition) (17) In three searate runs, one for each tye of, the genetic algorithm was used to otimise the Kalman filter rocess noise covariance matrix in terms of the osition, angular and the combination of these two. After the otimisation was comlete, the resulting rocess noise covariance matrices were taen as inuts in different runs of another version of the Kalman filter. In each of these runs, the 100 lots of testing data were fed into the Kalman filter one after another. For each set of testing data the osition, angular and the combination of these two was calculated. The averages of these 100 s for each tye of and each rocess noise covariance matrix, from the otimisation ste, were calculated.

7 6.2 Results Table 1 gives the erformance of the Kalman filter for the different rocess noise covariance matrices, otimised in terms of osition, angular and a combination of osition and angular. The results in terms of the averaged osition, angular and combined are given. Table 1: The results from exeriment 1. Process noise covariance otimised for: Test results in terms of: Position Angular Mixed Position 1.9 m Angular 1.9 m Mixed 1.9 m It is clear from the data in Table 1 that otimising the Kalman filter arameters in terms of just the osition gives a oor erformance in terms of angular, which aears to be relatively more sensitive to the values of the arameters. The best overall erformance has actually come by otimising in terms of the angular. However, this is very close to the erformance gained by considering both the angular and osition. In fact, the rocess noise covariance matrices for these two cases were found to be very similar. The conclusion drawn here is that it would be best to include angular in the otimisation of the rocess noise covariance matrix. In the exeriments that follow, the combination of both the osition and angular will be used to assess rocess noise covariance matrix erformance during otimisation. 7 xeriment 2 The second exeriment that was conducted investigated whether it was sufficient to otimise just in terms of the data generated by the ath lanner rogram and the sensor simulation rogram or if the robot driver rogram had to be used as well. If the rocess noise covariance matrix can be adequately otimised in terms of the data given by the ath lanner and sensor simulator, the imlication is that the robot driver simulator does not need to be used during arameter otimisation. This would suggest that the vehicle manoeuvres were the ey source of rocess noise. 7.1 Method Three sets of data were created for this exeriment. The test data was the same as for the last exeriment. The otimisation data was created using the five coies of the long ath from the ath lanner. In one case, this was fed through the robot driver simulator and then the sensor simulator. To mae the other set of data, only the sensor simulator was used. The same noise was used to create both sets of otimisation data. The erformance of a single set of rocess noise covariance matrix values was evaluated by using the Kalman filter to comute state estimates for each of the five aths of a single set of otimisation data. The combination of the angular and osition was calculated at each time ste of the Kalman filter. After the Kalman filter had finished running for the current rocess noise covariance matrix, the combination of the angular and osition was averaged over the entire eriod of time covered by the five aths. The genetic algorithm otimised the rocess noise covariance matrix in terms of the averaged combined angular and osition for both sets of otimisation data. The resulting air of rocess noise covariance matrices was tested in the same way as in the first exeriment. 7.2 Results The resulting averaged s in Table 2 show that there is virtually no difference between otimising the rocess noise covariance matrix with data that is fed through the robot simulator and erforming the same otimisation with data that is not created using the robot simulator. This is a useful result because it means that one ste can be eliminated in the rocess of creating data for the otimisation stage. Table 2: Results for exeriment 2. Case 1 is where the rocess noise covariance matrix had been otimised with data that had been run through the robot driver simulator. Case 2 used data that had not involved the robot driver simulator. Test results in terms of: Position Angular Mixed Case m Case m xeriment 3 The third exeriment in this research investigated if different manoeuvres should have different rocess noise covariances. This would require otimising two rocess noise covariances: one each for the bends and straights. 8.1 Method The roduction of otimisation and test data was the same in this exeriment as it was in the first exeriment. The ey changes were made in the otimisation and testing stages. Two different rocess noise covariance matrices were otimised by the same run of the genetic algorithm in this exeriment; whereas, in revious exeriments, only a single rocess noise covariance matrix was otimised by a single run of the genetic algorithm. The erformance of a air of rocess noise covariance matrices was evaluated by firstly defining one as being for straight movements and the other as being for turning movements. A turning movement included the eriod of deceleration into and acceleration out of the corner. The rocess noise covariance matrix used at any given time ste was deendant on whether the vehicle was moving straight or turning. For examle, when the vehicle was moving straight, the rocess noise covariance used in the Kalman filter was switched to the corresondingly defined matrix. Aart from this adjustment, the Kalman filter was run as before: for five otimisation aths with the average combination of angular and osition calculated at the end. The genetic algorithm then otimised the air of rocess noise covariance matrices in terms of the average combined angular and osition. 7

8 Both of the rocess noise covariance matrices were trialled using the test data. As with exeriments one and two, the testing was carried out by running a coy of the Kalman filter 100 times with 100 different sets of data, which had been created by the ath lanner, robot simulation and sensor simulation. The mean absolute angular, osition and combined was calculated for each run of the Kalman filter. The difference in this exeriment was that the matrix designated for straights was used for segments where the vehicle was travelling on straight line aths and the rocess noise covariance matrix that had been assigned to turns was used in the Kalman filter for sections where the vehicle was turning. 8.2 Results The first row of Table 3 (Case 1a) gives the erformance of the Kalman filter during testing when two rocess noise covariances were used: one for bends and one for straights. The second row (Case 2a) gives the erformance of the Kalman filter when one covariance matrix was used. Table 3: Test results for the case where different rocess noise covariance matrices are otimised for different manoeuvres. ote that the bottom row of data is from exeriment 1. Test results in terms of: Position Angular Mixed Case 1a 1.8 m Case 2a 1.9 m Firstly, consider the circumstance where the lanned aths used in otimisation are different from the lanned aths used to create the test aths. This situation is more liely to be the case. A comarison with the case where only one rocess noise covariance matrix is used reveals that using different rocess noise covariance matrices for different lanned manoeuvres gives an imroved erformance. Table 4: Test results for the case where different rocess noise covariance matrices are otimised for different manoeuvres. ote that the bottom row of data is from exeriment 2. Test results in terms of: Position Angular Mixed Case 1b 1.8 m Case 2b 1.8 m ow, consider Table 4. Case 1b is the same as case 1a in Table 3; it is where two rocess noise covariance matrices have been otimised for the two different manoeuvres of the robot driver. Case 2b is where the otimisation aths were the same as the test aths; this is Case 2 in Table 2. Comaring the results from these different cases shows that otimising for one covariance matrix by using the ath that will actually be travelled gives better Kalman filter erformance than otimising for two rocess noise covariance matrices by using aths different from the ath travelled. However, the difference in erformance is very small and it may not be ossible to otimise for every ath travelled. For long aths this would slow down the otimisation stage considerably. In this case, generally otimising rocess noise covariances for different manoeuvres should be the referred otion. 8 9 Field Tests A field test was erformed to demonstrate the erformance of a GPS/IMU Kalman filter, otimised offline using the findings of the three exeriments conducted reviously. The offline otimisation was erformed in terms of the mixed objective function. Searate rocess noise covariances were used for bends and for straights. 9.1 Method To create sensor data for the otimisation of the rocess noise covariances, measurements were collected from a GPS unit and an IMU, while driving over a mared course. The course was a rectangle with dimensions of aroximately 50 metres by 80 metres (Figure 6). To seeds of aroximately 30 m/h were reached along the straights. Corners were taen at low seeds with a very tight turning radius. The Field Test Path orthing (m) asting (m) Figure 6: GPS data oints showing the ath followed by the vehicle during the field test. The orthings and astings were taen relative to the origin, which was the starting oint. This GPS and IMU data was run through the genetic otimisation algorithm as before in exeriment 3, where searate rocess noise covariance matrices were found for bends and straights. However, a change was made to imrove the measurement model of the Kalman filter with an extra measurement being added. This measurement was the angle created by the two most recent GPS readings. During otimisation, aroximations for the osition and angular were made. Before otimisation began the rectangular course was surveyed using aroximately one thousand GPS measurements. Straight line equations for each edge of the rectangle were found in terms of orthings and astings. During otimisation, the osition was taen as the minimum distance between the Kalman filter osition estimate and the straight line equations for the course. The straight lines defining the edges of the rectangular ath were also used to find aroximate values for the angle at every time ste. The true angles at the corners were calculated as linear interolations of the angles of both corresonding edges of the rectangular ath. The difference between the true angle estimates and the Kalman filter angle estimates were used in the calculation of the mixed as in equation 17.

9 The definition of a bend and a straight was set based on a threshold value of the angular velocity seen in the gyroscoe measurements. Angular velocities below the threshold value indicated that the vehicle was travelling on a straight section. Gyroscoe measurements above the threshold indicated a bend. The oututs of the otimisation were two rocess noise covariance matrices; one for bends and the other for straights. These covariances were used in running the Kalman filter in a field test. The field test was conducted over the same course, which was used for the collection of otimisation data. However, faster driving was conducted. To measure the erformance of the Kalman filter, the average mixed, as defined in equation 17, was calculated for the Kalman filter state estimates. The osition s and angular s were calculated as described for otimisation. In addition, the raw GPS data was logged during the field test. This data was ost-rocessed to roduce angle estimates at each time ste using the two most recent GPS osition measurements. The mixed for the GPS data was also calculated. 9.2 Results The mixed roduced by the state estimates of the Kalman filter and the mixed calculated for the GPS measurements are comared in Table 5. Clearly, the Kalman filter roduces significant imrovements by combining the GPS data with the IMU measurements. This suggests that the method of tuning the Kalman filter by offline otimisation, using multiobjective otimisation and searate rocess noise covariances for the different manoeuvres, is a useful one. Table 5: Field test results comaring the mixed s from the state estimates of the Kalman filter and the state estimates calculated from GPS data alone. Mixed Kalman filter 4.9 GPS 6.6 The results in Table 5 also suggest that the two dimensional assumtion, discussed in section 5.4.3, was fair. The ath used in the field tests had a gradient of aroximately one ercent. Based on the success of the Kalman filter in the field tests, the two dimensional assumtion is justfied for relatively flat terrains. 10 Conclusions This research has tested three hyotheses. The conclusion from the first exeriment was that when otimising a Kalman filter offline using a metaheuristic, the otimisation should be multiobjective or in terms of the most sensitive state. Otherwise, there may be a serious degradation in erformance in terms of states not included in the otimisation. The second exeriment showed that the simulated ath measurements used in otimisation did not need to include rocess noise for the robot driver considered. The final exeriment showed that, where the otimisation aths are et the same, using different rocess noise covariance matrices for different manoeuvres gives better accuracy than a single otimised covariance matrix. The outcome of this research is a method that may be used when otimising the rocess noise covariance matrix of a Kalman filter in similar or analogous alications to the robot driver described. The first ey factor that would lead to the adotion of this method would be the absence of a model relating the inuts and the system states. This would lead to increased rocess noise. If this noise was somewhat redictable, as it is in this robot driver alication, then the otimisation method described in this aer may be relevant. The imortant oints of this method are, firstly, to consider carefully the states to be used in the objective function during otimisation. Multiobjective otimisation or otimisation in terms of the more sensitive variables may be aroriate. Secondly, simle aths may be sufficient for otimising the rocess noise covariance matrix. The rocess noise may not have to be included in simulated aths during the otimisation rocess, if the rocess noise created by not including inuts in the Kalman filter is dominant. Another ey result is that different rocess noise covariances may be used in distinctly different sections of the journey. This should give imroved erformance, when the rocess noise changes dramatically and redictably in different segments of a lanned ath. The roosed method of using multi-criteria otimisation and different rocess noise covariances for different manoeuvres, during offline otimisation, has been successfully demonstrated with a field test. 11 Future Wor This aer has described research that has tested three hyotheses, concerning the offline otimisation of the rocess noise covariance matrix of a GPS/IMU Kalman filter for a robot driver alication. The exeriments in this research have been conducted on simle two dimensional models. To extend the findings of this research to more general cases, it would be aroriate to test the same concets on more comlicated three dimensional models. Future wor for this research will involve develoing and exerimenting with Kalman filters in three dimensions. In addition, the erformance of an adative Kalman filter may be comared to the offline otimisation rocedure described in this aer. Acnowledgements Warm thans to Greg MacDonald and Alistair Poolman, undergraduate students at The University of Aucland, for roviding the robot driver simulator for this research. The authors wish to than Grand Challenge Z for roviding the oortunity to wor on the robot driver, which has formed the basis of this research. Thans also to the Mechanical ngineering Deartment of The University of Aucland for roviding summer studentshi funding for this research. References [Bolognani et al., 2003] Silverio Bolognani, Luca Tubiana and Mauro Zigliotto. xtended Kalman filter tuning in sensorless PMSM drives. I Transactions on Industry Alications, 39(6): , ovember/ December

10 [Chan et al., 2001] Zee S.H. Chan, H.W. gan, Y.F. Fung and A.B. Rad. An advanced evolutionary algorithm for arameter estimation of the discrete Kalman filter. Comuter Physics Communications, 142(1-3): , December [Charbonneau, 2002] Paul Charbonneau. An Introduction to Genetic Algorithms for umerical Otimization. CAR Technical ote, March [Farrel and Barth, 1998] Jay Farrell and Matthew Barth. The Global Positioning System and Inertial avigation. McGraw-Hill, ew Yor, [Grewal et al., 2001] Mohinder S. Grewal, Lawrence Weill and Angus P. Andrews. Global Positioning Systems, Inertial avigation, and Integration. John Wiley, ew Yor, [Gueye et al., 2005] Ousmane Gueye, Karina Lebel, Jean De Lafontaine and Charles-Antoine Brunet. Fine-tuning of a Kalman filter with a genetic algorithm with gradient based otimization methods. Advances in the Astronautical Sciences, 119(III): , [Hu et al., 2003] Congwei Hu, Wu Chen, Yongqi Chen and Dajie Liu. Adative Kalman filtering for vehicle navigation. Journal of Global Positioning Systems, 2(1): 42-47, ovember [Loebis et al., 2004] D. Loebis, R. Sutton and J. Chudley. A fuzzy Kalman filter otimized using a multi-objective genetic algorithm for enhanced autonomous underwater vehicle navigation. Proceedings of the Institution of Mechanical ngineers Part M: Journal of ngineering for the Maritime nvironment, 218(1):53-69, February [Powell, 2002] Thomas D. Powell. Automated tuning of an extended Kalman filter using the downhill simlex algorithm. Journal of Guidance, Control and Dynamics, 25(5): , Setember/ October [Shin et al., 1992] D.H. Shin, S. Singh and J.J. Lee. xlicit ath tracing by autonomous vehicles. Robotica, 10: , [Suarieh et al., 1999] Salah Suarieh, duardo M. ebot and Hugh F. Durrant-Whyte. A high integrity IMU/GPS navigation loo for autonomous land vehicle alications. I Transactions on Robotics and Automation, 15(3): , June [Welch and Bisho, 2004] Greg Welch and Gary Bisho. An Introduction to the Kalman Filter. University of orth Carolina at Chael Hill, orth Carolina, [Xiong et al., 2005] Zhilan Xiong, Yanling Hao, Jinchen Wei and Lijuan Li. Fuzzy adative Kalman filter for marine IS/GPS navigation. Proceedings of ICMA 2005, I International Conference on Mechanics and Automation, ,