One-to-Many Sensor Trouble — Part I In the world of automated vision, there’s only so much one can do with a single sensor. Optimize for one thing, and lose the other; and even with one-size-fits-all attempts, it’s hard to paint a full sensory picture of the world at 30fps. We use sensor combinations to overcome this restriction. This intuitively seems like the right play; more sensors mean more information. The jump from one camera to two, for instance, unlocks binocular vision, or the ability to see behind as well as in front. Better yet, use three cameras to do both at once. Add in a LiDAR unit, and see farther. Add in active depth, and see with more fidelity. Tying together multiple data streams is so valuable this act of is a whole discipline in itself. Sensor Fusion Yet this boon in information often makes vision-enabled systems harder to build, not easier. Binocular vision relies on stable intrinsic and extrinsic camera properties, which cameras don’t have. Depth sensors lose accuracy with distance. A sensor can fail entirely, like LiDAR on a foggy day. This means that effective sensor fusion involves constructing vision architecture in a way that . Sensors aren’t perfect, and data can be noisy. It’s the job of the engineer to sort this out and derive assurances about what is actually true. This challenge is what makes sensor fusion so difficult: it takes competency in information theory, geometry, optimization, fault tolerance, and a whole mess of other things to get right. minimizes uncertainty in uncertain conditions So how do we start? Guessing …just kidding. Though you would be surprised how many times an educated guess gets thrown in! No, we’re talking Kalman filters So let’s review our predicament: We have a rough idea of our current (e.g. the position of our robot), and we have a of how that state changes through time. state model We have , each with their own data streams. multiple sensor modalities All of these sensors give noisy and uncertain data. Nonetheless, this is all that we have to work with. This seems troubling; we can’t be certain about anything! Instead, what we do is . Through the beauty of mathematics, we can combine all of this knowledge and actually come out with a idea of our state through time than if we used any one sensor or model. can minimize our uncertainty more certain This 👆 is the magic of Kalman filters. Warning: Math. Phase 1: Prediction Model Behavior Let’s pretend that we’re driving an RC car in a completely flat, very physics-friendly line. There are two things that we can easily track about our car’s state: its position pₜ and velocity vₜ. We can speed up our robot by punching the throttle, something we do frequently. We do this by exerting a force on the RC car’s mass , resulting in an acceleration (see Newton’s Second Law of Motion). f m a With just this information, we can derive a model for how our car will act over a time period Δ using some classical physics: t We can simplify this for ourselves using some convenient matrix notation. Let’s put the values we can track, position pₜ and velocity vₜ, into a state vector: …and let’s put out applied forces into a that represents all the outside influences affecting our state: control vector Now, with a little rearranging, we can organize our motion model for position and velocity into something a bit more compact: By rolling up these terms, we get some handy notation that we can use later: Fₜ is called our . It models what our system would do over Δ , given its current state. prediction matrix t Bₜ is called our This relates the forces in our control vector uₜ to the state prediction over Δ control matrix. t. Uncertainty through PDFs However, we’re not sure whether or not our state values are true to life; there’s uncertainty! Let’s make some assumptions about what this uncertainty might look like in our system: exactly Any error we might get in an observation is inherently random; that is, there isn’t a bias towards one result. Errors are independent of one another. These two assumptions mean that our uncertainty follows the ! We can therefore assume that our error follows a , aka a . Central Limit Theorem Normal Distribution Gaussian curve We will use our understanding of Gaussian curves later to great effect, so take note! We’re going to give this uncertainty model a special name: a This represents how probable it is that certain states are the state. Peaks in our function correspond to the states that have the highest probability of occurrence. probability density function (PDF). true Fig. 1. Our first PDF Our state vector xₜ represents the mean μ of this PDF. To derive the rest of the function, we can model our state uncertainty using a Pₜ: covariance matrix There are some interesting properties here in Pₜ. The diagonal elements (Σₚₚ, Σᵥᵥ) represent how much these variables deviate from their own mean. We call this variance. The off-diagonal elements of Pₜ express between state elements. If Σₚᵥ is zero, for instance, then we know that an error in velocity won’t influence an error in position. If it’s any other value, we can safely say that one affects the other in some way. PDFs without covariance terms look like Figure 1 above, with major and minor axes aligned with our world axes. PDFs covariance are skewed off-axis depending on how extreme the covariance is: covariance with Fig. 2. A PDF with non-zero covariance. Notice the ‘tilt’ in the major and minor axes of the ellipsis. Variance, covariance, and the related of variables are valuable, as they make our PDF more information-dense. correlation We know how to predict xₜ₊₁, but we also need the predicted covariance Pₜ₊₁ if we’re going to describe our state fully. We can derive it from xₜ₊₁ using some (drastically simplified) linear algebra: Notice that Bₜuₜ got tossed out! Control has no uncertainty that we can directly observe, so we can’t use the same math that we did on Fₜxₜ. However, we can factor in the effects of noisy control inputs another way: by adding a Qₜ: process noise covariance matrix Yes, we are literally adding noise. Prediction step, solved? We have now derived the full : prediction step Our results are… ok. We got a good guess at our new state out of this process, sure, but we’re a lot more uncertain than we used to be! Fig. 3. Starting state PDF in green, predicted state PDF in red. Notice how the distribution is more spread out. Our state is less certain than before. There’s a good reason for that: everything up to this point has been a sort of “best guess”. We have our state, and we have a model of how the world works; all that we’re doing is using both to predict what might happen over time. We still need something to outside of our model. support these predictions Something like sensor measurements, for instance. We’re getting there! So far, this post has covered: The motivation for using a Kalman filter in the first place A toy use case, in this case our physics-friendly RC car The Kalman filter prediction step: our best guess at where we’ll be, given our state We’ll keep it going in Part II by bringing in our sensor measurements (finally). We will use these measurements, along with our PDFs, to uncover the true magic of Kalman filters! Spoiler: it’s not magic. It’s just more math. Happy new year from the team! Tangram Vision

BOON

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

Too Long; Didn't Read

People Mentioned

Companies Mentioned

Tangram Vision

About Author

TOPICS

THIS ARTICLE WAS FEATURED IN...