Kalman Filter(3) - Localisation in Continuous State Space

In past sessions, we tried to do localisation in a grid world, where our robot has different probabilities locating in different cells. In a word, the state space is discrete. To generalise the problem setting to continuous state space, we will get to the core of kalman filter.

Before we get to any details of kalman filter, please bear in mind of what we have learnt here, that there is going to be always 2 steps of kalman filter:

Sensing(Measurement): based on what the robot is seeing, update the posterior distribution
Moving(Prediction): estimate the new location after the robot makes a movement

The following has a structure of:

The ideas of extending to continuous state space
Implementing the sense(measurement) with gaussian distribution
Implementing the move(prediction) with gaussian distribution
Combine all together
Multi-dimensional Kalman Filter

Principles

In a discrete world, each state has a separate probability which adds up to 1, and the key to extend it to continuous state space is to find a continuous distribution, while here in kalman filter, the distribution is gaussian distribution.

This is saying that our estimation of the robot position always conforms to a gaussian distribution regardless it is in step measurement or prediction. The 2 parameters of gaussian each represent:

μ : the most likely estimation of robot’s location
σ : the uncertainty of our estimation caused by imprecision of detectors and predictions, etc.

And the task would be to find the final gaussian distribution with parameter μ and σ after a series of measurements and predictions. Let’s break it down into 2 parts.

Measurement(Sense)

In measurement phase, we update our belief based on the robot’s sensor. Suppose X is the location and Z is the measurement(what the robot observes), the problem we are solving is:

Our prior belief is p(X) , however our sensor sees it differently with a distribution of p(Z|X) , and now we are trying to calculate our posterior distribution p(X|Z) , that is given what we see( Z ), what is our best estimation now?

Remember that here all distributions are gaussian, in order to get the posterior distribution we only need to apply the Bayes Rule, which is to multiply these 2 distributions:

Above is the parameters of updated distribution. Notice that σ_new is smaller than both σ and γ ! This tells us that the uncertainty reduced after multiplication, which conforms to our knowledge that sensing is actually gaining information and reduce uncertainty. The implementation would be:

Prediction(Move)

Moving is addition which is straight forward. With initial distribution N(μ, σ), and move U with uncertainty γ^2, the distribution after movement is N(μ+U, σ^2+γ^2) , that is the mean is added with U and uncertainty is added with γ^2 .

Notice that by making a movement which is uncertain, the variance(uncertainty) increases which is in contrast with measurement that reduces variance(uncertainty).

1D Kalman Filter

Now combining measurement and prediction we got:

In the example, we set the initial position mu = 0 and uncertainty sig = 10000 , meaning we are super uncertain with the robot’s initial position. After a few rounds of iteration, we got the result:

We saw that by making the first measurement, the uncertainty instantly drops to 3.998 , which is smaller than both the initial variance and measurement variance. And finally we predict our robot landing in position 10.9999 with variance 4.0058 .

Multi-dimensional Kalman Filter

The above example we have only one variable which is the position p . Now let’s consider a second variable velocity v , then our task will be to get the final estimation of x = (p, v) and uncertainty matrix P = cov(x) .

Prediction

In the prediction phase, suppose we have the relation:

That given a speed and time t , we can get our next position and the velocity we assume remains the same. Then we got its matrix form:

Here F is called state transition matrix, P is covariance matrix and μ is external motion vector(say there is acceleration or pedal throttle).

Measurement

Same goes with measurement, there is a measurement transition matrix H . In its simplest form, let’s assume that H = (1, 0) and,

 μ_0 = Hx = (1, 0)(p, v)^T = p

This is saying that our measurement can only measure the position of the robot and we can not see the velocity. Now the problem turns into that we have two gaussian distributions, one is(the prior distribution):

and the second is(the measurement distribution):

Looks familiar with our previous examples, right? Now we need to combine these two distributions into posterior distribution. Here involves a series of deduction(one excellent explanation here), and I will put the result here: