13.1.1 Problem

We need to model the car dynamics. We will limit this to a planar model in two dimensions. We are modeling the location of the car in x∕y and the angle of the wheels, which allows the car to change direction.

13.1.2 Solution

Write a right-hand side function that can be called RungeKutta.

13.1.3 How It Works

Much like with the radar, we will need two functions for the dynamics of the automobile. RHSAutomobile is used by the simulation. RHSAutomobile has the full dynamic model including the engine and steering model. Aerodynamic drag, rolling resistance and side force resistance (the car doesn’t slide sideways without resistance) are modeled. RHSAutomobile handles multiple automobiles. An alternative would be to have a one-automobile function and call RungeKutta once for each automobile. The latter approach works in all cases, except when you want to model collisions. In many types of collisions two cars collide and then stick, effectively becoming a single car. A real tracking system would need to handle this situation. Each vehicle has six states. They are:

The velocity derivatives are driven by the forces and the angular rate derivative by the torques. The planar dynamics model is illustrated in Figure 13.1 [29]. Unlike the reference, we constrain the rear wheels to be fixed and the angles for the front wheels to be the same.

The dynamical equations are written in the rotating frame.

(13.1)

(13.2)

(13.3)

where the dynamic pressure is:

(13.4)

and

(13.5)

The unit vector is:

(13.6)

The normal force is mg, where g is the acceleration of gravity. The force at the tire contact point, where the tire touches the road, for tire k is:

(13.7)

where ρ is the radius of the tire and F_r is the rolling friction and is:

(13.8)

where is the velocity in the tire frame in the rolling direction. For front wheel drive cars, the torque, T, is zero for the rear wheels. The contact friction is:

(13.9)

This the force perpendicular to the normal rolling direction of the wheel, that is, into or out of the paper in Figure 13.2. The velocity term ensures that the friction force does not cause limit cycling. That is, when the y velocity is zero, the force is zero. μ_c is a constant for the tires.

The transformation from tire to body frame is:

(13.10)

where δ is the steering angle so that:

(13.11)

(13.12)

The kinematical equations that related yaw angle and yaw angular rate are

(13.13)

and the inertial velocity V , the velocity needed to tell you where the car is going, is:

(13.14)

We’ll show you the dynamics simulation when we get to the graphics part of the chapter in Section 13.4

13.2.1 Problem

The sensor utilized for this example will be the automobile radar. The radar measures azimuth, range, and range rate. We need two functions: one for the simulation and the second for use by the Unscented Kalman Filter.

13.2.2 Solution

Build a radar model in a MATLAB function. The function will use analytical derivations of range and range rate.

13.2.3 How It Works

The radar model is extremely simple. It assumes that the radar measures line-of-site range, range rate, and azimuth, the angle from the forward axis of the car. The model skips all the details of radar signal processing and outputs those three quantities. This type of simple model is always best when you start a project. Later on, you will need to add a very detailed model that has been verified against test data to demonstrate that your system works as expected.

The position and velocity of the radar are entered through the data structure. This does not model the signal-to-noise ratio of a radar. The power received by a radar goes as . In this model, the signal goes to zero at the maximum range that is specified in the function. The range is found from the difference in position between the radar and the target. If δ is the difference, we write:

(13.15)

Range is then:

(13.16)

The delta velocity is:

(13.17)

In both equations, the subscript r denotes the radar. The range rate is:

(13.18)

The AutoRadar function handles multiple targets and can generate radar measurements for an entire trajectory. This is really convenient because you can give it your trajectory and see what it returns. This gives you a physical feel for the problem without running a simulation. It also allows you to be sure the sensor model is doing what you expect! This is important because all models have assumptions and limitations. It may be that the model really isn’t suitable for your application. For example, this model is two-dimensional. If you are concerned about your system getting confused about a car driving across a bridge above your automobile, this model will not be useful in testing that scenario.

Notice that the function has a built-in demo and, if there are no outputs, will plot the results. Adding demos to your code is a nice way to make your functions more user friendly to other people using your code and even to you when you encounter the code again several months after writing the code! We put the demo in a subfunction because it is long. If the demo is one or two lines, a subfunction isn’t necessary. Just before the demo function is the function defining the data structure.

The second function, AutoRadarUKF is the same core code, but designed to be compatible with the Unscented Kalman Filter. We could have used AutoRadar, but this is more convenient. The transformation matrix, cITOC (inertial to car transformation) is two-dimensional, since the simulation is in a flat world.

The radar returns range, range rate, and the azimuth angle of the target. Even though we are using radar as our sensor, there is no reason why you couldn’t use a camera, laser range-finder or sonar instead. The limitation in the algorithms and software provided in this book is that it will only handle one sensor. You can get software from Princeton Satellite Systems that expands this to multiple sensors. For example, cars carry radar, cameras, and lidar. You may want to integrate all of their measurements together. Figure 13.3 shows the internal radar demo. The target car is weaving in front of the radar. It is receding at a steady velocity, but the weave introduces a time-varying range rate.

13.3.1 Problem

To have something interesting for our radar to measure, we need our cars to perform some maneuvers. We will develop an algorithm for a car to change lanes.

13.3.2 Solution

The cars are driven by steering controllers that execute basic automobile maneuvers. Throttle (accelerator pedal) and steering angle can be controlled. Multiple maneuvers can be chained together. This provides a challenging test for the MHT system. The first function is for autonomous passing and the second performs the lane change.

13.3.3 How It Works

The AutomobilePassing implements passing control by pointing the wheels at the target. It generates a steering angle demand and torque demand. Demand is what we want the steering to do. In a real automobile, the hardware will attempt to meet the demand, but there will be a time lag before the wheel angle or motor torque meets the wheel angle or torque demand commanded by the controller. In many cases, you are passing the demand to another control system that will try and meet the demand. The algorithms are quite simple. They don’t care if anyone gets in the way. They also don’t have any control for avoiding another vehicle. The code assumes that the lane is empty. Don’t try this with your car! The state is defined by the passState variable. Prior to passing, the passState is 0. During the passing, it is 1. When it returns to its original lane, the state is set to 0.

The second function performs a lane change. It implements lane change control by pointing the wheels at the target. The function generates a steering angle demand and a torque demand. The default gains work reasonably well. You should always supply defaults that make sense.

13.4.1 Problem

We want to visualize the cars as the maneuver.

13.4.2 How It Works

We create a function to read in .obj files. We then write a function to draw and animate the model.

13.4.3 Solution

The first part is to find an automobile model. A good resource is TurboSquid https://​www.​turbosquid.​com. You will find thousands of models. We need .obj format and prefer a low polygon count. Ideally, we want models with triangles. In the case of the model found for this chapter, it had rectangles so we converted them to triangles using a Macintosh application, Cheetah3D https://​www.​cheetah3d.​com. An OBJ model comes with an .obj file, an .mtl file (material file), and images for textures. We will only use the .obj file.

LoadOBJ loads the file and puts it into a data structure. The data structure uses the g field of the OBJ file to break the file into components. In this case, the components are the four tires and the rest of the car. The demo is just LoadOBJ( ’MyCar.obj’ ). You do need the extension, .obj. The car is shown in Figure 13.4.

The image is generated with one call to patch per component.

The first part of DrawComponents initializes the model and updates it. We save, and return, pointers to the patches so that we only have to update the vectors with each call.

The mesh is drawn with a call to patch. patch has many options that are worth exploring. We use the minimal set. We make the edges black to make the model easier to see. The Phong reflection model is an empirical lighting model. It includes diffuse and specular lighting.

Updating is done by rotating the vertices around the z-axis and then adding the x and y positional offsets. The input array is [x;y;yaw]. We then set the new vertices. The function can handle an array of positions, velocities, and yaw angles.

The graphics demo AutomobileDemo implements passing control. AutomobileInitialize reads in the OBJ file. The following code sets up the graphics window:

During each pass through the simulation loop, we update the graphics. We call DrawComponents once per car along with the stored patch handles for each car’s components. We adjust the limits so that we maintain a tight focus on the two cars. We could have used the camera fields in the axes data structure for this too. We call drawnow after setting the new xlim for smooth animation.

Figure 13.5 shows four points in the passing sequence.

13.5.1 Problem

You want to track a car using radar measurements to track an automobile maneuvering around your car. Cars may appear and disappear at any time. The radar measurement needs to be turned into the position and velocity of the tracked car. In between radar measurements you want to make your best estimate of where the automobile will be at a given time.

13.5.2 Solution

The solution is to implement an Unscented Kalman Filter to take radar measurements and update a dynamical model of the tracked automobile.

13.5.3 How It Works

We first create the function RHSAutomobileXY with the Kalman Filter dynamical model. The Kalman Filter right-hand side is just the differential equations.

(13.19)

(13.20)

(13.21)

(13.22)

The dot means time derivative or rate of change with time. These are the state equations for the automobile. This model says that the position change with time is proportional to the velocity. It also says the velocity is constant. Information about velocity changes will come solely from the measurements. We also don’t model the angle or angular rate. This is because we aren’t getting information about it from the radar. However, you may want to try including it!

The RHSAutomobileXY function is shown below; it is of only one code! This is because it just models the dynamics of the point mass.

The demonstration simulation is the same simulation used to demonstrate the multiple hypothesis system tracking. This simulation just demonstrates the Kalman Filter. Since the Kalman Filter is the core of the package, it is important that it works well before adding the measurement assignment part.

MHTDistanceUKF finds the MHT distance for use in gating computations using the Unscented Kalman Filter (UKF). The MHT distance is the distance between the observation and predicted locations. The measurement function is of the form h(x,d), where d is the UKF data structure. MHTDistanceUKF uses sigma points. The code is similar to UKFUpdate. As the uncertainty gets smaller, the residual must be smaller to remain within the gate.

The simulation UKFAutomobileDemo uses a car data structure to contain all of the car information. A MATLAB function AutomobileInitialize takes parameter pairs and builds the data structure. This is a lot cleaner than assigning the individual fields in your script. It will return a default data structure if nothing is entered as an argument.

The first part of the demo, is the automobile simulation. It generates the measurements of the automobile positions to be used by the Kalman Filter. The second part of the demo processes the measurements in the UKF to generate the estimates of the automobile track. You could move the code that generates the simulated data into a separate file if you were reusing the simulation results repeatedly.

The results of the script are shown in Figure 13.6, Figure 13.7, and Figure 13.8.

13.6.1 Problem

We need to demonstrate target tracking for automobiles.

13.6.2 Solution

Build an automobile simulation with target tracking.

13.6.3 How It Works

The simulation is for a two-dimensional model of automobile dynamics. The primary car is driving along a highway at variable speeds. It carries a radar. Many cars pass the primary car, some of which change lanes from behind the car and cut in front. The MHT system tracks all cars. At the start of the simulation there are no cars in the radar field of view. One car passes and cuts in front of the radar car. The other two just pass in their lanes. This is a good test of track initiation.

The radar, covered in the first recipe of the chapter, measures range, range rate, and azimuth in the radar car frame. The model generates those values directly from the target and from the tracked cars’ relative velocity and positions. The radar signal processing is not modeled, but the radar has field-of-view and range limitations. See AutoRadar.

The cars are driven by steering controllers that execute automobile maneuvers. Throttle (accelerator pedal) and steering angle can be controlled. Multiple maneuvers can be chained together. This provides a challenging test for the MHT system. You can try different maneuvers and add additional maneuver functions of your own.

The Unscented Kalman Filter described in Chapter 4 is used in this demo as the radar is a highly nonlinear measurement. The UKF dynamical model, RHSAutomobileXY, is a pair of double integrators in the inertial frame relative to the radar car. The model accommodates steering and throttle changes by making the plant covariance, both position and velocity, larger than would be expected by analyzing the relative accelerations. An alternative would be to use interactive multiple models (IMMs) with a “steering” model and “acceleration” model. This added complication does not appear to be necessary. A considerable amount of uncertainty would be retained even with IMM, since a steering model would be limited to one or two steering angles. The script implementing the simulation with MHT is MHTAutomobileDemo. There are four cars in the demo; car 4 will be passing. Figure 13.10 shows the radar measurement for car 3, which is the last car tracked. The MHT system handles vehicle acquisition well. The MHT GUI in Figure 13.11 shows a hypothesis with three tracks at the end of the simulation. This is the expected result.

Figure 13.12 shows the final tree. There are several redundant tracks. These tracks can be removed, since they are clones of other tracks. This does not impact hypothesis generation.