Kalmanfilter_basics_2


KF Basics - Part 2

Probabilistic Generative Laws

1st Law:

The belief representing the state $x_{t}$, is conditioned on all past states, measurements and controls. This can be shown mathematically by the conditional probability shown below:

$$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})$$

1) $z_{t}$ represents the measurement

2) $u_{t}$ the motion command

3) $x_{t}$ the state (can be the position, velocity, etc) of the robot or its environment at time t.

'If we know the state $x_{t-1}$ and $u_{t}$, then knowing the states $x_{0:t-2}$, $z_{1:t-1}$ becomes immaterial through the property of conditional independence'. The state $x_{t-1}$ introduces a conditional independence between $x_{t}$ and $z_{1:t-1}$, $u_{1:t-1}$

Therefore the law now holds as:

$$p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t} | x_{t-1},u_{t})$$

2nd Law:

If $x_{t}$ is complete, then:

$$p(z_{t} | x_{0:t},z_{1:t-1},u_{1:t})=p(z_{t} | x_{t})$$

$x_{t}$ is complete means that the past states, controls or measurements carry no additional information to predict future.

$x_{0:t-1}$, $z_{1:t-1}$ and $u_{1:t}$ are conditionally independent of $z_{t}$ given $x_{t}$ of complete.

The filter works in two parts:

$p(x_{t} | x_{t-1},u_{t})$ -> State Transition Probability

$p(z_{t} | x_{t})$ -> Measurement Probability

Conditional dependence and independence example:

$\bullet$Independent but conditionally dependent

Let's say you flip two fair coins

A - Your first coin flip is heads

B - Your second coin flip is heads

C - Your first two flips were the same

A and B here are independent. However, A and B are conditionally dependent given C, since if you know C then your first coin flip will inform the other one.

$\bullet$Dependent but conditionally independent

A box contains two coins: a regular coin and one fake two-headed coin ((P(H)=1). I choose a coin at random and toss it twice. Define the following events.

A= First coin toss results in an H.

B= Second coin toss results in an H.

C= Coin 1 (regular) has been selected.

If we know A has occurred (i.e., the first coin toss has resulted in heads), we would guess that it is more likely that we have chosen Coin 2 than Coin 1. This in turn increases the conditional probability that B occurs. This suggests that A and B are not independent. On the other hand, given C (Coin 1 is selected), A and B are independent.

Bayes Rule:

Posterior = $$\frac{Likelihood*Prior}{Marginal} $$

Here,

Posterior = Probability of an event occurring based on certain evidence.

Likelihood = How probable is the evidence given the event.

Prior = Probability of the just the event occurring without having any evidence.

Marginal = Probability of the evidence given all the instances of events possible.

Example:

1% of women have breast cancer (and therefore 99% do not). 80% of mammograms detect breast cancer when it is there (and therefore 20% miss it). 9.6% of mammograms detect breast cancer when its not there (and therefore 90.4% correctly return a negative result).

We can turn the process above into an equation, which is Bayes Theorem. Here is the equation:

$\displaystyle{\Pr(\mathrm{A}|\mathrm{X}) = \frac{\Pr(\mathrm{X}|\mathrm{A})\Pr(\mathrm{A})}{\Pr(\mathrm{X|A})\Pr(\mathrm{A})+ \Pr(\mathrm{X | not \ A})\Pr(\mathrm{not \ A})}}$

$\bullet$Pr(A|X) = Chance of having cancer (A) given a positive test (X). This is what we want to know: How likely is it to have cancer with a positive result? In our case it was 7.8%.

$\bullet$Pr(X|A) = Chance of a positive test (X) given that you had cancer (A). This is the chance of a true positive, 80% in our case.

$\bullet$Pr(A) = Chance of having cancer (1%).

$\bullet$Pr(not A) = Chance of not having cancer (99%).

$\bullet$Pr(X|not A) = Chance of a positive test (X) given that you didn't have cancer (~A). This is a false positive, 9.6% in our case.

Bayes Filter Algorithm

The basic filter algorithm is:

for all $x_{t}$:

  1. $\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1})dx$

  2. $bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)$

end.

$\rightarrow$The first step in filter is to calculate the prior for the next step that uses the bayes theorem. This is the Prediction step. The belief, $\overline{bel}(x_t)$, is before incorporating measurement($z_{t}$) at time t=t. This is the step where the motion occurs and information is lost because the means and covariances of the gaussians are added. The RHS of the equation incorporates the law of total probability for prior calculation.

$\rightarrow$ This is the Correction or update step that calculates the belief of the robot after taking into account the measurement($z_{t}$) at time t=t. This is where we incorporate the sensor information for the whereabouts of the robot. We gain information here as the gaussians get multiplied here. (Multiplication of gaussian values allows the resultant to lie in between these numbers and the resultant covariance is smaller.

Bayes filter localization example:


In [3]:
from IPython.display import Image
Image(filename="bayes_filter.png",width=400)


Out[3]: