State estimation of RC cars for the purpose of drift control

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

State estimation of RC cars for the purpose of drift

control

Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet

av

Jonatan Liljestrand

LiTH-ISY-EX--11/4528--SE Linköping 2011

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

State estimation of RC cars for the purpose of drift

control

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Jonatan Liljestrand

LiTH-ISY-EX--11/4528--SE

Handledare: Christian Conte

IfA, ETH

John Lygeros

IfA, ETH

Kristoffer Lundahl

isy, Linköpings universitet Examinator: Fredrik Gustafsson

isy, Linköpings universitet Linköping, 22 November, 2011

(4)

(5)

Avdelning, Institution

Division, Department

Division of Automatic Control Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

Datum Date 2011-11-22 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://www.control.isy.liu.se http://www.ep.liu.se ISBN — ISRN LiTH-ISY-EX--11/4528--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Tillståndsskattning på RC-bilar för driftreglering

State estimation of RC cars for the purpose of drift control

Författare

Author

Jonatan Liljestrand

Sammanfattning

Abstract

High precision state estimation is crucial when executing drift control and high speed control close to the stability limit, on electric RC scale cars.

In this thesis the estimation is made possible through recursive Bayesian fil-tering; more precisely the Extended Kalman Filter. By modelling the dynamics of the car and using it together with position measurements and control input sig-nals, it is possible to do state estimation and prediction with high accuracy even on non-measured states.

Focus is on real-time, on-line, estimation of the so called slip angles of the front and rear tyres, because of their impact of the car’s behaviour.

With the extended information given to the system controller, higher levels of controllability could be reached. This can be used not only for higher speeds and drift control, but also a possibility to study future anti-skid safety measures for ground vehicles.

Nyckelord

Keywords Vehicle Dynamics, Sensor Fusion, Extended Kalman Filter, Control Theory,

(6)

(7)

Abstract

High precision state estimation is crucial when executing drift control and high speed control close to the stability limit, on electric RC scale cars.

In this thesis the estimation is made possible through recursive Bayesian filter-ing; more precisely the Extended Kalman Filter. By modelling the dynamics of the car and using it together with position measurements and control input sig-nals, it is possible to do state estimation and prediction with high accuracy even on non-measured states.

Focus is on real-time, on-line, estimation of the so called slip angles of the front and rear tyres, because of their impact of the car’s behaviour.

With the extended information given to the system controller, higher levels of controllability could be reached. This can be used not only for higher speeds and drift control, but also a possibility to study future anti-skid safety measures for ground vehicles.

Sammanfattning

För att utföra driftreglering och reglering nära stabilitetsgränsen på elektriska skalbilar krävs precis kännedom om bilens beteende i varje tidpunkt.

I detta examensarbete realiseras denna skattning genom rekursiv sannolikhets-baserad filtrering. Genom att dra nytta av positionsmätning, bilens styrsignaler, samt utförlig matematisk modellering kan man räkna ut hur valda delar av bilens beteende ter sig. Man kan också prediktera hur bilen kommer att bete sig inom en snar framtid. Detta är möjligt även för icke mätbara tillstånd.

Uppsatsen har fokuserat på att i realtid estimera däckens så kallade slip-vinklar, på grund av deras inflytande på bilens beteende.

Med den utökade informationen som projektet erbjuder reglersystemet kan nya nivåer av styrbarhet erhållas. Detta kan nyttjas inte bara till ökad hastig-het och driftreglering, utan också möjlighastig-heten att studera metoder för framtida sladdhävande säkerhetssystem för markburna fordon.

(8)

(9)

Acknowledgments

I would like to thank Automation Control Laboratories, and its staff, for letting me do my Master’s thesis at their office.

I would also like to thank the people who has been involved in the project like Alexander Domahidi, Christian Conte, Fredrik Gustafsson, John Lygeros, Kristof-fer Lundahl.

Last I want to thank the people who has supported me through tough times; especially Paula, Boris and my family.

(10)

(11)

Introduction

The ORCA Project is a study oriented project at Automation Control Laboratory, ETH, Zürich. It is conducted through many different smaller projects (semester projects and master theses).

The main purpose of the project is to study high-speed, real-time control. The overall goal is to create a system with Kyosho dNano RC cars, which should be able to compete against each other on a pre-defined track. The car used is shown in Figure 1.1

This thesis focuses on state estimation and system observation. Without know-ing what to control, it is hard to control it.

Figure 1.1. The car used in the project is a Kyosho dNano radio controlled car with a

Lamborghini body.

1.1 Background and Initial Project State

The system consists of a predefined track (made from plastic parts enclosed by walls), 1:43 scale cars, a control computer and two IR-cameras which measure position and orientation (more information can be found in [16]). Figure 1.2 shows

(14)

2 Introduction

a photograph of the system setup and how the parts relate to the current system loop.

A computer is, at the moment, able to (one at the time) navigate the cars according to a position trajectory and a speed profile with either a PID or an MPC controller, as described in [20]. A desired position trajectory and a corresponding speed profile is generated offline (the process is further described in [3]). The system works well in low speed single car applications.

Two desired functionalities and milestones in the process of developing the ORCA project further are:

• To be able to drive the cars ”as fast as possible” • To be able to execute/handle drift control1

!

"#$%&#'!"#()*%+&!

,&-./! 012"-(+&-!

Figure 1.2. A Picture over the hardware. The graphics shows the closed loop system.

The computer sends control signals to the car on the track, the IR-camera measures car position and sends to the computer.

Both the goals coincide in one important thing, control at high slip (or drift) situations. In the first case, high slip is an outcome of wanting to take e.g. corners in as high speeds as possible (thus wanting to handle as high lateral accelerations as possible, [13]). Whether or not the second goal contributes to shorter lap times on high friction surfaces is debated, but is an art and a competition in itself.

1_{"Drifting is the art of vehicle control during manoeuvres at sustained, large-amplitude}

(15)

1.2 Tasks 3

Controllability in critical sideslip situations rise when the control system is able to handle drift, as it makes it possible to coop with such situations with less risk of losing control.

A big challenge with the goals is that the closed loop system will be acting close to the border of stability. The steady state cornering conditions are in fact unstable while drifting, according to [18]. To be able to execute such controlling under the circumstances, a more advanced controller is needed, and an observable model with more controllable states, thus high precision state estimation (i.e. an observer) is necessary [6].

The previous state estimation used in the ORCA Project was very basic2_{. It}

did not take the dynamics of a car into consideration, but could estimate longitu-dinal speed with reasonable accuracy. Much more information about the current behaviour of the car could be extracted, if extensive mathematical models of the car are used in the estimation process.

If the ORCA project succeeds with these goals controllability for the system would rise. It would provide the tools necessary, not only for speed and style, but also for a study of future safety systems mentioned in e.g. [11]. All models and theories used in the thesis are taken from the study of real vehicles, and the project thus resembles the real life situation in many ways.

1.2 Tasks

The thesis project is divided into two sub tasks. The main overall goal is to extract as much information about the interesting parts of the behaviour of the car as possible, with the given calculation capacity.

Software Development: The main task is to develop, implement and analyse a

new mathematical filter, which makes more accurate state estimation possi-ble. The filter should be able to be run in real time on the current control computer. It should, with high precision, estimate the parts of the behaviour of the car, which is crucial for speed and slip control.

Hardware Development: Another way of getting to know more about the

cur-rent behaviour of the car, apart from more advanced filters, is to measure more states of interest. During a previous project, an extended sensing plat-form with an IMU (Inertial Measurement Unit) was developed. As a part of the thesis, a further developed prototype of this sensing platform was to be developed. It can be said, already at this point, that the hardware development was executed successfully. However, due to lack of time, no analysis was carried out. All information about the development is placed in Appendix C and further reading about the functionality can be found in Srdjan Curcic’s work about the embedded sensing platform[4].

(16)

4 Introduction

1.3 Approach, Method and Initial Conditions

The project was executed at Automation Control Laboratories, ETH Zentrum, Zürich, under the supervision of John Lygeros. Christian Conte was main supervi-sor on site, Kristoffer Lundahl was supervisupervi-sor in Linköping and Fredrik Gustavsson was thesis examiner.

Initially, a working version of the system was provided including controller, ac-tuating equipment and measurement equipment. A Simulink/Realtime Workshop model was already developed, which was able to navigate one car according to a given trajectory, with a corresponding speed profile.

The project is approached by evaluating and analysing different vehicle motion equations, to create a set of functioning representations of the dNano cars. These are later used to build a simulation software in Simulink, for analysing purposes and as a base for the filtering process.

When a sense for the equations and the vehicle dynamics is achieved, the filters are implemented and tested, first offline and later online.

Thanks to an already functioning system, data acquisition from real scenarios was possible with ease. This is a great help in the developing process and when analysing and tweaking the filters.

The new prototype for the extended sensing platform was developed in parallel throughout the project.

1.4 Nomenclature

The list of parameters in Table 1.1 defines general model parameters for the project. It also states which state, if any, in the state vector representing the variable and what unit it possesses.

Table 1.2 defines the parameters associated with the control input of the vehi-cle.

1.4.1 Coordinate System

Two different coordinate systems are used in the thesis:

Global: A global coordinate system, used by the camera system wherein the track has a static position.

Local: A local coordinate system, used by the motion models for the car, where in the car position is static.

See Figure 1.3 for an overview of the coordinate definitions. The coordinate system used is proposed by the Society of Automotive Engineers [19].

(17)

1.4 Nomenclature 5

Parameter State Unit Description

Px x1 [m] Global x-position

Vx x2 [m/s] Longitudinal speed

Py x3 [m] Global y-position

Vy x4 [m/s] Lateral speed

ψ x5 [rad] Vehicle angle

˙

ψ x6 [rad/s] Yaw rate, i.e. the rotating speed around the ˆz-axis

αf [rad] Slip angle front

αr [rad] Slip angle rear

m [kg] Vehicle mass

l1 [m] Distance from center of gravity to front axel

l2 [m] Distance from center of gravity to rear axel

w [m] Distance from center of gravity to track, i.e. half the wheelbase.

δ [rad] Front wheel steering angle

Iz [N ms] Moment of inertia around z-axis

Ffy [N ] Lateral forces acting on front wheel Ffx [N ] Longitudinal forces acting on front wheel

Fry [N ] Lateral forces acting on back wheel

Frx [N ] Longitudinal forces acting on back wheel Table 1.1. Motion model parameter descriptions.

Parameter Unit Description

D [-] The control input (0-1) applied as a voltage to the electric motor.

CM0 [N ] Engine force parameter. A constant converting the

input D to an applied force.

C0 [N ] A constant resistive parameter for the driveline.

C1 [N s/m] A resistive parameter for the driveline which depends

on the speed of the vehicle

δ [rad] Steer angle

(18)

6 ! Introduction

ˆ

x

ˆ

y

ˆ

X

ˆ

Y

ψ

Figure 1.3. An image showing the coordinate systems used in the thesis. The global

coordinate system ˆX and ˆY and the local coordinate system ˆx and ˆy. The centre of

(19)

Chapter 2

Theoretical framework

There are many ways of extracting information from a physical system, and there are probably even more challenges in the process of doing so.

The first approach of getting information about the behaviour of a system often is to measure the entity of interest. It is not uncommon however that the signal contains uncertainties, so called noise.

If the data is too noisy more accurate (and expensive) sensing equipment could be used. In an industry where money matter however it is more and more common to use many cheap sensors instead and fuse the data to achieve better accuracy.

One popular way to approach this problem is through recursive Bayesian filters; a probabilistic approach. By using mathematical models of the system of interest together with e.g. Weighted Least Squares for each incoming new measurement it is possible to find the most probable value of the interesting entity, including the uncertainty (variance) of it.

This way it should be possible to extract information with higher accuracy than with only raw sensor data. It is also possible to create so called ”virtual sensors”, to estimate the value of non-measured states.

2.1 The Kalman Filter

The Kalman Filter is model based filter which in an intuitive way makes it possible to update a model with many different measurements. The relevance and trust of the measurements in turn can, in an easy way, be weighed against each other and also against the information taken from the model behaviour.

The Kalman Filter is basically a recursive (or sequential) implemention of the Weighed Least Squares method, with the new incoming measurements updating the old result in each iteration. When the model describing the process is linear and its noise is additive and gaussian, the Kalman Filter is the optimal way of estimating the state vector from the input measurements[10].

(20)

8 Theoretical framework

2.2 Nonlinear State Equations

Even though the Kalman Filter is the optimal estimator for linear systems with gaussian noise, there is nothing guaranteeing the system to be linear. In the general case, very few systems actually are linear.

In this thesis, the system of interest is highly non linear (as described in Chap-ter 3).

2.2.1 The Extended Kalman Filter

The approach in the so called Extended Kalman Filter (EKF) is to linearise the system of interest with its Jacobian. In each iteration the system is linearised around the latest prediction both with respect to the states, but also with respect to system inputs.

The EKF is a filter simple to implement and which gives good results in many cases. In this thesis the EKF is implemented according to Section 2.3.

2.2.2 Other Possibilities

There are numerous other filters able to estimate the states of a state space model, from given measurement inputs. These are however not tested in this thesis.

Among the popular ones is ”The sequential Monte Carlo method”, or ”Particle Filter”, where a large amount instances of the states are taken from the probability function of the system is propagated sequentially. This gives a good estimation and prediction of the states of interest. The the mean and variance of the particles are used. For more information see [9].

The Particle Filter is very calculation heavy. A way of lessening the calculation burden is to use the so called Unscented Kalman Filter. In this filter a few so called ”sigma points” are chosen from the probability function of the system of interest and a Kalman Filter is then applied to these points. The mean and variance of the prediction of the sigma points gives an approximation of the states of interest. The problem with these filters in this thesis is that both of them relies on an accurate probability function of the system [9]. As will be seen in Chapter 3, this is not the case in this project.

2.3 The Riccati-based EKF Algorithm

Equations (2.1)-(2.7) show the algorithm for the extended Kalman Filter, which is used in this thesis. h(x) is short for h(x, u, 0) and defines the sensor equations. f (x) is short for f (x, u, 0) and defines the state equations. h0_x(ˆxk|k−1) is the Jacobian with respect to the state vector x around the previous state prediction ˆxk|k−1.

(21)

• Instead of the matrices representing the linear system, Jacobians around the previous prediction are used

• The sensor prediction in Equation (2.3) and ”one step prediction” in Equa-tion (2.6) is sets of non linear equaEqua-tions, instead of matrix operaEqua-tions The algorithm is taken from [9].

(22)

(23)

Chapter 3

Modelling

A fundamental part of successful use of a probabilistic approach to navigation and tracking is to model the motion of the object of interest [9].

Before digging into extensive mathematical modelling however, it is reasonable to question

• What is the purpose of the model? • What is the purpose of the filter?

As stated in the introduction, the goal of the state estimation is to make use of it in high speed/slip situations and drifting. As will be seen in this chapter, two in-teresting entities to analyse for this purpose are the slip angles of the wheels1_{. This}

makes longitudinal speed, lateral speed, yaw rate and steering angle interesting parameters, due to their simple geometrical relation to the slip angles.

One necessity was to implement the filter in Realtime Workshop, and make it run with the current system. Due to the restricted computational capacity when running realtime systems [5], this forces an aim for rather simple equations with not too many states (mainly due to the matrix inversion in the Kalman Filter).

3.1 The Single Track Model

Ground vehicle dynamics has been extensively investigated in literature. A way of modelling the horizontal motion of a four wheel road vehicle, used in numerous publications, is the so called ”single track model” or the ”bicycle model”.

3.1.1 The Principle of the Single Track model

The single track model originates from the assumption of small track width in comparison to the turning radius[15]. When having low centre of gravity and flat

1_{In this thesis the tyre’s slip angles are defined as the angle (in radians) in which the heading}

of the vehicle differs from the angle of which the wheel is pointing. See Figure 3.1 for more details.

(24)

12 Modelling

ground, and thus no load transfer, it becomes even more relevant; this due to similar and constant normal forces on the right and left wheels of the car.

The idea is to lump together the two front wheels and the two back wheels, to a two wheeled model. To make this work, the tyre forces of the wheels are doubled, to simulate force from two wheels. All models are simplifications of the truth, as described further in [7]. The trick is to make simplifications which makes a big difference in complexity, but a small difference in the description of the system dynamics.

Figure 3.1 shows an illustration2_{of the forces acting on the model.}

Figure 3.1. Graphics showing the forces acting on the bicycle model. The picture shows

the two wheeled vehicle ”from above”, where the turned wheel is the front wheel. Z-axis points downwards and the rotation ˙ψ rotates around the centre of gravity.

3.1.2 Motion Model of the Single Track Model

The system of differential equations (3.1) defines the basic model of the motion of a two wheeled single track ground vehicle, with zero height, without pitch or roll and without air drag or rolling resistance.

The wind force is neglected due to very small frontal area of the car and due to low speed (1-3 m/s). The rolling resistance is hard to estimate because of the car’s inability to freewheel. The rolling resistance is in a way included in the resistance of the driveline and applied to the car through Frx.

(25)

3.2 Tyre Forces 13            ˙

xglob = cos(ψ) ˙x − sin(ψ) ˙y

m¨x = m ˙y ˙ψ + Ffxcos δ − Ffysin δ + Frx

˙

yglob = sin(ψ) ˙x − cos(ψ) ˙y

m¨y = −m ˙x ˙ψ + Ffxsin δ + Ffycos δ + Fry Izψ¨ = l1Ffxsin δ + l1Ffycos δ − l2Fry

(3.1)

According to Rami Y. Hindiyeh and J. Christian Gerdes [11] a model like this actually is able to predict drift very similarly to a four wheel model. This should make the model suitable for the purpose of this thesis.

As previously mentioned the slip angles of the car originates from the states of the vehicle. The trigonometric equations can be seen in Equation (3.2) and Equation (3.3) . αf= δ − arctan  l1ψ + ˙˙ y ˙ x (3.2) αr= arctan  l2ψ − ˙˙ y ˙ x (3.3)

3.2 Tyre Forces

What drives the differential Equation (3.1) is the forces acting on the system. These forces are, in the given model, the forces between the tyres and the ground. • Ffx - Longitudinal force acting on front wheel, e.g. rolling resistance in the

case of the dNano car. This force is assumed zero.

• Frx - Longitudinal force acting on rear wheel, i.e. the net force from the

driveline.

• Ff_y - Lateral force acting on front wheel, depends on front slip angle. • Fr_y - Lateral force acting on rear wheel, depends on rear slip angle and also

longitudinal force.

3.2.1 Longitudinal Forces and Motor Control

The modelling of the force equation from control input, to applied force on rear wheels has been done in previous master thesis’s in the Orca Project. Equa-tion (3.4) shows the way it is done in this thesis. D is the control input (between 0-1), CM₀ is the force parameter for the electric motor, C0is a static resistive force

in the driveline and C1is a resistive force in the driveline depending on the speed

of the vehicle.

Frx = CM0D − C0− C1vx (3.4)

The parameters in the longitudinal force equations are based on previous work and tweaked empirically to fit the purpose. More about the drive line model can

(26)

14 Modelling

Parameter Value

CM0 0.1298

C0 0.0096

C1 0.0193

Table 3.1. Table showing the parameters of the electrical motor.

be found in [17]. A problem with the force on the rear wheel is that it is highly dependent on the state of the battery, and is most likely also affected by the temperature of the motor. This makes these parameters quite hard to estimate accurately. The parameters received in the work are shown in table 3.1.

The parameters influenced by rolling resistance and air resistance of the vehicle. This might not be optimal, but those forces are assumed to be very small compared to the motor force.

The front wheel rolling resistance is assumed very small, and the longitudinal force of the front wheels is thus sett to zero. This is a compromise made because the wheels actually roll very easily, and the rolling resistance of the wheels are hard to qualitatively measure.

3.2.2 Lateral Forces

The lateral tyre forces are closely, but highly non-linear, related to the so called slip angles. The slip angles are shown as αf and αrin Figure 3.1 and mathematically defined in Equation (3.2) and Equation (3.3) .

Simple Tyre Force Model

An often used model to describe the forces of a pneumatic tyre is the Pacejka’s Magic Formula. It is basically a black-box model, based on empirical data, because of the complex way the forces are generated in the contact patch of the tyre.

Equation (3.5) shows the base equation for the magic formula and the param-eters, b, c, d and e, are set through extensive measurements and fitting tools.

R(k) = d sinc arctan b(1 − e)k + e arctan(bk)

(3.5)

Figure 3.2 shows a typical example of how the lateral force could depend on the current slip angle of the tyre. At low slip angles the force has a close to linear dependancy, but then saturates at a max side force and at the end asymptotically to the max sliding force.

(27)

3.2 Tyre Forces 15

Figure 3.2. An example curve of how the lateral tyre force could depend on the slip

angle of the tyre.

The obvious problem of using such a formula for the thesis is that no high precision measurement equipment is available at the laboratory. The parameters in the formula would be very uncertain so a simpler way of modelling the tyre forces is used instead.

A simple way of approximating the tyre force is to make it proportional to the slip angle until a maximum force and then saturate the force at this level. See Figure 3.3 for a visual example. Instead of having a physical explanation to the parameters in the formula (like friction coefficient and normal force) the tyre force will depend on a force coefficient, called cornering stiffness, [2]. See Equation (3.6). The tyre stiffness is set through experiments. The saturation force was measured by a previous thesis and manually tweaked to fit the situation.

(28)

16 Modelling

Figure 3.3. An example of how the principle for the lateral forces is modelled in the

thesis. The force is proportional (linear) to the slip angle until a certain max force and is saturated there.

Fy = Cαα (3.6)

To be able to use the non differentiable equation this modelling brings forward, four different models is used in the implemented filter. One where no wheel is saturated, one with both wheels saturated, and two with either wheel saturated.

It can be added that during the automated test runs the wheels were very hard to saturate at all. However this is a zone of the wheel force which should be much more explicit when executing more extreme manoeuvres.

The Force Ellipse

There is a non negligible coupling between the possible lateral force and an applied longitudinal force on a wheel[19]. This is simply described by a degrading factor based on the so called friction ellipse. The idea is that the contact patch of the wheel against ground can induce a certain max force. Thus, if a force is applied in longitudinal direction, the maximal possible force in lateral direction is reduced.

(29)

3.2 Tyre Forces 17  Fy_deg Fymax 2 + Fxappl Fxmax 2 = 1 (3.7)

which describes how the maximum lateral force degrades when a lateral force is applied according to the friction ellipse principle, shown in Figure 3.4.

!

F

x

max

F

ymax

F

xappl

F

ydeg

F

max

Figure 3.4. A figure showing the friction ellipse principle. When a longitudinal force is

applied to the tyre, the maximum lateral force is reduced.

Because the relation between lateral force and slip angle is based (among other things) on the maximum lateral force Fy_max, or rather the reduced maximum lateral force, the whole force curve is reduced. Figure 3.5 Shows an illustration of the force reduction.

Figure 3.5. An illustration showing an example of how the slip angle to lateral force

curve changes when applying a longitudinal force on the tyre.

The equations (3.8)-(3.10) shows how the force decay is implemented in the thesis.

(30)

18 Modelling Fry = ηCrαr (3.8) Where η = Fymax Fxmax q F2 xmax− F 2 x (3.9)

The equations (3.8) and (3.9) together gives:

Fry =  Fymax Fxmax q F2 xmax− F 2 x Crαr (3.10)

In this fashion it should be possible to predict induced drift, through applied longitudinal force on the rear wheels.

3.3 Model Uncertainties and Noise Modelling

The purpose of mathematical modelling is to make a simplification of reality, but that still accurately enough describes the sought behaviour. To be able to execute Bayesian filtering, it is important to have an idea of how well you can trust the different equations and measurements in the system model.

3.3.1 Process Noise

The process noise of the model has been assumed gaussian, to make the Kalman Filter approach possible. The noise has also been assumed uncorrelated.

The process noise has been used as a tweaking parameter, to get the filter to run as wanted. Equation (3.11) shows the final result. The variances in the matrix are extremely small. The reason for this is that the measurement noise is very small and it is the size of the measurement noise compared to the process noise that is important, not the actual values. It should be possible to multiply the variance of both process noise and measurement noise with for example 10, but since the measurement noise variance comes from real data and it still gives the right scaling, it is left as it is.

vk =         0.0008 0 0 0 0 0 0 0.0800 0 0 0 0 0 0 0.0008 0 0 0 0 0 0 0.0800 0 0 0 0 0 0 0.0450 0 0 0 0 0 0 0.3200         · 10−3 (3.11)

3.3.2 Measurement Noise

As mentioned earlier in the thesis, the stochastic part of the measurement noise is extremely low. Figure 3.6 shows an example of camera measurements of the

(31)

3.3 Model Uncertainties and Noise Modelling 19

car’s position and orientation while standing completely still. Since the car is not moving, the plots show measurement noise from the cameras.

0 1 2 3 4 5 6 7 8 9 10

0.315 0.32

Camera Measurement Noise

X−position, m 0 1 2 3 4 5 6 7 8 9 10 −0.76 −0.755 Y−position, m 0 1 2 3 4 5 6 7 8 9 10 −2.75 −2.7 −2.65 Direction, radians time, s

Figure 3.6. Measurements for analysis of variance of stochastic noise in position and

orientation measurements. The variance is extremely low.

The variance of the data shown in Figure 3.6 is calculated with Matlab with the results shown in (3.12)-(3.14).

V ar(Px) = 2.05 · 10−8 (3.12)

V ar(Py) = 5.3 · 10−8 (3.13)

(32)

(33)

Chapter 4

Implementation

The implementation of the filter is done in an ”Embedded Matlab Function” in Simulink. To make the filter run with the control system, all code and simulink blocks need to be compatible with Realtime Workshop.

This chapter describes a couple of the implementation steps. For the source code of the filter implementation, see Appendix B.

4.1 Discretisation

To be able to implement the filter in Matlab/simulink, all equations needed to be discretised. This is done with the Euler method.

The Euler method simply approximates the time derivatives of the equations with the current value, minus the previous value and divides with the time in between (the sample time) [12].

˙

x ≈ x[k] − x[k − 1] Ts

(4.1) The Euler method is chosen because it is a method simple to implement. It might be seen as inaccurate, but the measurement update of 100Hz was seen as high enough to still make the Euler method very accurate for the purpose.

The resulting equations are

          

Px[k] = Px[k − 1] + Ts(cos(ψ[k − 1])vx[k − 1] − sin(ψ[k − 1]) ˙vy[k − 1])

mvx[k] = mvx[k − 1] + Ts(mvy[k − 1] ˙ψ[k − 1] + Ffxcos δ − Ffysin δ + Frx) Py[k] = Py[k − 1] + Ts(sin(ψ[k − 1])vx[k − 1] − cos(ψ[k − 1])vy[k − 1])

mvy[k] = mvy[k − 1] − Ts(mvx[k − 1] ˙ψ[k − 1] + Ffxsin δ + Ffycos δ + Fry) Izψ[k]˙ = ψ[k − 1] + T˙ s(l1Ffxsin δ + l1Ffycos δ − l2Fry)

(4.2) 21

(34)

22 Implementation

4.2 Linearisation

The linearisation of the model in an EKF is done in each iteration, so it needs to be created as a mathematical function, not as a static matrix.

What is needed in the EKF is the Jacobian of the state space model. This is extracted with Matlab’s Symbolic Toolbox. By using the variable names of the filter when extracting the Jacobian, the transfer from workspace to code was immediate.

Four different Jacobians were derived.

• One Jacobian for when no wheel forces are saturated • One Jacobian for when the rear wheel is saturated • One Jacobian for when the front wheel is saturated • One Jacobian for when both wheel’s forces are saturated.

Equation (4.3) shows, as an example, the result of the Jacobian when both front and rear wheel forces are saturated. This is the simplest case; the other Jacobians became very large and are not printed out in the report. The equation shows the Jacobian for the system of differential equations with respect to the states.



  

1 Tscos(x5) 0 Tscos(π2 + x5) −Tsx2sin(x5) − Tsx4sin(π2+ x5) 0

0 1 0 Tsx6 0 Tsx4

0 −Tssin(x5) 1 −Tssin(π2 + x5) −Tsx2cos(x5) − Tsx4cos(π2+ x5) 0

0 −Tsx6 0 1 0 −Tsx2 0 0 0 0 1 Ts 0 0 0 0 0 1     (4.3)

Equation (4.4) shows the Jacobian for the system of differentials with respect to input signals, where u1 is the throttle signal and u2is the steering signal.

          0 0 −2αrFrymaxTscos(u1) m 2Ts m 0 0 −2αrFrymaxTssin(u1) m − 2αrFrymaxTsu2 F_rxmaxm(F2 rxmax−u22)1/2 0 0 −2αrFrymaxTsl1sin(u1) Iz 2αrFrymaxTsl2u2

F_rxmaxIz(Frxmax2 −u22)1/2

          (4.4)

4.3 Realisation

The filter is pretty straight forward to implement. Codewise the Riccati-based EKF is implemented as the definition in Section 2.3 suggests, apart from that it updates different models depending on whether the wheel forces are saturated or not.

(35)

4.3 Realisation 23

Figure 4.1 shows the simulink implementation of the filter. The control signals are converted to a steering angle and a longitudinal force respectively and inputed as u. Time is inputed for the sake of initialisation and timing of filter update.

P is fed back for covariance matrix update, xhatis fed back for state estimation update, param is a vector with a couple of necessary variables. The most essential variable in ”param” is the old angle from the camera measurements, to be able to avoid phase jumps in the measurements.

Figure 4.1. The figure shows the simulink implementation of the filter. An embedded

MATLAB function, with three feedback loops. One for a couple of parameters, one for the old states and one for the old covariance matrix. Up to the left are the conversions from control signals to steering angle and longitudinal force respectively.

(36)

(37)

Chapter 5

Analysis

In this chapter some results from the state estimation applied on real scenario are presented, when a car is driven around the circuit. As a reference, the estimation from the new filter is compared to the previous estimator.

It is difficult to analyse the data, when there is no ground truth to validate towards. Therefore are the resulting conclusions based more reasoning than prov-able facts. Section 5.2 is meant to shed a little extra light on whether or not the result might be trustable.

5.1 Comparison Between Previous and Current

State Estimation

The picture 5.1 shows the estimation of a lap, by the new developed Extended Kalman Filter. The picture in it self does not give much information. What can be seen however is the effect of the extremely accurate camera measurements. Here the filter has still been given much room for it’s own predictions. This is important, so that the speeds of the vehicle is given room for a sensible estimation. If too much trust is given to the measurements, the speeds of the vehicle will be extremely ”jumpy”.

The reason for this is that the Kalman Filter simply will ”put” the car at the measurement position and set the speed to whatever places it at this position. This will defy the purpose of the filter. It is better to trust the speed to some extent, and to let the position also be corrected by the filter. After all, the position measurements are not perfect regardless of how good they are.

(38)

26 Analysis −1 −0.5 0 0.5 1 1.5 2 −1.5 −1 −0.5 0 0.5 1 1.5 x−position, m y−position, m

Position plot during lap

t = 1 s t = 2 s t = 3 s t = 4 s t = 5 s t = 6 s t = 7 s t = 8 s t = 9 s Start, t = 0 s End Estimated trajectory True Trajectory

Figure 5.1. A plot of vehicle position during 1.5 laps. The position estimation is the

blue dashed line. The position measurement given by the camera is the red solid line. Even though the estimation is nearly on top of the measurement, the filter has still been given some room for its own work, to not trust measurements entirely.

When looking at the speed estimation, seen in Figure 5.2, the longitudinal speed does not differ much between the two. Because there is no knowledge about the actual true speed, it is hard to argue which one is the best. However, an interesting thing happens in the estimation of the lateral speed. Due to the fact that the previous state estimator does not possess knowledge about how the vehicle is able to move it estimates quite high lateral speed1throughout the track. This is basically the same as estimating the car to be skidding around the whole scenario, which did not occur2. Here is a strong indicator that the new filter should possess the ability to estimate the motion of the vehicle more accurately than the old one. The reason for the small jagged structure in the longitudinal speed comes from the speed controller’s PID, not because the speed is corrected due to too much trust in position measurements. The large jagged structure comes from the speed profile of the track (which is calculated offline).

1_{Up to a third of the longitudinal speed, even during quite straight parts of the track.} 2_{This is hard to prove, but observation suggests that the car very seldomly seamed to drift}

(39)

5.1 Comparison Between Previous and Current State Estimation 27 0 2 4 6 8 10 12 −0.5 0 0.5 1 1.5 2

Longitudinal and Lateral speed, New Filter VS Previous Filter

Time, s

Speed, m/s

Longitudinal Speed, New Lateral Speed, New Longitudinal Speed, Previous Lateral Speed, Previous

Figure 5.2. A plot showing the comparison between the old state estimator and the

new state estimator. The data is recorded from the same lap as Figure 5.1. No true trajectory is present so it is only possible to speculate which one is the better one. One thing that can be said however is that the lateral speed of the old estimator seems very unlikely. It suggests that the vehicle is in a constant side drift throughout the scenario. The jagged shape (the small saw teeth, not the large ones) of the longitudinal speed comes most likely from the speed controller.

Comparing the estimation of the tyre slip of the two estimators shows the real weakness of the old filter. Figure 5.3 shows the front and rear wheel slip angles of the old and the new estimators. They are recorded from the same run as Figure 5.1. As can bee seen, the old filter predicts truly extreme slip angles of the tyres throughout the run. This comes partly from the high lateral speed seen in Figure 5.2. In the data from the old estimator, you can really recognise the profile of the lateral speed in the profile of the wheel slip in Figure 5.3 It is highly doubtable if this estimation could be used in a drift controller.

The new estimator shows very small and probable slip during the whole run. One can see a few places with notably higher slip angles, in the sharp corners of the track. This data should be accurate enough to use by a drift and high performance controller.

As been mentioned a few times already, it is hard to draw any real conclusions whether or not the new filter estimates the speed and slip angles close to the truth. What can be said is that it estimates them in a much more sensible fashion than the previously used estimator.

(40)

28 Analysis 0 2 4 6 8 10 12 −0.8 −0.6 −0.4 −0.2 0 0.2 0.4 0.6

Front and rear wheel slip angles, New Filter VS Previous Filter

Time, s

Radians

Slip angle front, New Slip angle rear, New Slip angle front, Previous Slip angle rear, Previous

Figure 5.3. The plot shows front and rear tyre slip from the same run as Figure 5.1.

Since the size of the slip angles are highly dependant of the lateral speed of the tyres, it is obvious that the speed estimation of the old filter seen in Figure 5.2 affects the slip estimation to the worse on the old filter. The new filter estimates the slip angles much more sensible.

5.2 Offline Simulation

One way to get a qualitative appreciation of how well the model in the filter actually can predict the motion of the car is to perform offline simulation and compare it to real accurate estimations. As said in Chapter 3, it is essential that the model used by the filter can represent the motion of the vehicle which is being tracked. If the model is simulated with the control inputs from the scenario, the results should resemble the true trajectory.

In offline simulations, where no updates from the camera is received, all small deviations in estimation of the states propagates through time and makes very big differences in the overall result. It is obvious when comparing Figure 5.4 to Figure 5.5, how a small error in e.g. yaw (which comes from a small error in tyre force) can change the outcome of a simulation.

As a side note it should be said that this part cannot be compared to the previous state estimator used in the project. That estimator is driven only by measurement updates and would remain at the initial position throughout the scenario.

In Figure 5.4 the position estimation of an offline simulation is compared to the true trajectory. As can be seen, the model predicts the motion of the car very well. There is of course deviations3_{, but the overall shape of the track is very}

similar. This gives confidence that the results in Section 5.1 really should be quite accurate.

(41)

5.2 Offline Simulation 29 −1 −0.5 0 0.5 1 1.5 2 −1.5 −1 −0.5 0 0.5 1 1.5 2 x−position, m y−position, m

t = 1 s t = 2 s t = 3 s t = 4 s t = 5 s t = 6 s t = 7 s t = 8 s t = 9 s Start, t = 0 s End True Trajectory Estimated trajectory

Figure 5.4. A plot showing the comparison between how the car was actually going

compared to how the model developed in Chapter 3 estimates the movement, with given input signals. To clarify, no measurement updates was given to the filter in this run. The result indicates that the model together with measurement updates really should give a very accurate prediction of the movement of the car.

An important part of the model (for the purpose of the project) is, as described in Section 3.2.2, the force decay. When the car applies force to the rear wheels, the lateral grip of the wheels lessens according to the force ellipse and a drift could be induced more easily. The filter should be able to predict such drift. Shown in Figure 5.5 is the same case as in Figure 5.4, but without force degeneration on the rear wheels.

It is obvious that this model does miss something in the prediction. The major parts where the prediction fails are at two curves. In Figure 5.5 these points are marked by a red circle, showing the first point and two blue circles showing the second point. It is, as previously declared, hard to prove that the force degeneration is actually the missing link, but intuitively it seems that way.

To further investigate the probable source, Figure 5.6 and Figure 5.7 show the lateral speed of the vehicle together with the rear wheel force at the time of the first ”missed curve”.

It is quite obvious that the lack of grip on the rear wheel (due to an applied force) in the first case causes a large lateral speed in the vehicle. In the second

(42)

30 Analysis ï1 ï0.5 0 0.5 1 1.5 2 2.5 ï2 ï1.5 ï1 ï0.5 0 0.5 1 1.5 xïposition, m yï position, m

t = 1 s t = 2 s t = 3 s t = 4 s t = 5 s t = 6 s t = 7 s t = 8 s t = 9 s Start, t = 0 s End True Trajectory Estimated trajectory

Figure 5.5. A plot showing the same scenario as Figure 5.4, but without force

degenera-tion on the rear wheels. The circles marks two places where the estimadegenera-tion is significantly worse, than previously. As seen in Figure 5.6, the sharp turns predicted in Figure 5.4 seem to come from the degeneration of the force at the rear wheels. This indicates that the model should be able to predict an initiated drift.

(43)

5.2 Offline Simulation 31 2.5 3 3.5 4 −0.1 −0.05 0 0.05 0.1

Lateral wheel force, N

Comparison between simulation and. estimation with measurements

Prediction from offline simulation Estimation from extended kalman filter

2.5 3 3.5 4 0 0.1 0.2 0.3 Time,s Lateral speed, m/s

Figure 5.6. A plot showing the rear wheel force together with lateral speed at the

time of the blue, dashed, circle in Figure 5.5. In this plot degeneration of the rear wheel lateral force is clearly making a dip in grip on the rear wheels, creating high lateral speed. Compare to Figure 5.7, to see how the curves look without degeneration.

2.5 3 3.5 4 −0.1 −0.05 0 0.05 0.1

Lateral wheel force, N

Comparison between simulation and. estimation with measurements

2.5 3 3.5 4 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 Time,s Lateral speed, m/s

Figure 5.7. A plot showing the exact same scenario as Figure 5.6 but with rear wheel

force degeneration turned off. The black circles show the time for the red circle in Figure 5.5. The simulation cannot, in this case, predict the high lateral speed of the car, which is one of the ingredients for the sharp curve predicted in Figure 5.4. This is likely to be the reason for the bad result in Figure 5.5.

(44)

(45)

Chapter 6

Possible extensions

Even though the results from this thesis turned out fine indeed, there are many things still possible to explore.

The three most obvious things possible to do in the future are: • Use state estimation to create more advanced controller • Include inertial measurements

• Expand the state vector

6.1 Advanced Control

In the project a new estimator is developed. It should be possible to do much more sophisticated control with the new knowledge. The whole idea was to do high speed and slip control.

It should for example be possible to control the drift angle of the car. It might also be possible to include boundaries for the wheel’s slip angles in an MPC controller.

6.2 Include New Measurements

The new sensing platform is further developed and should be used as a base for new measurements. There are some challenges with this however.

6.2.1 Measurement Noise

The first challenge with the new measurements, as mentioned in Appendix C, is that the acceleration seems very noisy. The measurements in themselves do not possess very much variance, but when the vehicle is moving a large amount of noise is induced.

(46)

34 Possible extensions

Maybe it is possible to look at the acceleration signals during controlled cir-cumstances and analyse the noise. Perhaps there are dynamics in the signal which can selectively be reduced through filtering before using the measurements.

6.2.2 Inclusion of Measurements

The next problem is how to include the new measurements in the model. The yaw rate is already in the state vector, which makes it straight forward to include. The accelerations of the car, however, are not natively in the state vector of the single track model.

One way of including the acceleration measurements is simply to model the ac-celerations as constant and use a double integrator as shown in Equation (6.1) and, with a Kalman Filter, estimate the speed out of the acceleration measurements. This way it is possible to include the acceleration measurements as a measurement update for the speed of the vehicle.

vk+1 ak+1 = 1 Ts 0 1 vk ak + noise (6.1)

6.2.3 Drift and Offset in Measurements

Another problem with using IMU1_{measurements is the non constant measurement}

offset2_{. This drift could be included as a constant in the state vector, and be}

mapped during filtering; An adaptive observer, like in [8]. The problem is however that there is no equations for the time varying dynamics of the acceleration of the vehicle. If the IMU measurements simply measures acceleration plus drift and both of these are modelled as constants then there is no possibility to separate the two states.

An idea of solving this problem for the longitudinal case was received by a hint in [9] and could be interesting to investigate further.

If the longitudinal force (instead of acceleration) was included in the state vector, another possibility arise. Then it is possible to make measurement updates to the longitudinal force with the motor control signal and measurement update to the longitudinal force plus offset with IMU measurement. That way it should be possible to separate the acceleration and offset from each other and at the same time receive a better accuracy of the longitudinal motion estimate thanks to the accelerometer.

6.3 Expansion of State Vector

There are ways of including more dynamics of the vehicle, to better describe its motion. If the calculation capacity would rise, or if for example the update frequency would be lowered, this would actually be a reasonable thing to do.

1_{Inertial Measurement Unit}

2_{Depending on the pitch and roll angle of the unit, it always adds a significant and slowly}

(47)

6.3 Expansion of State Vector 35

Two things possible to include in the state vector, except for the ones mentioned in Section 6.2, are rear wheel speed, motor current and the dynamics of the tyre slip angles.

The tyre slip angles dynamics would be reasonable to include, due to a better prediction to for example an MPC. This could be done by differentiating the equations of the slip angles.

˙ αf = (vy+ l1ψ)a˙ x− ayvx (δvx− vy+ l1ψ)˙ 2+ v2x (6.2) ˙ αr=

−ayvx+ (vy− l2ψ)a˙ x (vy− l2ψ)˙ 2+ vx2

(6.3)

Equation (6.2) and (6.3) show what the state equations could look like. These states could then be updated with the geometric expressions in Section 3.1.

The slip ratio3_{would be especially beneficial to include when doing drift}

con-trol. To be able to include this a tyre and an engine motion model is needed. Equations (6.4) and (6.5) show the equations needed to predict the rear wheels and the electric motor. Uinwould be the control signal4_{, kB, f and Rm}_are

resis-tance constants, Lm is the inductance in the electrical motor, Im is the moment of inertia in the driveline and kr is a torque constant5.

Lm˙i = Uin− kBωr− Rmi (6.4)

Imω˙r= f ω − kri (6.5)

Using this way of modelling the drive line it is possible to update ω with the wheel speed sensors described in Appendix C.

An obvious challenge with this approach is however that the inductance of the motor and the moment of inertia of the driveline are extremely small, making these equations hard to model stably. If it is doable, then the benefit is the possibility to compare the estimated rear wheel speed with the longitudinal speed of the car and thus get a slip ratio, which is usable in drift control.

3_{The slip ratio is the wheel speed calculated from the vehicle speed divided by the actual}

wheel speed. The idea is to receive a perception of how much the rear wheel is slipping.

4_{The control signal is actually the voltage applied to the car. At the moment the net force of}

the car is assumed linear and proportional to this voltage and therefore usable as control signal together with a force constant.

(48)

(49)

Chapter 7

Summary and conclusions

In this thesis a real-time implementation of an Extended Kalman Filter is devel-oped, for the purpose of estimating the dynamic behaviour of a 1:43 scale car from Kyosho. The goal of the state estimation is to make use of it in the future for real-time drift and high speed control in the ORCA Project. Focus is largely on estimating the slip angles of the vehicle’s tyres, since those are essential when dealing with high slip situations for ground vehicles.

7.1 The State Estimation

The developed filter produces highly reliable results and will be of benefit for future controllers in the ORCA Project. Since there is no ground truth for com-parison, the reliability is not provable at this point. Reasoning and most of all good offline simulation results, however, gives confidence in the accuracy of the state estimations.

Longitudinal speed accuracy is vital in the goal of making the vehicles in the project go faster. Previously the longitudinal speed of the cars had to be lower than the actual physical limit since the speed estimation could not be trusted entirely. With the new state estimator it should be possible to get closer to the theoretical speed limit of the cars in e.g. narrow corners.

The lateral speed and yaw rate accuracy is vital in the control of skidding and drifting situations. Since these occurrences are unstable by nature, they demand high performance output from the controller, and thus reliable state estimations. The filter developed in this thesis should be good enough for the controller to be used as reference in drift control and as limits (on e.g. slip angles in an MPC controller) for crash avoidance in high speed control. The controller is also able to predict induced drifting (by applied rear wheel torque), which is essential in the art of drifting.

Moreover is the thesis result interesting in the study of real cars safety equip-ment, since all theories and models are applicable on larger vehicles.

(50)

38 Summary and conclusions

7.2 Performance Test

The new EKF has been tested on the ORCA Project rig with success. Just by using the new, more reliable state estimation, the car runs noticeably more smoothly and steadily than previously possible.

Without any tweaking or tempering with the PID, the car reduces the lap time with almost a second. In a test with the same conditions, the old estimator could produce a lap time of 8.738 seconds where the new EKF estimator produced a lap time of 7.605 seconds.

(51)

Bibliography

[1] Mujahid Abdulrahim. On the dynamics of automobile drifting. SAE 2006-01-1019, pages 169–178, Jan 2006.

[2] R. Anderson and D.M. Bevly. Estimation of tire cornering stiffness using GPS to improve model based estimation of vehicle states. Intelligent Vehicles Symposium, June 2005.

[3] Stéphane Colas. A convex optimization approach to time-optimum path planning. Master’s thesis, ETH, Zurich, 2009. Available on: https://sites.google.com/site/orcaracer/.

[4] Srdjan Curcic. An embedded sensing platform prototype for the ORCA racecar project. Master’s thesis, ETH, Zurich, 2011. Available on: https://sites.google.com/site/orcaracer/.

[5] Ola Dahl. Realtidsprogrammering (Real-time programming). Studentlitter-atur AB, 2004.

[6] Torkel Glad and Lennart Ljung. Control Theory. Taylor & Francis Ltd, 2000. [7] Torkel Glad and Lennart Ljung. Modellbygge och Simulering (Modelling and

Simulation of Dynamic Systems). Studentlitteratur AB, 2 edition, 2003. [8] Håvard Fjær Grip, Lars Imsland, Tor A. Johansen, Thor I Fossen, Jens C.

Kalkkhul, and Avshalom Suissa. Nonlinear vehicle side-slip estimation with friction adaptation. Automatica, 44(3):611–622, 2008.

[9] Fredrik Gustafsson. Statistical Sensor Fusion. Studentlitteratur AB, 1 edition, 2010. ISBN 978-91-44-05489-6.

[10] Fredrik Gustafsson, Lennart Ljung, and Mille Millnert. Signal Processing. Studentlitteratur AB, 1 edition, 2010. ISBN 978-91-44-05489-6.

[11] Rami Y. Hindiyeh and J. Christian Gerdes. Design of a dynamic surface controller for vehicle sideslip angle during autonomous drifting. 6th IFAC Symposium Advances in Automotive Control, July 2010.

[12] Arieh Iserles. A First Course in the Numerical Analysis of Differential Equa-tions. Cambridge University Press, 2 edition, 2008.

(52)

40 Bibliography

[13] Krisada Kritayakirana and J. Christian Gerdes. Autonomous cornering at the limits. 6th IFAC Symposium Advances in Automotive Control, July 2010. [14] Faraz M. Mirzaei and Stergios I. Roumeliotis. A Kalman filter-based

algo-rithm for IMU-camera calibration: Observability analysis and performance evaluation. IEEE Transactions on Robotics, 24(5), October 2008.

[15] Hans B. Pacejka. Tyre and Vehicle Dynamics. SAE International.

[16] Martin Rutschmann. Infrared based vision system for tracking 1:43 scale race cars. Master’s thesis, ETH, Zurich, 2010. Available on: https://sites.google.com/site/orcaracer/.

[17] Patrick Spengler and Christoph Gammeter. Modeling of 1:43 scale race cars. Master’s thesis, ETH, Zurich, 2010. Available on: https://sites.google.com/site/orcaracer/.

[18] Christoph Voser, Rami Y. Hindiyeh, and J. Christian Gerdes. Analysis and control of high sideslip maneuvers. 6th IFAC Symposium Advances in Auto-motive Control, July 2010.

[19] Jo Young Wong. Theory of Ground Vehicles. John Wiley & Sons, Inc, 3 edition, 2001. ISBN 0-471-35461-9.

[20] Lukas Wunderli. MPC based trajectory tracking for 1:43 scale race cars. Master’s thesis, ETH, Zurich, 2011. Available on: https://sites.google.com/site/orcaracer/.

(53)

Appendix A

The Previous State

Estimation

In this chapter the equations of the previous state estimator of the ORCA Project are presented.

A.1 Motion Model

The motion model of the vehicle is a double integrator model without correlation between coordinates. Equation (A.1) shows the model in discrete time. Each coordinate is built out of constant speed model.

Xk =         1 Ts 0 0 0 0 0 1 0 0 0 0 0 0 1 Ts 0 0 0 0 0 1 0 0 0 0 0 0 1 Ts 0 0 0 0 0 1         · Xk−1+ vk (A.1) Cov(vk) =            T4 s 4 σ 2 Vx T3 s 2 σ 2 Vx 0 0 0 0 T3 s 2 σ 2 Vx T 2 sσV2x 0 0 0 0 0 0 Ts4 4 σ 2 Vy T_s3 2 σ 2 Vy 0 0 0 0 Ts3 2 σ 2 Vy T 2 sσ2Vy 0 0 0 0 0 0 Ts4 4 σ 2 ˙ ϕ T_s3 2 σ 2 ˙ ϕ 0 0 0 0 Ts3 2 σ 2 ˙ ϕ Tsσ2ϕ˙            (A.2)

A.2 Sensor Model

The previous sensor model is the same as used in the new filter. X-position, Y-position and orientation of the vehicle is directly measured and used in the Kalman

(54)

42 The Previous State Estimation

filter. The equation is shown in the Equations (A.3) and (A.4)

Yk=   1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0  + ek (A.3) Cov(ek) =   σ2 Px 0 0 0 0 0 0 0 σ2_P_y 0 0 0 0 0 0 0 σ2 ϕ 0   (A.4)

(55)

Appendix B

Source Code

1 function [ Alpha , xhat , P , param ] = e k f ( u , t , y , xhat_old , P_old , old_param , P_x0_small , P_v_small , P_e_small , T_s , T _ s _ f i l t , m, L , W, m_dist , l_1 , l_2 , Mu_front , Mu_rear , W_zf , W_zr , F_fy_max

, F_ry_max , F_rx_max , F_const_f , F_const_r , I_z , x h a t _ i n i t )

%%%%%%%%%%%%%%%%%%%%%%%

3 % Alpha ( 1 ) = S l i p a n g l e f r o n t % Alpha ( 2 ) = S l i p a n g l e r e a r

5 % Alpha ( 3 ) = L a t e r a l Tyre Force f r o n t % Alpha ( 4 ) = L a t e r a l Tyre Force r e a r

7 % % x h a t and x h a t _ o l d : 9 % x h a t ( 1 ) = X−p o s i t i o n , g l o b a l c o o r d i n a t e s % x h a t ( 1 ) = X−s p e e d , l o c a l c o o r d i n a t e s 11 % x h a t ( 1 ) = Y−p o s i t i o n , g l o b a l c o o r d i n a t e s % x h a t ( 1 ) = Y−s p e e d , l o c a l c o o r d i n a t e s 13 % x h a t ( 1 ) = Car a n g l e , l o c a l / g l o b a l c o o r d i n a t e s % x h a t ( 1 ) = Car a n g l e r a t e l o c a l / g l o b a l c o o r d i n a t e s 15 % % y ( 1 ) = Measured X−p o s i t i o n , g l o b a l c o o r d i n a t e s 17 % y ( 2 ) = Measured Y−p o s i t i o n , g l o b a l c o o r d i n a t e s % y ( 3 ) = Measured Car a n g l e , g l o b a l / l o c a l c o o r d i n a t e s 19 % % P and P_old : 21 % P = C o v a r i a n c e M a t r i x f o r s t a t e s %

23 % param and old_param :

% param ( 1 ) = Angle phase , f o r t h e h a n d e l i n g o f p h a s e jump

25 % param ( 2 ) = Car a n g l e , f o r t h e h a n d e l i n g o f p h a s e jump % param ( 3 ) = S t e e r i n g a n g l e

27 % param ( 4 ) = Force i n p u t % param ( 5 ) = S l i p a n g l e f r o n t

29 % param ( 6 ) = S l i p a n g l e r e a r

% param ( 7 ) = L a t e r a l Tyre Force f r o n t

31 % param ( 8 ) = L a t e r a l Tyre Force r e a r % 33 % The r e s t o f t h e i n p u t s i s m o s t l y c a r p a r a m e t e r s from t h e w o r k s p a c e , and % f i l e w h i c h a r e d e f i n e d i n car_param_setup .m; 35 %%%%%%%%%%%%%%%%%%%%%% 43

(56)

44 Source Code 37 39 % I n i t i a t e p a r a m t e r v e c t o r s param = zeros ( 8 , 1 ) ; 41 Alpha = zeros ( 4 , 1 ) ; 43 45 % I n i t i a t e s t a t e s i f ( t == 0 ) 47 xhat_old = x h a t _ i n i t ; xhat_old ( 1 ) = y ( 1 ) ; 49 xhat_old ( 3 ) = y ( 2 ) ; xhat_old ( 5 ) = y ( 3 ) ; 51 P_old = P_x0_small ; 53 old_param ( 1 ) = 0 ; old_param ( 2 ) = y ( 3 ) ; 55 old_param ( 4 ) = u ( 2 ) ; old_param ( 3 ) = u ( 1 ) ; 57 old_param ( 5 ) = 0 ; old_param ( 6 ) = 0 ; 59 end 61 63 %% To h a n d l e p h a s e jump i n c a r a n g l e measurement 65 i f y ( 3 ) − old_param ( 2 ) > 5∗ pi /6 param ( 1 ) = old_param ( 1 ) − 2∗ pi ; 67 e l s e i f y ( 3 ) − old_param ( 2 ) < −5∗pi /6 param ( 1 ) = old_param ( 1 ) + 2∗ pi ; 69 e l s e param ( 1 ) = old_param ( 1 ) ; 71 end 73 param ( 2 ) = y ( 3 ) ; y ( 3 ) = param ( 1 ) + y ( 3 ) ; 75 %%%%%%%%%%%%%%%%%%%%%%%%%%%%% 77 79 81 83 %% S a t u r a t e i n p u t f o r c e s ( b o t h n e g a t i v e and p o s i t i v e ) t o F_max i f u ( 2 ) > F_rx_max 85 u ( 2 ) = F_rx_max ; e l s e i f u ( 2 ) < −F_rx_max 87 u ( 2 ) = −F_rx_max ; end 89 % Output f o r c e s , t o i t i s p o s s i b l e t o p l o t them param ( 3 ) = u ( 1 ) ; 91 param ( 4 ) = u ( 2 ) ; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 93

State estimation of RC cars for the purpose of drift control

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

State estimation of RC cars for the purpose of drift

control

State estimation of RC cars for the purpose of drift

control

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Abstract

Sammanfattning

Acknowledgments

Contents

Chapter 1

Introduction

1.1

Background and Initial Project State

1.2

Tasks

1.3

Approach, Method and Initial Conditions

1.4

Nomenclature

1.4.1

Coordinate System

ˆ

x

ˆ

y

ˆ

X

ˆ

Y

ψ

Chapter 2

Theoretical framework

2.1

The Kalman Filter

2.2

Nonlinear State Equations

2.2.1

The Extended Kalman Filter

2.2.2

Other Possibilities

2.3

The Riccati-based EKF Algorithm

Chapter 3

Modelling

3.1

The Single Track Model

3.1.1

The Principle of the Single Track model

3.1.2

Motion Model of the Single Track Model

3.2

Tyre Forces

3.2.1

Longitudinal Forces and Motor Control

3.2.2

Lateral Forces

F

x

F

F

F

F

3.3

Model Uncertainties and Noise Modelling

3.3.1

Process Noise

3.3.2

Measurement Noise

Chapter 4

Implementation

4.1

Discretisation

4.2

Linearisation