1.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Anders Engman July 6, 2006
2.
AbstractThis report looks into four different ﬁlters for sensor data fusion of three sensors,GPS, gyro and odometry. The complementary characteristics of the sensors meanthat they together give an increased navigational performance. The report mainlyfocuses on the ﬁlters ability to estimate the ﬁlter states that is being used to supportthe navigation ﬁlters during GPS blockouts. The ﬁlters ability to estimate a path isnot the main focus.The sensor data used in the report comes from a wheel based experiment platformequipped with a GPS, a gyro and odometry. The encoders for the odometry is con-structed and evaluated in the report. The GPS is used as a measurement of position,heading and speed, while the gyro and odometry is either used as a measurement,or to drive the system equations, in different combinations. The four ﬁlters exam-ined estimates the vehicle position, heading, wheel radii, rear wheel axis width,gyro bias, gyro scale factor, wheel radii error and heading angular speed in differ-ent combinations. SammanfattningDenna rapport undersöker fyra stycken ﬁlter för sammanvägning av sensordatafrån tre olika sensorer, GPS, gyro och odometri. Sensorernas komplementära egen-skaper innebär att de tillsammans ger ökad navigeringsprestanda. Rapporten un-dersöker i första hand de olika ﬁltrens förmåga att estimera de tillståndsvariablersom används för att stötta navigeringen vid bortfall av GPSdata. Filtrets förmågaatt estimera en färdad bana kommer först i andra hand.Sensordata som används i rapporten kommer från en hjulbaserad experimentplat-tform försedd med en GPS, ett gyro och odometri. Encodrarna för odometrin kon-strueras och utvärderas i rapporten. GPSen används som mätning för position,riktning och hastighet medan gyro och odometri antingen som mätning, eller attdriva systemekvationerna, i olika kombinationer. De fyra ﬁltren som undersöks es-timerar, förutom fordonets position och riktning, även hjulradier, hjulbasavstånd,gyrobias, gyroskalfaktor, hjulradiefel och riktningens vinkelhastighet i olika kom-binationer.
3.
LIST OF FIGURES2.1 Different types of gyros. . . . . . . . . . . . . . . . . . . . . . . 42.2 Encoder pulse waves. . . . . . . . . . . . . . . . . . . . . . . . . 62.3 Parameter deﬁnitions. . . . . . . . . . . . . . . . . . . . . . . . . 73.1 The vehicle shown without the top cover. . . . . . . . . . . . . . 123.2 Block diagram of the autonomous vehicle system. . . . . . . . . . 133.3 Reﬂective tachometer schematics. . . . . . . . . . . . . . . . . . 143.4 Opacity encoders. . . . . . . . . . . . . . . . . . . . . . . . . . . 153.5 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 173.6 Encoder wheel assembly. . . . . . . . . . . . . . . . . . . . . . . 184.1 Parameter deﬁnitions. . . . . . . . . . . . . . . . . . . . . . . . . 225.1 The GPS data derived from the test run in Kista. . . . . . . . . . . 395.2 The estimated vehicle path generated by odometer data. . . . . . . 395.3 The gyro path derived from the test run. . . . . . . . . . . . . . . 405.4 Wheel radius estimate plot with the nominal value removed. . . . 415.5 This plot shows the innovation of the position of the Kalman ﬁlter together with their 1-sigma standard deviation. . . . . . . . . . . . 415.6 This plot shows the innovation of heading of the the Kalman ﬁlter together with their 1-sigma standard deviation. . . . . . . . . . . . 425.7 This plot shows the innovation of speed of the the Kalman ﬁlter together with their 1-sigma standard deviation. . . . . . . . . . . . 425.8 This is the estimated path when the GPS has been active during the whole path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.9 Figure of the ﬁlter estimated path when GPS data is unavailable during the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . 435.10 This plot shows the integrated distance error that the ﬁlter is doing. 445.11 Heading error plot. . . . . . . . . . . . . . . . . . . . . . . . . . 445.12 Wheel radius estimate plot with its one and three sigma standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 455.13 Base width estimate plot with one and three sigma standard deviation. 455.14 Position innovation plot with one and three sigma standard deviation. 46 —page i—
4.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation 5.15 Heading innovation plot with one and three sigma standard deviation. 46 5.16 Velocity innovation plot with one and three sigma standard deviation. 47 5.17 The plot shows ﬁltered position estimation where the GPS has been available during the whole estimation time. . . . . . . . . . . . . 47 5.18 Estimated path where the GPS was turned off during the last lap. . 48 5.19 Position difference between ﬁlter where GPS is used for the whole path and where it is turned off during the last lap. . . . . . . . . . 48 5.20 Heading estimation difference between the path using GPS for the whole path and the path where the GPS was turned off during the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49 5.21 Estimation of the gyro bias. . . . . . . . . . . . . . . . . . . . . . 50 5.22 Scale factor estimation. . . . . . . . . . . . . . . . . . . . . . . . 50 5.23 The Kalman ﬁlter position innovation. . . . . . . . . . . . . . . . 51 5.24 The Kalman ﬁlter heading innovation. . . . . . . . . . . . . . . . 51 5.25 Filter estimated path with GPS data during the whole test time. . . 52 5.26 Filter estimated path with GPS data removed during the last lap. . 52 5.27 Position drift plot. . . . . . . . . . . . . . . . . . . . . . . . . . . 53 5.28 Heading difference between estimated path where GPS has been used during the whole path and where it has been disregarded dur- ing the last lap. . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 5.29 Wheel radius estimate plot with its one and three sigma standard deviation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 5.30 The bias estimate. . . . . . . . . . . . . . . . . . . . . . . . . . . 54 5.31 Position innovation plot with one and three sigma standard deviation. 55 5.32 Heading innovation plot with one and three sigma standard deviation. 55 5.33 Speed innovation plot with one and three sigma standard deviation. 56 5.34 Path estimated using GPS during the whole test time. . . . . . . . 56 5.35 Path estimated with the GPS disregarded during the last lap of the path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 5.36 The position difference between the ﬁlter run with GPS during the whole path and the ﬁlter disregarding the GPS during the last lap. 57 5.37 The heading difference between the ﬁlter run with GPS during the whole path and the ﬁlter disregarding the GPS during the last lap. 58 —page ii—
5.
LIST OF TABLES3.1 Karnaugh map for software comparison minimization. . . . . . . 195.1 The advantages and disadvantages of the different ﬁlters. . . . . . 58 —page iii—
9.
CHAPTER 1 IntroductionThis report is written as an master thesis for the Royal Institute of Technology(KTH) in Stockholm, Sweden on commission of the Swedish Defence ResearchAgency. I wish to pay a special thanks to Peter Strömbäck and Bengt Boberg forthe endless help with the project, my tutor at FOI, Petter Ögren for helping me startup at FOI, my tutor at KTH, Magnus Lindé for the extra help when I needed it, myexaminer Karl Henrik Johansson for the help with the thesis.1.1 BackgroundBy direct controlling a vehicle over a radio link, either by looking out from thevehicle using a camera or looking directly at the vehicle, the person holding theremote control device is navigating through the terrain. The person controlling thevehicle visually identiﬁes the position of the platform and decides what to do next.Autonomous vehicles need to make the same kinds of decisions.One way to do this is to give the autonomous vehicle a camera connected to acomputer. The computer then identiﬁes objects on the camera image and navigatesusing these. A computer is not nearly as advanced as a human brain and positionusing images is very computationally demanding in all but the most basic cases.An easier way for the autonomous vehicle to solve this problem of navigation is togive it a map of places where it can and can not move. For the autonomous vehicleto be able to use this map its position on the map, more or less exact, needs to beknown. There are a number of different devices developed that can keep track ofposition and heading of vehicles. In this thesis focus lies on using gyro, odom-etry and GPS. An INS keeps track of all the forces applied onto the vehicle andby integrating those forces the position, speed and orientation can be calculatedfrom initial values. Using the GPS the instantaneous position of the vehicle canbe decided, and this position has a limited absolute error. Using an odometer thenumber of revolutions the wheels turn is counted and thus the distance the vehiclehas traveled can be calculated. The calculations needed to keep track of the vehicleare a lot lower using these sensors than for the camera imaging navigation. —page 1—
10.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation1.2 Motivation for PositioningThe need for the autonomous vehicle to know its position very exactly has morethan one reason. One is to keep itself on the passable parts of the map or just tofollow a number of preset waypoints. An autonomous vehicle most often has atleast one more mission then just to navigate. One might be to identify the positionof objects. For the autonomous vehicle to ﬁnd a position of an object with any kindof precision it ﬁrst needs to know its own position and heading with high precision,since it can only identify the position of the object in comparison to itself. Addinga low certainty of the position of the vehicle itself to the low certainty of the sensoridentifying the objectt’s position relative to the vehicle will give a poor total cer-tainty of the object position.1.3 Problem DeﬁnitionThe objective of this thesis is to derive and implement sensor fusion of a GPS re-ceiver, an automotive grade gyro and an odometer to obtain a cheap, recyclablenavigation system. These three sensors have three unique sets of properties andusing sensor fusion the complementary properties can be used to obtain reliablenavigation.The sensors are placed on a wheeled platform and an onboard computer performsdata logging of the sensor data. The sensor fusion is to be implemented and testedin matlab using the logged data. The vehicle is outﬁtted with a PC-104 computerfor all onboard computations, a camera, an gyro, a GPS receiver and W-LAN. Theodometer solution is to be developed during the project.The ﬁrst task is to derive a solution for the odometer, to produce the mechanicsfor the odometer and to make a mathematical model for it. Then a GPS-odometry,GPS-gyro and GPS-odometry-gyro fusion ﬁlter should be developed and evaluated. —page 2—
11.
CHAPTER 2 Sensors and Sensor Fusion2.1 SensorsThere are a number of sensors that can be used for navigation. All of them havetheir advantages and drawbacks. For this thesis GPS, odometry and a gyro werechosen to be evaluated. Gyro because it cannot be jammed and has a high reso-lution and high sample rate, GPS because it gives a global position with limitedabsolute error, and odometry because it cannot be jammed, does not drift while thevehicle is standing still, and has high resolution and sample rate.2.1.1 GyroThe gyroscope is a ﬂywheel turning at high velocity hanging on three near frictionless axis so that the ﬂywheel does not change rotational axis even if the casingholding the gyroscope does. The angular difference between the ﬂywheel and thecasing can then be measured. A good gyroscope is an advanced and expensivepiece of equipment. Recently, solid state gyroscopes using micro technology havebeen developed. These are smaller and a lot cheaper. They are not really gy-roscopes since they do not have a ﬂywheel, but since they solve the exact sameproblem, they may be called gyroscopes.The gyro, also called rate gyro, however is not perfect. The rate gyro in this thesis isa "MicroElectroMechanical Systems gyro" (MEMS gyro). It has two main errors.One is a scale factor error, meaning that the output from the rate gyro has a scalefactor multiplied with it, thus making the output either smaller or larger dependingon the scale factor being larger or smaller than one. The other error is a bias,meaning that the signal from the gyro at standstill is not zero but rather showseither positive or negative rotation. The measured gyro signal can thus be writtenas r(t) = sr(t) + b + v(t), ˜ (2.1) —page 3—
12.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation (a): Gyroscope (b): MEMS Gyro Figure 2.1: Different types of gyros.where r(t) is the signal from the rate gyro, s is the scale factor, r(t) is the real ˜rotational velocity and b is the bias and v(t) is white measurement noise.One can estimate the angular change from initial direction to current direction ofa rate gyro by integrating the signal. To do this the error parameters have to betaken care of in one way or another, or the estimated direction will drift away fastfrom the real one. A rate gyro such as the one used in this thesis can only measureangular change along one axis.2.1.2 GPSThe Global Positioning System is used to get a global position with limited error.It is based on a number of satellites which send coded signals to a receiver. Thereceiver internally generates signals identical to the ones from the satellites and bycorrelating these with the incoming signals a measurement of signal propagationtime is obtained. This time corresponds to different distances of different satellitesand the position of the receiver can be calculated using the known position of thesatellites.The time (range) measurements are afﬂicted with several errors where receiverclock error, satellite clock error, ionosphere and troposphere delays, satellite posi-tion error and noise are some of the most prominent ones. These errors all affectthe errors on calculated position [1]. In this thesis the position obtained from thereceiver is used with a simpliﬁed error model where the position error is regardedas white noise, according to v1 (t) ˜ x = x+ , (2.2) v2 (t) —page 4—
13.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation ˜where x is the position indicated from the GPS, x is the actual position and, v1 (t)and v2 (t) are white noise.The GPS is based on very weak signals (10−16 W at the receiver) [1] p.283. Thatis 19 dB lower than the thermal noise ﬂoor and therefore the GPS is sensitive tojamming and interference, intentional or unintentional. The receiver also requiresa clear line of sight to the satellites. This is especially difﬁcult to obtain in ur-ban environments and terrain covered with heavy foliage. Another solution to thisproblem is collaborative navigation [2]. The position update rate of a modern GPSreceiver is typically a few Hz which can provide problems for a control system ifthe vehicle dynamics are too fast.2.1.3 OdometryOdometry works by having encoders on one or more of the wheels of the platform.Encoders measure the rotational velocity of an axis, here the wheel axis. An en-coder consists of two tachometers. These are placed with a small shift betweenthem so that when one of them has its transition from either low to high or high tolow, the other is just in between two transitions. In this way the square waves fromthe tachometers have a phase shift, the ﬁrst tachometer waveform being ahead ifthe wheel is turning forwards and the second tachometer waveform being ahead ifthe wheel is turning backwards as shown in ﬁgure 2.2. The wheel speeds measuredon a vehicle with two encoders, together with the diameter of the wheel give thedistance the vehicle has moved according to equation 2.3. ωr Rr + ωl Rl v= , (2.3) 2where Rr and Rl are the right and left wheel radius, respectively, ωr and ωl are theright and left wheel speeds, respectively, and v is the rear wheel axis center velocityas deﬁned in ﬁgure 2.3.Since the outer wheels travel further than the inner ones, in a turn the direction ofthe vehicle, using two encoders, can also be calculated, thus enabling the calcu-lation of current position relative to the initial position. The equation for vehicleheading using odometry is ωr Rr − ωl Rl r= , (2.4) Bwhere r is the rear wheel axis center angular velocity and B is the rear wheel axiswidth as deﬁned in ﬁgure 2.3.A wheel compresses somewhat during travel over uneven terrain, thus changingthe wheel radius, and the wheels might easily slip when moving on gravel at highvelocities or if the platform hits some object in its environment. The encoders alsohave a discretization error. These errors are accumulative over distance and theestimated position drifts signiﬁcantly after a while of travel. —page 5—
14.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 2.2: Encoder pulse waves.Detecting wheel slips from the odometry is very hard since there is no way tellingwhen or where they happen. Because of this, the error of the odometry sensor ishard to describe. In this thesis the odometry error is simpliﬁed and seen as a whitenoise on the wheel encoders. The measurement in equation 2.5 is used for vehiclevelocity and 2.6 for vehicle angular rate. ˜ ˜ ωr Rr + ωl Rl va (t)Rr + vb (t)Rl v= + , (2.5) 2 2 ˜ ˜ ωr Rr − ωl Rl vc (t)Rr + vd (t)Rl r= + , (2.6) B Bwhere va (t), vb (t), vc (t) and vd (t) is white noise with a large enough variance todescribe both the possible slip error and the quantization error. The positive signbetween the two terms in the noise part of the heading equation 2.6 is due to thatwhite noise is symmetric around zero.2.2 Sensor FusionThe rate gyro and odometer drift over time and distance, the GPS does not. Ifthe autonomous vehiclet’s only mission is to navigate, using only the GPS may insome applications be sufﬁcient. The path planning system within the vehicle justhas to navigate with a margin large enough to the edges of the navigational areathat the vehicle is guaranteed to be within that area.There are, however, a number of reasons why a higher accuracy is needed. Thepositioning of an autonomous vehicle is most often used for more than just gettingfrom point A to point B. For many real life tasks it has a mission other than just —page 6—
15.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 2.3: Parameter deﬁnitions.navigation. One of them might be to ﬁnd the position of an object by looking at itwith an onboard camera or a laser scanner from a distance. The camera and laserscanner themselves have uncertainties. If one adds the uncertainty of the GPS tothe uncertainty of the platform sensors, the position of the object will not be exactenough since the position and heading information from the GPS is very rough.The vehicle heading is especially important when working with angles to triangu-late an objects position. The position of where the measurement was taken has tohave a small uncertainty. Since the heading information from the GPS has high un-certainty, the angle to the studied position has a high uncertainty as well. The GPSsystem is also dependent on radio connections to the satellites. These connectionscan be jammed either intentional or unintentional. The GPS alone will not be goodenough; an integration of sensors is needed.2.2.1 GPS-Gyro IntegrationGPS has a limited absolute error but the high frequency error is larger, the gyrohas a small high frequency error but a much larger low frequency error causing theposition to drift over time. Combining the two would lead to an more accurate androbust navigation system. In this thesis this is accomplished by using an ExtendedKalman Filter (EKF).2.2.2 GPS-Odometry IntegrationSimilar to the GPS-gyro integration in 2.2.1 ﬁltering together GPS and odometryhas many times proven to give good results [3], [4]. Here, as with the GPS-gyrointegration, the GPS is not totally reliable, and odometry does drift during GPSblackouts. —page 7—
16.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation2.2.3 GPS-Gyro-Odometry IntegrationThe rate gyro and odometry produce a drift when used alone. However, the rategyro and odometry drift is not exactly the same. During a GPS blackout the rategyro and odometry can be weighed together and thus decrease the system drift. Thegyro can then compensate the odometry when the wheels slip during hard turns orwhen running over bumps in the terrain.2.3 Kalman Filtering2.3.1 IntroductionThe Kalman ﬁlter (KF) is named after its inventor Rudolf E. Kalman. The ﬁlterwas initially developed by Swerling in 1958, and then enhanced by Kalman in1960 and Kalman and Bucy in 1961. A wide variety of Kalman ﬁlters have beendeveloped from Kalman’s original formulation. In its original form the Kalmanﬁlter works for discreet linear systems. Later the so called extended Kalman ﬁlter(EKF) was developed which also handles nonlinear systems. In this thesis an EKFis used since the both the system and some measurement functions are nonlinear.A Kalman ﬁlter is a special kind of observer that estimates the states of a linearsystem. The issue is that the states cannot be measured directly but rather have tobe estimated though a series of measurements affected with white noise. There area number of ﬁlters doing this but the way the variation of the white noise of themeasurements is used is special for Kalman ﬁlters.2.3.2 The System ModelThe state of the system to be estimated by the EKF is described by a vector of realnumbers x, each number describing one of the system states. At each discrete timestep k, an nonlinear function f(xk ) is applied to the state to generate the new state.This operation is affected with the system noise, called Qk . Another linear operatoror nonlinear function h(xk ) is then used to transfer the system measurements to thesystem vector. These measurements are affected with measurement noise calledRk . Some of the system states x may be directly observable from the measure-ments, others might be more complicated and might need certain conditions to beobservable at all.2.3.3 ObserverThe observer estimates the states of the system using the model for time update.During the Kalman prediction state the time transfer function is used to propagatethe system state xk to the next time step xk+1 . This can be done using a mathemati-cal model derived as the dynamics of the system or from measurements made thatdescribes the state update. This stage can also be made multiple times between two —page 8—
17.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationmeasurements.Measurements made by other sensors is then made to correct errors the predictionstage has. These measurements should have other error dynamics than the mea-surements used to drive the prediction stage. Using same measurements in predic-tion state and the measurement state does not deliver any more information to thesystem since the measurement noise will be correlated with the system noise. Theweighting done between the predicted state and the measured state is calculatedusing the noise of the measurement and the covariance matrix, P. The covariancematrix is calculated for every prediction stage as shown in equation 2.7 and de-scribes the system states estimate uncertainties. This weighting between the twonoises is done in an optimal way yielding a minimum variance estimate using thetwo noise matrices P and Q.2.3.3.1 Kalman Gain K and Covariance PP and K is calculated optimaly using equation 2.7 and equation 2.8. How theseequations has been derived is described in [5] p. 117-121. P− = Fk Pk FT + Qk k+1 k (2.7) Kk = P− HT (Hk P− HT + Rk )−1 k k k k (2.8)Where Hk is the linearization of the measurement matrix h(x).2.3.3.2 Kalman Filter DynamicsFor the optimality of K and P to be calculated the noise parameters of R and Q alsohas to be chosen correctly. Q describes as mentioned above the noise of the systemstate prediction. If the system states propagation is driven by measurements thenoise of Q is taken from the error dynamics of the sensors. If the system statespropagation is driven by a mathematical model, the noise of error dynamics of themodel describes Q. Measurement noise R is taken from the sensors error dynam-ics. How the exact correlation between sensor noise and R and Q is can be studiedin [5]. It is usually hard to ﬁnd an exact value of the noises in R and Q and it iscommon practice to use the calculated R and Q as starting values and then manu-ally tune the matrices until the EKF exhibits the desired performance. If there arenon modeled errors dynamics in the measurements or system model, the noise in Rand Q can be increased so that these errors also include the non modeled errors. Ifthese errors does not have white noise characteristics the ﬁlter might not converge,however this is still often used.2.3.3.3 Filter InitializationFor the ﬁlter to converge it has to be correctly initialized. The initial covariancehas to be chosen large enough to include the difference between the actual state —page 9—
18.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationand the estimated state vector.2.3.4 Difference Between SensorsIf the time propagation function is chosen to be driven by sensors the ﬁlter will havetwo types of Sensors. Sensors used to drive the system prediction and sensors usedas measurements to correct the prediction. Typically the sensors used to drive thesystem prediction are sensors that have a high output rate and only low frequencynoise. Sensors used for measurements typically have a lower output frequency, ahigher frequency error and preferably no zero frequency error at all.2.3.5 Linear and Nonlinear Functions and MeasurementsIn a linear system all time propagation functions, measurement functions and co-variances can be written in matrix form. This is practical since the Kalman ﬁltercalculations uses matrix inversions. If the system is not linear, it is not possibleto put the equations on matrix form. The prediction stage can still be done in nonlinear form, xk+1 = f(xk ). But the calculations of the covariance propagation, equa-tion 2.7, demands a system transpose, thus the time propagation function has to belinearized and transposed for this purpose. Same system is applicable for the nonlinear measurements. The difference between measured value and estimated valuecan be calculated using the non linear equations, but calculations of the kalmanﬁlter gain, equation 2.8, require matrix form and thus the measurement functionhas to be linearized.2.3.5.1 Linear and Nonlinear ConvergenceAs described in [5], chapter 4, convergence and optimality can be proven for linearﬁlters. In the case of the EKF neither convergence nor optimality can be proven,described in [5], chapter 5. This causes some problems since it is not possible todetermine ﬁlter convergence in any other way then to test the ﬁlter with data. How-ever this method of estimating the states of non linear systems is widely used andworks with good performance in most cases.2.3.6 Extended Kalman Filter MathematicsFor the EKF to be able to estimate the internal state of the process given a se-quence of noisy measurements the process has to be modeled with accordance tothe Kalman ﬁlter framework, specifying the functions f(ˆ k ) and h(ˆ k ) and the ma- x xtrices Qk and Rk , each time step. Where f(ˆ k ) is the system time propagation xtransfer function, h(ˆ k ) is the measurement function, Qk the system noise vari- xation matrix and Rk the measurement noise variation matrix. Given the discreetnonlinear system 2.9 and 2.10 the method for using a Kalman ﬁlter is as follows. ˆ xk+1 = f(ˆ k ) + wk x (2.9) zk = h(ˆ k ) + vk x (2.10) —page 10—
19.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation ˆ ˆWhere f(ˆ k ) is the transfer function propagating the system vector xk to xk+1 , x ˆh(ˆ k ) transfers xk to the observable data zk and vk and wk is white noise originat- xing from the sensor or system model error dynamics. The estimated states in the ˆstate vector xk covariance Pk is updated for all time steps and after all measure-ments. For all time steps the linearized system time propagation transfer function ˜Fk and the linearized measurement function Hk is calculated. yk is an externalmeasurement. The Kalman ﬁltering is done as follows.Choose initial value for the state vector x− . The initial value of the covariance ˆmatrix must be chosen so that the initial values of the states are within distance toreal value described by the covariance. Calculate Qk = E[wk wT ], Rk = E[vk vT ] k kand P− = E[(xk − x− )(xk − x− )T ] where k = 0 and xk the actual state. k ˆk ˆk1. Calculate the Kalman gain. Kk = P− HT (Hk P− HT + Rk )−1 k k k k (2.11)2. Calculate new estimate of state vector with measured values zk . xk = x− + Kk (˜ k − h(ˆ k )− ) ˆ ˆk y x (2.12)3. Calculate covariance matrix. Pk = (I − Kk Hk )P− (I − Kk Hk )T + Kk Rk KT k k (2.13)4. Propagate state vector. x− = f(ˆ k ) ˆ k+1 x (2.14)5. Project covariance matrix. P− = Fk Pk FT + Qk k+1 k (2.15)6. Calculate linearized Fk and Hk , k = k + 1, restart at point 1. —page 11—
20.
CHAPTER 3 Hardware Implementation3.1 The Autonomous VehicleThis thesist’ autonomous vehicle is based upon the chassis of a standard commer-cial radio controlled 1/10 scale vehicle. The vehicle is 486 mm long and 414 mmwide. Figure 3.1: The vehicle shown without the top cover.The top cover is removed and replaced by an aluminum mounting plate. Themounting plate is equipped with a PC/104 computer running Linux. The car isalso equipped with a servo control device, a camera, a GPS receiver and antenna,an inertial navigation system and a wireless LAN, all connected to the PC/104.Figure 3.2 is a schematic diagram over the components of the autonomous vehicle —page 12—
21.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.2: Block diagram of the autonomous vehicle system.studied in this master thesis.3.2 Odometer ConstructionThere are a number of different ways to measure how far a vehicle has traveled.One is to put an encoder on one or more of the wheels of the vehicle. The encodersends a signal to a microcontroller or computer that keeps count of how far thevehicle has traveled since the counting started. If encoders have been placed onone left and one right wheel, the direction of the vehicle can also be calculated asseen in subsection 2.1.3. One error of the measurements is due to slipping of thewheels when the vehicle is accelerated fast on gravel or driving in uneven terrain.Another emanates from the fact that when the car moves in uneven terrain the tireswill compress and thus change their radius; this causes the calculated stretch ofthe wheels to differ from the real stretch, which affects both distance traveled andvehicle heading. All driven wheels always have a microslip towards the ground.In the case of the autonomous vehicle in this thesis, the wheels are constructed ina way that microslip can occur between the rim and the tire. This means that thetires might slowly turn around on the rims when the vehicle is driving. Since thetwo latter error sources are small compared to the slip caused by uneven terrain,they may be disregard. The microslip appears evenly distributed over the distancetraveled and the distance error microslip will be handled by a ﬁlter estimating thewheel radii. —page 13—
22.
Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationOne way to handle the slip and microslip problems is to add one or more wheelsto the car, have them springloaded towards the ground at all time and then put theencoder on these wheels. Since they are not driving there will be no micro slippageand the wheels are a lot less likely to spin. The problem with this solution is thatthe car mechanics will be more advanced and developing the system will take a lotof time. In the case of the autonomous vehicle in this master thesis, it is easier touse the standard wheels.3.2.1 Different EncodersThere are a few different types of encoders. In this section their advantages anddisadvantages will be discussed. A tachometer is a primitive part of an encoder asexplained in subsection 2.1.3.3.2.1.1 Optical TachometerThere are two different types of optical tachometers, reﬂex tachometers and trans-parency tachometers. Both have a light source and a light sensitive sensor. Thereﬂex tachometer has the light source and the light sensor close to each other onthe same side of the tachometer disc. The tachometer disc is painted with manysmall sections of black and white across the disc circumference, the black areasreﬂecting less light than the white areas. The light sensitive sensor registers howmuch light is reﬂected from the light source. Counting these high and low reﬂec-tions when the disc spins by the light source and sensor gives a measurement of thenumer of turns the disc has made. Figure 3.3: Reﬂective tachometer schematics.The transparency tachometer has the light source on one side of the disc and thelight sensitive sensor on the other. The transparency tachometer disc has holes init, thus letting light through when a hole is between the sensor and light source,and keeping the light from reaching the sensor otherwise. This way a much higherdifference between light and dark areas may be achieved. The drawback is that the —page 14—
23.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationlight source and the light sensor cannot be on the same side of the tachometer disc.This is sometimes very difﬁcult to obtain, not even possible or just too expensive. (a) (b) Figure 3.4: Opacity encoders.Figures 3.4 (a) and (b) show optical transparency tachometers. The red plasticcomponent in the ﬁgure to the left is the light source and the white transparentcomponent is the light sensitive sensor. Between them the tachometer disc can beseen with its slots where the light is let trough.The largest drawback for both of these tachometers is dirt. When dirt or dust getsstuck on the black and white areas of the reﬂective wheel or in the holes of thetransparency encoder the readings will be corrupt. Since the thesist’ autonomousvehicle will be moving outdoors, dirt will be a big problem for these sensors.3.2.1.2 Magnetic TachometerIf the light and dark areas of the optical tachometers are exchanged for magneticnorth and south poles, a magnetic ﬁeld sensor can be used to sense the change inmagnetic ﬁeld instead of light intensity. One sensor that can sense magnetic ﬁeldis the Hall Effect sensor. The Hall Effect sensors can be placed at a distance fromthe magnets and the magnetic ﬁeld will not be disturbed by dirt or dust. Hall Effectsensors might be a little slower than the optical ones.3.2.1.3 From Tachometer to EncoderA tachometer only counts the transitions between light and dark, or between mag-netic north and south poles, but will not tell which direction the wheel is turning.By adding another sensor placed half a count away from the ﬁrst one the directionof the rotation can also be derived as explained in subsection 2.1.3.The signal from the ﬁrst sensor is called channel A and the second channel B. Forsome applications one more channel is needed. This channel is called I (for index —page 15—
24.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationchannel). The index channel has only one count per lap and may be placed any-where in the tachometer disc. This is useful both when the system is ﬁrst turnedon, and a certain position of the wheel needs to be found, and when for some rea-son the encoder misses a count, which can be corrected by the index channel. Thiscorrection is done by keeping track of how many ticks there are on a lap, and howmany has been counted since the last time the index was counted. If for instancenine positive counts has been made since the last index, and there should be ten ina lap, the electronics know that one tick was missed and this can be corrected byadding one positive count.3.3 The Encoder SolutionThere are three parts that has to be done to make an encoder for the autonomousvehicle in this thesis. First, the mechanics has to be made considering the environ-ment in which it will be working. Second, the electronics must be able to transformthe signals from the sensor so that the micro controller can count the transitions.The micro controller must also be fast enough not to miss any of the transitions andat the same time handle the serial communication to the vehicle onboard computer.Third, the software has to be efﬁcient enough that the limited computational powerof the micro controller can handle it.3.3.1 Encoder MechanicsBecause of outdoor application of the thesis autonomous vehicle the optical en-coders are dismissed. The dust problem is considered too big and magnetic en-coders are well suited for dusty environments.Neodymium magnets are chosen for their magnetic strength. Higher magneticstrength means higher signal-noise ratio and that the Hall Effect sensors are al-lowed to be placed further away from the magnets. The magnets are placed be-tween the rim and the tire where they are protected from moist and dirt. Neodymiumis very corrosive and protection from moist and dirt is needed.The neodymium magnets are ﬁxed to the rims using a system of thin lamellas. Toget high enough precision together with low weight the lamellas are made usinga water cut machine cutting the lamellas from 1.2mm thick plastic sheets. Watercutting machines have about ﬁve hundreds of a millimeter of precision, which isconsidered to be high enough. At both ends of the lamellas one more lamella isplaced to keep the magnets from moving coaxially.Keeping the weight low is needed since the sensors are placed at the wheel ofthe vehicle, and unspringed weight should be kept as low as possibly to reduce theenergy of that mass when moving over uneven terrain.The lamellas are placed as far as possible towards the inner side of the wheels —page 16—
25.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.5: Encoder wheel assembly.so that the sensors are easy to place on the other side of the rims. The magnets areplaced with every other magnet having its magnetic north pole facing outwards,and every other inwards. The lamellas are ﬁxed using screws to hold the lamellastogether with the magnets in their slots. The lamellas are glued to the ribs using atwo component glue.3.3.2 Encoder ElectronicsThe purpose of the encoder electronics is to count the transitions of magnetic ﬁeldsensed by the Hall Effect sensors and send the number of transitions done during asample period to the vehicle onboard PC/104. The sensors are placed half a magnetwidth from each other, so the pulse wave from the magnets were 90 degrees shiftedas explained in subsection 2.1.3.Four different Hall Effect sensors were tested. Two digital (Hall Effect switches)and two analog. The digital sensors had a schmitt trigger function which wouldmake the square waves a little different when the wheel is turning clockwise andwhen it is turning counterclockwise, they were therefore dismissed. The two ana-log sensors were Allegrot’s A3515 and A3518. A3515 has a maximum sensitivityof 10 G and A3518 20 G. Both sensors were tested with the magnetic encoder discand it proved that neodymium magnets were strong enough to saturate both sensorsbut A3518 had, because of its longer sensitivity range, a little smoother transitionbetween high pulse and low pulse. Therefore A3518 was chosen. The sensors werefastened onto the wheel suspension system close to the rim.The analogue signal generated by the Hall Effect sensors connected to connec-tor SV1, is fed into a comparator (IC4A-IC7A and IC4B-IC7B in ﬁgure 3.6), sothat any positive magnetic ﬁeld sensed generates a logic 1 (5 volt), and any neg- —page 17—
26.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation Figure 3.6: Encoder wheel assembly.ative magnetic ﬁeld sensed generates a logic 0 (0 volt). These logic level squarewaves are suitable to be counted by a micro controller (IC3).There are a number of different micro controllers on the market. Microchip Tech-nology delivers a micro controller called PIC, Motorola has their 68HC-series,Ubicom has a series of SX chips and Atmel has their AVR series. To handle thecounting a Atmel AVR micro controller is chosen. The Atmel series of 8-bit AVRcontrollers are a modern solution containing all the features needed for this task.The AVR-series are also easy to program using C-code and the programming en-vironment CodeVisionAVR delivered by Hp InfoTech.Each of the four square wave signals, two from each rear wheel, is fed into theAVR micro controller using four interrupt driven inputs. Each time one of thesignals change state a certain code is run in the micro controller to decide whichchannel changed and if the count to make is positive or negative.When moving at slow velocities the sensors bounce a few times before settlingat the new state. The expression "bounce" means that the signal changes from onestate to the other a few times back and forth before settling at the new state. Ifthe amount of bounces is too high the micro controller will not be able to countall the transitions and thereby fail to count correctly. To reduce bouncing a 2.2µFcapacitor (C3-C6 in ﬁgure 3.6) is added between the analog signal and ground tosmooth the Hall Effect sensor output. When this is done almost all of the bouncingdisappears, and for higher velocity there is no bouncing at all. —page 18—
27.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation S1 = 0 S2 = 0 S1 = 0 S2 = 1 S1 = 1 S2 = 1 S1 = 1 S2 = 0 P1 = 0 P2 = 0 X + X - P1 = 0 P2 = 1 - X + X P1 = 1 P2 = 1 X - X + P1 = 1 P2 = 0 + X - X Table 3.1: Karnaugh map for software comparison minimization.3.3.3 Micro Controller SoftwareThe purpose of the software is to keep track of the number of passing magneticﬁeld transitions and at certain time intervals deliver them to the onboard PC/104.Since there are four inputs that can change transition, there are 16 different statesthat the input could have. Binary 0000, 0001, 0010, and so on. To know if oneshould be added or subtracted, the prior state has to be remembered. From eachwheel there are only two prior states from which a transition to the present statecan be made. If the program is split up into two parts, one for each wheel sensorpair, then each sensor has four states that each can be transitioned to from two priorstates. That means that eight comparisons have to be made for each sensor pair toderive if it is an positive or negative transition, 16 totals for both wheels. Even thouthe electronic is very fast the bouncing that is still present even when the capaci-tors are used might cause problems. To minimize the risk of this problem time tocalculate a transition has to be as short as possible. Not all combinations of the twobits of new sensor data and two bits of prior sample data is possible, they can onlymake transitions from 1 to 2 to 3 to 4 to 1 again, or the opposite direction. A jumpfrom 2 to 4 can never be done. Therefore a minimizing of the comparisons is done.The transitions are fed into a Karnaugh map, table 3.1. + marks if the transitionshould lead to an increase of the number of transitions, - marks the decrease and Xmarks an illegal change. Note that no change is seen as an illegal change since ifno change has been made, the comparison code in the micro controller is not run.S1 and S2 are the present state of the sensors and P1 and P2 are the prior state.According to the Karnaugh map minimizing rules [6] the states were grouped to-gether four and four in a square. Each containing only + and X or - and X. Giventhe Karnaugh map rules the only comparison that has to be made is between S1 andP2 . S1 = P2 produces an increase of transitions and S1 = P2 a decrease. The microcontroller now only has to do two times two comparisons instead of the earlier twotimes eight comparisons, effectively now cutting the computation time in four.The calculations of the transitions is set to be done during the ﬁx time period of 20.000.000about 19 Hz ( 1024∗256∗4 Hz). The numbers of calculated transitions are then sentvia the RS-232 serial link to the PC-104 and the two transition counters are resetto zero and then start calculating again. —page 19—
28.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation3.3.4 Sensor Error AnalysisThere are a few different errors of the odometer sensor. Quantization, nonlinearityand missed tick are the most prominent.Quantization error is the largest right before and right after a transition. If thequantization step is from one to two, and the transition between one and two hap-pens at 1.5, then the absolute quantization error is 0.5 just before and just after thetransition. For the wheel encoder this quantization is about 0.16 radians thus thelargest quantization error is 0.08 radians.For movement forward in higher velocities this does not matter much since 0.08 ra-dians to little during one sample will be 0.08 radians to many the next. Where thisquantization does cause problem is during slight turns. If one wheel should makefor i.e. 10 ticks the other should make for i.e. 10.1, thus for nine samples measur-ing 10 ticks and for one sample 11 ticks. The quantization error does not cause thedistance the vehicle traveled or the heading of the vehicle to differ from the realvalue over time, but it does affect the estimated position. This since the positiondepends on that heading and velocity are correlated, which they are not. Quan-tization error for heading at one time is corrected later, but if the vehicle movesduring this error time the calculation for position change will be in slightly wrongdirection.Linearity error is very small since the mounting for the magnets inside the rimswere water cut with a precision of about of about 2 micrometers. The error is thus3 × 10−5 radians and can be neglected compared to quantization error. This errordoes not increase by time.If the wheel is spinning too fast, either the micro controller or the comparatormight not be able handle the frequency of transitions from the encoder. The wheelradius is about 72 cm so the circumference of the wheel is about 0.45 meters. If thevehicle is traveling at 100 km/h, which is a lot higher than any reasonable veloc-ity for this autonomous vehicle, the wheel will turn less than 65 times per second.With 40 pulses per lap, 2.600 ticks per wheel and second has to be handled. That’s5.200 transitions for the micro controller to count using both encoders. Every tickuses two if statements each being about 20 instructions for the micro controllerwhich is 208.000 instructions totally. The micro controller computes 20 millioninstructions per second so the margin is about 100 times.Each of the four comparators handle 1.300 pulses. According to the compara-tor datasheet the comparators are made to work for frequencies of up to 1.1 MHz.That’s a 800 times margin.The quantization error does cause the position to drift over time and this is takeninto account when setting the odometry noise. The nonlinearity is a lot smallerthan the quantization error and is disregarded. The electronics with 100 times mar-gin and the comparators with 800 times margin will have no trouble handling thecounting of ticks from the encoders for any realistic velocity of the autonomous —page 20—
29.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationvehicle studied in this thesis. —page 21—
30.
CHAPTER 4 Kalman Filter ImplementationIn this chapter the different navigation ﬁlters and their performance will be de-scribed brieﬂy. In chapter 5 the ﬁlters are evaluated in more detail. In this thesisa ﬂat earth representation is used providing navigation states expressed in east andnorth coordinates. The third dimension, up, is disregarded. Figure 4.1: Parameter deﬁnitions.The heading angle of the vehicle is deﬁned from north to west, since some of theﬁlters have been copied from [4], in which this deﬁnition is used.The system equations for all the systems are on the form d x = f(x,t) + g(x,t)w(t), (4.1) dt y = h(x,t) + v(t), (4.2) —page 22—
31.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationwhere x is the system states, f(x,t) is the system function, g(x,t) is the noise distri-bution for the system function, w(t) is white noise, y is the system output, h(x,t) isthe measurement function and v(t) is the measurement noise. In the sequel, theseequations will be given without the time argument t.Two ﬁlters use only odometry and GPS. The ﬁrst estimates the wheel radii, po-sition and heading and is described in section 4.2. The second estimates the wheelradii, position, heading and rear wheel axis width and is described in section 4.3.Both are found to work well when the GPS data is present, but have some troubleto ﬁnd a good estimates for the wheel radii and the rear wheel axis width. WhenGPS data is not present, resulting in position and heading drift.In order to overcome this problem, a ﬁlter is tested where a heading gyro predictingthe vehicle heading is used. This ﬁlter is described in section 4.4. The odometerheading information is disregarded in this system. This ﬁlter is designed to estimateposition, heading, gyro bias and gyro scale factor and uses the odometry velocity.This system gives slightly better results than the GPS-odometry integration but itstill have quite a large drift. This drift, however, does not have the same featuresas the odometer drift. The odometry has a steady drift towards one direction whilethe gyro seems to have a more random drift back and forth.A ﬁlter using the external measurements of the vehicle heading angular rate fromboth the odometry and gyro is developed in subsection 4.5. The vehicle estimatedheading angular rate is there made to drive the system heading. For a measurementfunction for the odometry to be determined, the wheel radius error instead of thewheel radius it self has to be estimated. Thus an initial guess of the wheel radiushas to be made, from which the wheel radius errors are estimated. For this systemthe position, heading, heading angular rate, wheel radius error and gyro bias is es-timated. This ﬁlter has some problems with the system noise speciﬁcation for theheading angular rate since the odometer and gyro measurements do not execute atthe same time. However, for a certain value of noise the system runs smoothly alsoduring GPS outages.For all the ﬁlters, the vehicle ground velocity v is obtainable at all time steps usingequation 2.3, which is repeated as equation 4.3 for convenience. ωr Rr + ωl Rl v= (4.3) 2For each ﬁlter the linear observability is analyzed. For the Kalman ﬁlter to be ableto estimate the system states all the states have to be observable. To prove observ-ability, a nonlinear observability analysis has to be done. Since the mathematicsfor nonlinear observability is very complex this is not done in this thesis. Linearobservability does not prove nonlinear observability, but still gives a hint about theobservability of the system at least around the linearization point.Linear observability is made by looking at the Jacobian matrices H and F of themeasurement function h and the system functions f , respectively, and construct the —page 23—
32.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigationobservability matrix as described in equation 4.4. H HF . . , (4.4) . HF k−1where k is large enough to get a matrix of full rank, if possible. If there is no k largeenough to give the observability matrix full rank the system is not observable.4.1 Filter and System Variable Deﬁnitions.Many of the system and ﬁlter variables are the same for all of the ﬁlters. Some areonly used in one or two ﬁlters. All variables are deﬁned here. —page 24—
33.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation E The east position. ˜ E The east position measurements from the GPS. N The north position. ˜ N The north position measurements from the GPS. ψ The estimated vehicle heading angle from north to west ˜ ψ The GPS heading calculated using the GPS measurement vEGPS ˜ and vNGPS of the velocity components and equation 4.5. For veloc- ˜ ˜ ities lower than 0.5 m/s the ψ is disregarded because of its inaccu- racy. vGPS ˜ the GPS velocity calculated from the GPS measurement vEGPS and ˜ vNGPS of the velocity components and equation 4.6. ˜ Rr The estimated right wheel radius. δRrˆ The right wheel radius deviation from hand measured value. ˜ r + δRr R ˆ The real right wheel radius. Rl The estimated left wheel radius. δRlˆ The left wheel radius deviation from hand measured value. ˜ l + δRl R ˆ The real left wheel radius ˜ ωr The measurements of the right wheel angular rate using the en- coders. ˜ ωl The measurements of the left wheel angular rate using the en- coders. B The rear wheel axis width. ˜ ˆ ˜ ˆ v ˜ The measured velocity ωr Rr +ωl Rl derived from the odometer and 2 the wheel radii. r The real vehicle angular rate. r ˜ The gyro measured vehicle angular rate. b The gyro bias. s The gyro scale factor. vgyro (t) The gyro angular rate noise as described in section 4.6. vEGPS The GPS east position measurement noise as described in section 4.6. vNGPS The GPS north position measurement noise as described in section 4.6. vψGPS The GPS heading noise as described in section 4.6. vvGPS The GPS heading noise as described in section 4.6. vodometry (t) The odometry speed noise as described in section 4.6. wEodometry The odometry east position system noise whose characteristics is described in section 4.6. wNodometry The odometry north potition system noise whose characteristics is described in section 4.6. wψodometry The odometry heading system noise whose characteristics is de- scribed in section 4.6. wψgyro The gyro heading system noise whose characteristics are described in section 4.6. vEGPS ˜ ˜ ψ = arctan2( ) (4.5) vNGPS ˜ —page 25—
34.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation vGPS = ˜ v2 GPS + v2 GPS ˜E ˜N (4.6)Where arctan2 is the four quadrant version of arctan. The inverse trigonometricfunction arctan2(y, x) is deﬁned as r = arctan(y/x) using:if x = y = 0, then r = inde f initeif x > 0 and y = 0, then r = 0if x < 0 and y = 0, then r = π, elseif y < 0, then −π < r < 0if y > 0, then 0 < r < πThe function is used to ﬁnd the direction from one point to another in 2-dimensionEuclidean space.4.2 Odometry-GPS Estimating Wheel Radii.This model is taken, somewhat modiﬁed, from [4].In this system only the GPS and the odometry is used. The odometry is usedin the Kalman ﬁlter prediction step and the GPS serves as measurements. Position,heading and the wheel radii are estimated in the ﬁlter. The system is as in equation4.7. ˜ ˜ E − ωr Rr +ωl Rr sin(ψ) 2 wEodometry ˜ ˜ ωr Rr +ωl Rr cos(ψ) wNodometry N d = 2 ˜ ˜ ωr Rr −ωl Rr + wψ ψ (4.7) dt B odometry Rr 0 0 Rl 0 0The state vector is denoted x1 . The east and north position, angle between thenorth axis and the velocity vector and ground velocity is used as measurements.The measurement function h1 (x1 ) is described with its noise in the ﬁlter outputfunction in equation 4.8. ˜ E E vEGPS N ˜ N vNGPS y1 = ˜ ψ = h1 (x1 ) + v(t) = + vψGPS . (4.8) ˜ ψ ˜ ˜ ωr Rr +ωl Rl vGPS ˜ 2 vvGPSThe prediction step is performed by solving the nonlinear equation 4.7 without thenoise term by using Euler integration (Euler integration is assumed sufﬁcient sincethe dynamics is relatively low and the update rate high). The covariance matrixis propagated using the Ricatti equation whence the system function has to be lin-earized according to equation 4.9. —page 26—
35.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation ˜ ˆ ˜ ˆ −ωr sin ψ −ωl sin ψ 0 ˜ ˆ 0 −v cos ψ 2 2 ˜ ωr cos ψˆ ˜ ωl cos ψˆ ˆ ∂f1 0 0 −v sin ψ ˜ 2 ˜ 2 ˜ F1 = = 0 0 0 ωr − ωl , (4.9) ∂ˆ 1 x B B 0 0 0 0 0 0 0 0 0 0Further, the measurement function h1 is already linear and its Jacobian matrix is 1 0 0 0 0 ∂h1 0 1 0 0 0 H1 = = x1 = ˜ 0 0 1 0 0 . (4.10) ∂ˆx ˜ ωr ˜ ωl 0 0 0 2 24.2.1 ObservabilityFor the ﬁlter in this section, a k of 2 is enough to get full rank under some circum-stances. The observability matrix is as equation 4.11. 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 ˜ ωr ˜ ωl H1 2 2 = ˜ ˆ ˜ ˆ (4.11) H1 F1 0 0 −v cos ψ − ωr sin ψ ˜ ˆ 2 − ωl sin ψ 2 ˜ ˆ ˜ ˆ 0 0 −v sin ψ ωr cos ψ ˜ ˆ 2 ωl cos ψ 2 ˜ ˜l 0 0 0 ωr B −ω B 0 0 0 0 0As seen in equation 4.11 a matrix of full rank can be constructed using rows onethrough four and row seven. The rest of the rows are not contributing to the matrix ˜ ˜rank. One observations done is that if ωr and/or ωl are zero the matrix does nothave full rank thus is not observable. ˜ ˜The observability analysis result is that if either ωr = 0 and/or ωl = 0, the sys- ˜tem is not observable for this k. Otherwise the system is observable. ωr = 0 andω˜ l = 0 corresponds to the vehicle having no velocity meaning that the two wheelradii cannot be estimated.4.3 Odometry-GPS Estimating Wheel Radii and Rear Wheel Axis WidthThis model is taken, somewhat modiﬁed, from [4]. —page 27—
36.
Study of Gyro and Differential Wheelspeeds for Land Vehicle NavigationIn this system the GPS and the odometry is used. The odometry is used in theKalman ﬁlter prediction step and the GPS serves as measurements. Position, head-ing, wheel radii and the rear wheel base width is estimated in the ﬁlter. The systemis as equation 4.12. ˜ ˜ E − ωr Rr +ωl Rl sin(ψ) 2 wEodometry N ωr Rr +ωl Rl cos(ψ) wNodometry ˜ ˜ 2 d ψ ˜ ˜ ωr Rr −ωl Rl wψodometry = ˆ B + (4.12) dt Rr 0 0 Rl 0 0 B 0 0The estimated state vector is denoted x2 . The east and north position, angle be-tween the north axis and the velocity vector and ground velocity is used as mea-surements. The measurement function h2 (x2 ) is described with its noise in theﬁlter output function in equation 4.13. ˜ E E vEGPS ˜ N N vNGPS y2 (ˆ ) = ˜ x = h2 (x2 ) + v(t) = + vψGPS (4.13) ˜ ψ ψ ˜ ˜ ωr Rr +ωl Rl vGPS ˜ 2 vvGPSThe prediction step is performed by solving the nonlinear equation 4.12 withoutthe noise term by using Euler integration (Euler integration is assumed sufﬁcientsince the dynamics is relatively low and the update rate high). The covariance ma-trix is propagated using the Ricatti equation whence the system function has to belinearized according to equation 4.14. ˜ −ωr sin ψˆ ˜ −ωl sin ψˆ 0 ˜ ˆ 0 −v cos ψ 2 2 0 0 ˆ 0 −v sin ψ ˜ ˜ ωr cos ψˆ ˜ ωl cos ψˆ 0 2 2 ∂f2 0 0 0 ˜ ωr ˜ − ωˆl ˜ ˆ −˜ ˆ − ωr RrB2ωl Rl F2 = = Bˆ B ˆ (4.14) ∂ˆ 2 x 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0Further, the measurement function h2 is linearized and its Jacobian matrix is 1 0 0 0 0 0 ∂h2 0 1 0 0 0 0 H2 = = (4.15) ∂ˆ 2 0 x 0 1 0 0 0 ˜ ωr ˜ ωl 0 0 0 2 2 0 —page 28—
37.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.3.1 ObservabilityFor the ﬁlter in this section, a k of 2 is enough to get full rank under some circum-stances. The observability matrix is as equation 4.16. 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 ˜ ωr ˜ ωl 0 H2 2 2 = ˜ ˆ ˜ ˆ (4.16) H2 F2 0 0 −v cos ψ − ωr sin ψ ˜ ˆ 2 − ωl sin ψ 2 0 ˜ ˆ ˜ ˆ 0 0 −v sin ψ ωr cos ψ ˜ ˆ ωl cos ψ 0 2 2 ˜ ˜ ˆ−˜ ˆ 0 0 0 ˜ ωr − ωˆl − ωr RrB2ωl Rl Bˆ B ˆ 0 0 0 0 0 0As seen in equation 4.16 all the columns are independent, thus the matrix has full ˜ ˜rank. One observations that is done, is that if ωr and/or ωl are zero the matrix does ˜ ˆ ˜ ˆnot have full rank thus is not observable. If ωr Rr = ωl Rl , then row seven, columnsix is zero. Since this is the only term in column six the system is not observablein this chase. ˜ ˜ ˜ ˆ ˜ ˆThe observability analysis result is that if either ωr = 0, ωl = 0 or ωr Rr = ωl Rl thesystem is not observable for this k. Otherwise the system is observable. ωr = 0˜ ˜and ωl = 0 corresponds to the vehicle having no velocity meaning that the two ˜ ˆ ˜ ˆwheel radius cannot be estimated. If ωr Rr = ωl Rl the vehicle is not turning, thusthe vehicle rear wheel axis width does not matter for the position or velocity of thevehicle, it is not observable.4.4 Odometry-Gyro-GPS estimating Gyro Bias and Scale FactorThis model is taken, somewhat modiﬁed, from [4].In this system the GPS, the gyro and the odometry is used. In the Kalman ﬁlterprediction stage the odometry is used for vehicle velocity and the gyro for vehicleheading and the GPS serves as measurements. Position, heading, gyro bias andgyro scale factor is estimated. The gyro measurement model chosen is as equation4.17. r(t) − b r(t) = ˜ + v1 (t) (4.17) s —page 29—
38.
Study of Gyro and Differential Wheelspeeds for Land Vehicle Navigation4.17 is a variable substitution from 4.18 r1 (t) = s1 r(t) + b1 + v2 (t) ˜ (4.18)made to avoid nonlinearities in the covariance matrix 4.21 for the correspondingelements. In the chase of this system the white noise v1 (t) becomes system noisewψgyro . This gives the system given in equation 4.19. ˜ ˜ E − ωr Rr +ωl Rl sin(ψ) 2 wEodometry N ˜ r Rr +ωl Rl ω ˜ cos(ψ) wNodometry d = 2 + , ψ −˜s − b r wψgyro (4.19) dt b 0 0 s 0 0ψ is here the vehicle heading angle from north to west obtained from the gyro witha negative sign. This because the gyro angular rate is deﬁned from north to westand the system heading from north to east. s and b is modeled as random constants.The estimated state vector is denoted x3 . The east and north position and the anglebetween the north axis is used as measurements. The measurement function h3 (x3 )is described with its noise in the ﬁlter output function in equation 4.20. ˜ E E vEGPS y3 = N = h3 (x3 ) + v(t) = N + vNGPS ˜ ˜ (4.20) ˜ ψGPS ψ vψGPSThe prediction step is performed by solving the nonlinear equation 4.19 withoutthe noise term by using Euler integration (Euler integration is assumed sufﬁcientsince the dynamics is relatively low and the update rate high). The covariance ma-trix is propagated using the Ricatti equation whence the system function has to belinearized according to equation 4.21. 0 ˜ 0 −v cos ψ 0 ˜ 0 0 ˜ 0 −v sin ψ 0 ˜ 0 ∂f3 F3 = ˜3 = =x 0 0 0 −1 −rgyro , (4.21) ∂ˆ 3 x 0 0 0 0 0 0 0 0 0 0Further, the measurement function h2 is already linear and its Jacobian matrix is 1 0 0 0 0 ∂h3 H3 = = 0 1 0 0 0 . (4.22) ∂ˆ 3 x 0 0 1 0 0 —page 30—
Clipping is a handy way to collect and organize the most important slides from a presentation. You can keep your great finds in clipboards organized around topics.
Be the first to comment