in this video we'll discuss the set of
equations that you need to implement the
common filter algorithm let's revisit
the example that we introduced in the
previous video you join a competition to
win the big prize you're asked to design
a self-driving car that needs to drive
one kilometer on hundred different
terrains in each trial the car must stop
as close as possible to the finish line
at the end of the competition the every
trial position is competed for each team
and the owner of the car with the
smallest error variance and an average
final position closes to one kilometer
gets the big prize in that example we
also shot the car dynamics and the car
model for our single state system and we
discussed process and measurement noises
along with their covariances finally we
said that you could win the competition
by using a Kalman filter which computes
an optimal unbiased estimate of the cars
position with minimum variance this
optimal estimate is found by multiplying
the prediction and measurement
probability functions together scaling
the result and computing the mean of the
resulting probability density function
computationally the multiplication of
these two probability density functions
relates to the discrete Kalman filter
equation shown here does this ring a
bell doesn't it look similar to the
state observer equation that we
discussed in previous videos actually a
caramel filter is a type of saito
observer but it is designed for
stochastic systems here is how the
common filter equation relates to what
we'll discuss with the probability
density functions the first part
predicts the current state by using
state estimate from the previous time
step and the current input note that
these two state estimates are different
from each other will show the predicted
state estimate with this notation this
is also called the a priori estimate
since it is calculated before the
current measurement is taken we can now
rewrite the equation like this the
second part of the equation uses the
measurement and incorporates it into the
prediction to update the prior estimate
and we'll call the result the a
posteriori estimate you wanna win the
big prize right then these are the
equations you need to run on your car's
ECU looks a little scary what if we turn
everything upside now doesn't change
much does it okay we'll go where the
algorithm equations step by step the
Kalman filter is a two-step process
let's first start with the prediction
part here the system model is used to
calculate the a priori state estimate
and the error covariance P for our
single state system pizzas the variance
of the prior estimate and it can be
thought of as a measure of uncertainty
in the estimated state this variance
comes from the process noise and
propagation of the uncertain X hat K
minus 1 at the very start of the
algorithm the K minus 1 values for X hat
and P come from their initial estimates
the second step of the algorithm uses
their prior estimates calculated in the
prediction step and updates them to find
our posterior estimates of the state and
error covariance the Kalman gain is
calculated such that it minimizes the
posterior error covariance let this bar
represent the calculation of X hat K by
weighing the correction term the Kalman
gain determines how heavily the
measurement and the prior estimate
contributes to the calculation of X hat
K if the measurement noise is small
the measurement is trusted more and
contributes to the calculation of xhk
more than the prior state estimate das
in the opposite case word error in the
prior estimate is small the prior
estimate is trusted more and the
computation of accepts K mostly comes
from this estimate we can also show this
mathematically by looking at two extreme
cases assume that in the first case the
measurement covariance is 0 to calculate
the Kalman gain we take its limit as R
goes to 0 we plug in 0 for R and see
that these two terms cancel each other
out as R goes to 0 the combine gain
approaches to the inverse of C which is
equal to 1 in our system
plugging K and immerse of C into the
posterior state estimate shows that exit
K is equal to YK so the calculation
comes from the measurement only as
expected now if we update our plot we
can show the measurement with an impulse
function which is shown with this orange
vertical line note that the variance in
the measurement is 0 since R goes to 0
we found that the posterior estimate is
equal to the measurement so we can show
it by the same impulse function on the
other hand if the a priori error
covariance is close to zero then the
current gain is firm as 0 therefore the
contribution of this term to accept K is
ignored and the computation of exits K
comes from the prior state estimates on
the plot will show that prior state
estimate with an impulse function which
has zero variance and since the a
posterior estimate is equal to the prior
estimate will show it with the same
impulse function
once we've calculated the update
equations in the next type step the
upholsterer estimates are used to
predict the newer prior estimates and
the algorithm repeats itself notice that
to estimate the current state the
algorithm doesn't need all the past
information it only means the estimated
state and error covariance matrix from
the previous time step and the current
measurement this is what makes the
common filter recursive now you know the
set of equations needed to implement the
Kalman filter algorithm what are you
going to do with the big price when you
win the competition if you can't decide
here is a suggestion
note that the common filter is also
referred as a sensor fusion algorithm so
you can buy an additional sensor such as
an IMU an experiment to see whether
using multiple sensors will improve your
self driving cars estimated position if
you have two measurements the dimensions
of Y C and K matrices will change as
shown here but basically we will still
follow the same logic to compute the
optimal state estimate on the plot we'll
have one more probability density
function for the measurement from IMU
and this time we'll be multiplying three
probability density functions together
to find the optimal estimate of the cars
position so far we had a linear system
but what if you have a nonlinear system
and want to use Kalman filter in the
next video we'll discuss nonlinear state
estimators