Phase 2: Updating Your Prediction From State to Sensors While we were busy predicting, the GPS on our RC car was giving us positional data updates. These zₜ give us valuable information about our world. sensor measurements However, zₜ and our state vector xₜ₊₁ may not actually correspond; our measurements might be in one space, and our state in another! For instance, what if we’re measuring our state in meters, but all of our measurements are in feet? We need some way to remedy this. Let’s handle this by converting our state vector into our measurement space using an : observation matrix H These equations represent the mean μₑₓₚ and covariance Σₑₓₚ of our predicted measurements. For our RC car, we’re going from meters to feet, in both position and velocity. 1 meter is around 3.28 ft, so we would shape H to reflect this: …leaving us with predicted measurements that we can now compare to our sensor measurements zₜ. Note that is entirely dependent on what’s in your state and what’s being measured, so it can change from problem to problem. H Fig. 1: Moving our state from state space (meters, bottom left PDF) to measurement space (feet, top right PDF). Our RC car example is a little simplistic, but this ability to translate our is a big part of what makes Kalman filters so powerful. We can effectively compare our state with any and all sensor measurements, from any sensor. That’s powerful stuff! predicted state into predicted measurements An aside: The behavior of is important. Vanilla Kalman filters use a linear Fₜ and ; that is, there is only one set of equations relating the estimated state to the predicted state (Fₜ), and predicted state to predicted measurement ( ). H H H If the system is , then this assumption doesn’t hold. Fₜ and might change every time our state does! This is where innovations like the (EKF) and the come into play. EKFs are the de facto standard in sensor fusion for this reason. non-linear H Extended Kalman Filter Unscented Kalman Filter Let’s add one more term for good measure: Rₜ, our This represents the noise from our measurements. Everything is uncertain, right? It never ends. sensor measurement covariance. The Beauty of PDFs Since we converted our state space into the measurement space, we now have 2 comparable Gaussian PDFs: μₑₓₚ and Σₑₓₚ, which make up the Gaussian PDF for our predicted measurements zₜ and Rₜ, which make up the PDF for our sensor measurements The strongest probability for our future state is the overlap between these two PDFs. How do we get this overlap? Fig. 2: Our predicted state in measurement space is in red. Our measurements z are in blue. Notice how our measurements z have a much smaller covariance; this is going to come in handy. We multiply them together! The product of two Gaussian functions is just another Gaussian function. Even better, this common Gaussian has a than either the predicted PDF or the sensor PDF, meaning that our state is now much more certain. smaller covariance ISN’T THAT NEAT. Update Step, Solved. We’re not out of the woods yet; we still need to derive the math! Suffice to say… it’s a lot. The basic gist is that multiplying two Gaussian functions results in its own Gaussian function. Let’s do this with two PDFs now, Gaussian functions with means μ₁, μ₂ and variances σ₁², σ₂². Multiplying these two PDFs leaves us with our final Gaussian PDF, and thus our final mean and covariance terms: When we substitute in our derived state and covariance matrices, these equations represent the of a Kalman filter. update step See reference ( ) for a good explanation on how to multiply two PDFs and derive the above. It takes a good page to write out; you’ve been warned. Bromiley, P.A. This is admittedly pretty painful to read as-is. We can simplify this by defining the Kₜ: Kalman Gain With Kₜ in the mix, we find that our equations are much kinder on the eyes: We’ve done it! Combined, the new create our final Gaussian distribution, the one that crunches all of that data to give us our updated state. See how our updated state spikes in probability (the blue spike in Fig. 3). That’s the power of Kalman Filters! x and P Fig. 3: Original state in green. Predicted state in red. Updated final state in blue. Look at how certain we are now! Such. Wow. What Now? Well… do it again! The Kalman filter is recursive, meaning that it uses its output as the next input to the cycle. In other words, The cycle continues in the next prediction state. the final x is your new xₜ ! Kalman filters are great for all sorts of reasons: They extend to anything that can be modeled and measured. Automotives, touchscreens, econometrics, etc etc. Your sensor data can come from anywhere. As long as there’s a connection between state and measurement, new data can be folded in. Kalman filters also allow for the selective use of data. Did your sensor fail? Don’t count that sensor for this iteration! Easy. They are a good way to model prediction. After completing one pass of the prediction step, just do it again (and again, and again) for an easy temporal model. Of course, if this is too much and you’d rather do… anything else, is developing solutions to these very same problems! We’re creating tools that help any vision-enabled system operate to its best. If you like this article, you’ll love seeing what we’re up to. Tangram Vision Code Examples and Graphics The code used to render these graphs and figures is hosted on Tangram Vision’s public repository for the blog. Head down, check it out, and play around with the math yourself! If you can improve on our implementations, even better; we might put it in here. And be sure to with your improvements. tweet at us Check it out the Tangram Vision Github here Further Reading If this article didn’t help, here are some other sources that dive into Kalman Filters in similar ways: Faragher, R. (2012, September). Understanding the Basis of the Kalman Filter. https://drive.google.com/file/d/1nVtDUrfcBN9zwKlGuAclK-F8Gnf2M_to/view Bzarg. How a Kalman filter works, in pictures. https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ Teammco, R. Kalman Filter Simulation. https://www.cs.utexas.edu/~teammco/misc/kalman_filter/ Bromiley, P.A. Products and Convolutions of Gaussian Probability Density Functions. [PDF] https://bit.ly/3nGDewe Also published on: https://medium.com/tangram-visions/one-to-many-sensor-trouble-part-2-3a9212c1fef9

Google

Velocity

We make it simple to manage and optimize perception sensors for vision-enabled platforms like robots, drones and AVs.

How to Kalman Filter Your Way Out (Part 2: Updating Your Prediction)

Too Long; Didn't Read

People Mentioned

Companies Mentioned

Tangram Vision

About Author

TOPICS

THIS ARTICLE WAS FEATURED IN...