Kalman Filter

Background:

• Kalman is born on 19th may 1930 (86 years old).

• Earned his bachelor’s from MIT in 1953.

• Earned his master’s from MIT in 1954.

• Completed his PhD in 1957 from Columbia University.

• Discovered Kalman Filter in 1960.

To understand Kalman Filter we will use three
cases:
• Static Case or Static State
• Dynamic state without control input
• Dynamic state with control input

All variables of interest of a system at a particular time determine the state of the system.
Static case
• Estimation of location of a ship lost in the sea.
Assuming that the effect of waves
is random in nature, i.e., the waves
do not impart any velocity to the
ship.
 2
µ1 𝜎2 + µ2 𝜎1 2 𝜎1 2 𝜎2 2
µ𝑓𝑢𝑠𝑒𝑑 = 𝜎𝑓𝑢𝑠𝑒𝑑 2 = 2
𝜎1 2 + 𝜎2 2 𝜎1 + 𝜎2 2

𝜎1 2 (µ2 − µ1 ሻ 𝜎1 4
µ𝑓𝑢𝑠𝑒𝑑 = µ1 + 𝜎 2𝑓𝑢𝑠𝑒𝑑 2
= 𝜎1 − 2
𝜎1 2 + 𝜎2 2 𝜎1 + 𝜎2 2
𝑜𝑟
2 2
𝜎2 4
𝜎 𝑓𝑢𝑠𝑒𝑑 = 𝜎2 − 2
𝜎1 + 𝜎2 2
Assumptions used in Kalman Filter

• System is linear

• Process noise distribution is Gaussian

• Measurement noise is Gaussian

Dynamic State without control Input
Suppose that the state of a robot is completely known if we
know the position of the robot p(t) at a particular time and its
velocity v(t) at that instant
Robot moving along a
straight line

So the state vector x(t) contains the position and velocity of the
Robot robot
𝑝 𝑡
𝑥 𝑡 =
𝑣 𝑡
Position

𝑑𝑥
The motion is of the simple form =𝑣+𝑤
𝑑𝑡
 Kalman Filter Terminology
• 𝑥ො 𝑡2 =Optimal estimate at time 𝑡2

• 𝑥ො 𝑡1 = Estimated value at time 𝑡1

• 𝑥ො 𝑡2 − = Predicted value (mean) for 𝑡2

• 𝑧2 = Measured value of state at time 𝑡2

• 𝜎𝑥 2 𝑡2 = Modified variance of the optimal estimate

• 𝜎𝑥 2 𝑡1 = Variance of estimated value at time 𝑡1

• 𝜎𝑥 2 𝑡2 − = Variance of predicted value at time 𝑡2

• 𝐾 𝑡2 = Kalman gain for this particular system at time 𝑡2

 𝑥ො 𝑡2 − = 𝑥ො 𝑡1 + 𝑣[𝑡2 − 𝑡1 ] Now we have 𝑥ො 𝑡2 − from the prediction and 𝑧2 from
measurement. So our next task is to combine these two
𝜎𝑥 2 𝑡2 − = 𝜎𝑥 2 𝑡1 + 𝜎𝑤 2 𝑡2 − 𝑡1
to arrive at the best possible estimate for the state 𝑡2 .
 𝜎1 2 (µ2 − µ1 ሻ 𝜎1 4
µ𝑓𝑢𝑠𝑒𝑑 = µ1 + 𝜎 2𝑓𝑢𝑠𝑒𝑑 = 𝜎1 2 − 2
𝜎1 2 + 𝜎2 2 𝜎1 + 𝜎2 2

By the same process as before, the density with mean 𝑥ො 𝑡2 − and variance 𝜎𝑥 2 𝑡2 − is combined with the
density with mean 𝑧2 and variance 𝜎 2 𝑧2 , to yield a Gaussian density with mean
𝑥ො 𝑡2 = 𝑥ො 𝑡2 − + 𝐾 𝑡2 [𝑧2 − 𝑥ො 𝑡2 − ]
and variance
𝜎𝑥 2 𝑡2 = 𝜎𝑥 2 𝑡2 − − 𝐾 𝑡2 𝜎𝑥 2 𝑡2 −
where the gain 𝐾 𝑡2 is given by
𝜎𝑥 2 𝑡2 −
𝐾 𝑡2 =
[𝜎𝑥 2 𝑡2 − + 𝜎 2 𝑧2 ]
 Dynamic case with control Input

We provide constant acceleration to the robot. The control vector can be expressed as u(t)
𝑓
𝑢 𝑡 =
𝑚

From kinematic relationships we know that

𝑓(𝛥𝑡ሻ2
𝑝 𝑡2 = 𝑝 𝑡1 + 𝑣 𝑡1 × 𝛥𝑡 +
2𝑚
𝑓(𝛥𝑡ሻ
𝑣 𝑡2 = 𝑣 𝑡1 +
𝑚
In matrix form this can be written as

𝑥 𝑡2 =
𝑝 𝑡2
=
1 𝛥𝑡 𝑝(𝑡1 ሻ
+ 𝛥𝑡 2 Τ2 𝑓
𝑣 𝑡2 0 1 𝑣(𝑡1 ሻ 𝛥𝑡 𝑚
We introduce a negative superscript on time values to describe predicted value of the state at that time. So

𝑥ො 𝑡2 − represents predicted state of the system at time 𝑡2 . So,

𝑥ො 𝑡2 − =
1 𝛥𝑡
𝑥ො 𝑡1 + 𝛥𝑡 2 Τ2 𝑢 𝑡
1
0 1 𝛥𝑡

Let, F =
1 𝛥𝑡
𝑎𝑛𝑑 𝐵= 𝛥𝑡 2 Τ2 , then
0 1 𝛥𝑡

ෝ 𝒕𝟐 − = 𝑭ෝ
𝒙 𝒙 𝒕𝟏 + 𝑩𝒖 𝒕𝟏

Where, F = State transition matrix and

B = Control Input Matrix

Actual state can be expressed as

𝑥 𝑡2 = 𝐹 𝑥ො 𝑡1 + 𝐵𝑢 𝑡1 + 𝑤(𝑡1 ሻ

Where, 𝑤(𝑡1 ሻ = noise that is assumed to be Gaussian

𝑤(𝑡1 ሻ is a vector and can be expressed as

𝑤1 (𝑡1 ሻ
𝒘(𝒕𝟏 ሻ =
𝑤2 (𝑡1 ሻ

Expressing the covariance matrix by 𝑃𝑡 ,

𝑃(𝑡2− ሻ = 𝐸[ 𝑥 𝑡2 − 𝑥ො 𝑡2 − 𝑥 𝑡2 − 𝑥ො 𝑡2 − 𝑇]

Upon simplification this will reduce to

𝑷(𝒕𝟐 − ሻ = 𝑭𝑷(𝒕𝟏 ሻ 𝑭𝑻 + 𝑸𝒕

Where, 𝑄𝑡 is the process noise covariance matrix. 𝑄𝑡 = [𝒘𝒘𝑻 ].

Most of the times the prediction and measurements are mapped into a common domain, usually to measurement domain.
𝑥ො 𝑡 −
(𝑠− 2 ሻ2
− 𝑐
1 𝜎𝑥 (𝑡2 − ሻ 2
𝑥𝑝𝑟𝑒𝑑𝑖𝑐𝑡𝑖𝑜𝑛 𝑠, 𝑥ො 𝑡2 − , 𝜎𝑥 2 𝑡2 − , 𝑐 ≜ 𝑒 2( 𝑐 ሻ
𝜎𝑥 (𝑡2 − ሻ 2
2𝜋( ሻ
𝑐
and
(𝑠−𝑧2 ሻ2
1 −
2𝜎2 𝑧2
𝑧 𝑠, 𝑧2 , 𝜎 2 𝑧2 , 𝑐 ≜ 𝑒
2𝜋𝜎 2 𝑧2

𝜎𝑥 𝑡2 − 2 𝑥ො 𝑡2 −
µ𝑓𝑢𝑠𝑒𝑑 𝑥ො 𝑡2 − ( ሻ (𝑧2 − ሻ
= + 𝑐 𝑐
𝑐 𝑐 𝜎𝑥 𝑡2 − 2 2
( ሻ +𝜎 𝑧2
𝑐

𝜎𝑥 2 𝑡2 − 𝑥ො 𝑡2 −
( ሻ(𝑧2 − ሻ
ෝ 𝒕𝟐 = 𝑥ො 𝑡2 −
⇒𝒙 + 𝑐 𝑐
𝜎𝑥 𝑡2 − 2 2
( ሻ +𝜎 𝑧2
𝑐
𝐻𝜎𝑥 2 𝑡2 −
Substituting H= 1/c and 𝑲 𝒕𝟐 = results in
𝐻𝜎𝑥 2 𝑡2 − +𝜎2 𝑧2

ෝ 𝒕𝟐 − + 𝑲 𝒕𝟐 (𝒛𝟐 − 𝑯ෝ
ෝ 𝒕𝟐 = 𝒙
𝒙 𝒙 𝒕𝟐 − ሻ
 𝜎𝑥 (𝑡2 − ሻ 4
𝜎𝑓𝑢𝑠𝑒𝑑 2 𝜎𝑥 (𝑡2 − ሻ 2 𝑐
=( ሻ −
𝑐 𝑐 𝜎𝑥 𝑡2 − 2 2
( 𝑐 ሻ +𝜎 𝑧2

𝜎𝑥 2 𝑡2 −
( 𝑐 ሻ 𝜎𝑥 2 (𝑡2 − ሻ
2 2 −
𝜎𝑥 𝑡2 = 𝜎𝑥 (𝑡2 ሻ − = 𝜎𝑥 2 𝑡2 − − 𝐾 𝑡2 𝐻𝜎𝑥 2 𝑡2 −
𝜎𝑥 𝑡2 − 2 2 𝑐
( 𝑐 ሻ +𝜎 𝑧2

So in Kalman Notation 𝑷 𝒕𝟐 = 𝑷(𝒕𝟐 − ሻ − 𝑲 𝒕𝟐 𝑯𝑷(𝒕𝟐 − ሻ

𝐻𝜎𝑥 2 𝑡2 −
𝑲 𝒕𝟐 = 𝐻𝜎𝑥 2 𝑡2 − +𝜎2 𝑧2

𝑲 𝒕𝟐 = 𝑷 𝒕𝟐 − 𝑯𝑻 (𝑯𝑷 𝒕𝟐 − 𝑯𝑻 + 𝑹ሻ−𝟏

Where R is the uncertainty matrix (covariance matrix) associated with a noisy set of measurements.
 An Example
Initial Condition: p(0)=0, v(0)= -8, 𝛥t=0.1,acceleraton(a)=0.4 (all in SI)

Noise provided for estimation= 0.1*Gaussian(0,1)

Noise provided for measurement = Gaussian(0,4)

System dynamics known: System obeys Newton’s Laws

Measurement Accuracy = Very Poor

 Measurement
with heavy
noise. No
certainty in
With noise measurement

Without noise
Result from Kalman
Filter
 Advantages

• So the advantage of Kalman Filter is that, if we are damn sure about our system dynamics,

then even in the case of poor measurements, we will get good estimation.

• This is also true the other way around, i.e., if we are damn sure about our measurement

accuracy, then even in the case of our system dynamics being not known exactly, Kalman

Filter will give good estimates of system states.