Making Sense of Sensing in Self Driving Cars
Understanding key fundamentals of Unscented Kalman Filters
Autonomous vehicles are getting quite the limelight in recent times. Its research and usage has a tremendous influence in our everyday lives, and will change the way we live and work! The study of self-driving cars — in particular — has the potential to change the very nature of transport and commuting. That includes superior mobility, reduced infrastructure costs, giving respite to traffic related chaos, and of course — lesser accidents!
In this blog post, I hope to help you get a good insight on how some of the components behind self driving cars work. More specifically —
How do self driving cars navigate and make sense of their surroundings?
The ability of a self driving car to sense its surroundings, is of paramount importance. The car needs to be able to sense the presence of other cars and objects on the road, in order to navigate and transport the passenger safely and observe traffic rules.
We’ll begin with learning some of the essential pieces behind sensor fusion, that’s used by self driving cars to track other objects on the road. Here are the contents of this tutorial:
Part 1: (What you’re reading right now):
- What are Kalman Filters?
- Learn how to develop a process model
So let’s begin!!
What are Kalman filters? When to use them?
Here are the 3 important scenarios where Kalman filters are useful .
Indirect Measurements
Consider the case of monitoring the temperature of the internal combustion engine of a rocket. But the powerful rocket propellant burns with extreme intensity at 5500 degrees Fahrenheit.
This is not an easy task, since a sensor placed inside the chamber would melt. Instead, it needs to be placed on a cooler surface close to the chamber. The problem you’re facing here is that you want to measure internal temperature of the chamber but you can’t. Instead, you have to measure external temperature. In this situation, you can use a Kalman filter to find the best estimate of the internal temperature from an indirect measurement. This way, you’re extracting information about what you can’t measure from what you can.
Fusing measurements
A Kalman filter can be used to fuse the measurements from different sensors to find the optimal estimate of the state of the system.
In the case illustrated above, The inertial measurement unit (IMU) uses accelerometers and gyroscopes to measure the car’s acceleration and angular velocity. The odometer measures the relative distance traveled by the car. The GPS receiver receives signals from satellites to locate the car on earth’s surface.
But here are the issues involved in directly considering the measurement from the above sensors.
- IMU’s are prone errors in their measurements.
- Odometer measurements may be affected by the tire pressure and road conditions
- The GPS receiver provides your absolute location, but it gets updated less frequently and it may be noisy.
So What do we do?
In this scenario, a Kalman filter can be used to fuse these three measurements to find the optimal estimate of the exact position of the car.
Approximating actual values in case of uncertainty in measurements.
The sensor measurements are not generally accurate, they usually have unpredictable, random and uncertain error/variation in their measurements. What does that mean ? Consider a thermometer which reads/measures room temperature, In a room with steady/constant temperature the sensor outputs a varying temperature measurement within a given range, the readings will be scattered around the actual room temperature, this is because the sensors generally have a degree of uncertainty in their measurement.
Using Kalman filter will help us quickly estimate the actual room temperature by making use of the uncertain measurements.
Motion models — CTRV (Constant Turn Rate Velocity) model
A motion model makes certain assumptions about objects motion, there are various motion models, in this case we’ll work in with CTRV model.
In CTRV motion model the car moves along a straight line, but also it can move with constant turn rate and velocity magnitude
The following parameters constitutes the state vector for CTRV model,
- px: The X co-ordinate of the car’s position.
- py: The Y co-ordinate of the car’s position.
- v : The velocity magnitude of the car. This is constant in CTRV model.
- ψ : The orientation of the car or the yaw angle. In CTRV model This keeps changing at a constant rate when the car takes a turn.
- ψ` : The rate change of yaw angle, yaw rate. This is constant in CTRV model.
Process model
Given the state vector of the car at time k and the noise vector, the process model predicts the state vector at time k + 1.
Let’s derive the process model!
We’ll use a popular procedure wherein the change rate of the state will be used to derive the process model.
We can derive the relationship between the change rate of the state with the state itself. The change rate of the state is nothing but a differential equation of the states,
For example, let’s derive the rate change of position px and express it in terms of the state variable.
Rate change of x position (px) = velocity in x direction (vx).
But vx is not part of the state, but we can express it using the velocity magnitude(v) and the yaw (ψ), these two are part of the state.
Similarly let’s express the other derivatives of the state in terms of the state itself,
- py˙ = sin(ψ)⋅v
- Since velocity magnitude(v) is constant its derivative is 0.
- The change rate of yaw, that is, the yaw rate is already part of the state, so it can be just included.
- Since the yaw rate is constant, the rate change of yaw rate is 0.
Here is the rate change of the states expressed in terms of state themselves,
Showtime !
State x(k+1) at time k+1 is given by,
state at time k + 1 = state at time k + change in state from time k to k+1x(k+1) = x(k) + change in the state in time Δtx(k+1) = x(k) + use rate change of state and sum it up for every time stepx(k+1) = x(k) + sum_it_up_every_time_step(rate change of the state)x(k+1) = x(k) + integral_of(rate change of the state)
x(k+1) = x(k) + Integral_of( rate change of state).
Δt is the time difference between time k+1 and time k.
Here is a small video which could help you understand the relationship between integral and differential terms and how integral is related to summation.
Here is the step one of the deduction of the integral.
Since the rate change of velocity and yaw rate are 0, they sum upto 0 after the integration.
Rate change of px and py are dependent on the yaw ψ, (rate change of px = cos(ψ)⋅v), If the yaw (ψ) were to be constant than the integral_of(rate change of px) would have been just cos(ψ)⋅v Δt. But since the yaw (ψ), that is, the orientation of the car will be changing during a turn at a constant yaw rate in CTRV model, the integral for rate change of px and py are not simple. The summation of its value across every step has to be taken into account and its integral is evolved.
Now let’s get into the next step of solving the first two integrals,
First we need to find an equation to express the variables in the integral at any time t in future. As you can see the only variable in the integral is the yaw (ψ). Here is how we can express the yaw at any time t in the future
yaw = yaw + yaw_rate * ΔtSince velocity magnitude v is a constant it can moved out.
Now we need to solve the following integrals to complete the process model,
Here is the solution for the integral of above form,
By using the formula above we arrive at the following solution for the integrals,
Here’s the final solution and our process model for CTRV motion,
Let’s step into further steps of the Unscented Kalman filters in the next part of the blog. Till then, Happy coding !!
Image credits : Udacity , Mathworks, iLectureonline
