Extended Kalman Filter (EKF)

sources:
1-http://users.isr.ist.utl.pt/~mir/pub/kalman.pdf
2-http://en.wikipedia.org/wiki/Extended_Kalman_filter

The Kalman filter dynamics results from the consecutive cycles of prediction
and filtering. The dynamics of these cycles is derived and interpreted in the framework
of Gaussian probability density functions. Under additional conditions on
the system dynamics, the Kalman filter dynamics converges to a steady-state filter
and the steady-state gain is derived.
The nonlinear version of the Kalman filter which linearizes about the current mean and covariance is the Extended Kalman Filter.With no loss of generality we will consider that
the system has no external inputs.


One iteration of the EKF is composed by the following consecutive steps:

1. Consider the last filtered state estimate ˆx(k|k),
2. Linearize the system dynamics, xk+1 = f(xk) + wk around ˆx(k|k),
3. Apply the prediction step of the Kalman filter to the linearized system dynamics
just obtained, yielding ˆx(k + 1|k) and P(k + 1|k),
4. Linearize the observation dynamics, yk = h(xk) + vk around ˆx(k + 1|k),
5. Apply the filtering or update cycle of the Kalman filter to the linearized
observation dynamics, yielding ˆx(k + 1|k + 1) and P(k + 1|k + 1).

Let F(k) and H(k) be the Jacobian matrices of f(.) and h(.), denoted by
F(k) = 5fk |ˆx(k|k)
H(k + 1) = 5h |ˆx(k+1|k)

The Extended Kalman filter algorithm is stated below:

Predict Cycle

ˆx(k + 1|k) = fk(ˆx(k|k))
P(k + 1|k) = F(k)P(k|k)FT (k) + Q(k)

Filtered Cycle
//not formatted
x(k + 1|k + 1) = ˆx(k + 1|k) + K(k + 1)[yk+1 − hk+1(ˆx(k + 1|k))]
K(k + 1) = P(k + 1|k)HT (k + 1)[H(k + 1)P(k + 1|k)HT (k + 1) + R(k + 1)]−1
P(k + 1|k + 1) = [I − K(k + 1)H(k + 1)]P(k + 1|k)

It this important to state that the EKF is not an optimal filter, but rathar it is
implemented based on a set of approximations. Thus, the matrices P(k|k) and
P(k + 1|k) do not represent the true covariance of the state estimates.
Moreover, as the matrices F(k) and H(k) depend on previous state estimates
and therefore on measurements, the filter gain K(k) and the matrices P(k|k) and
P(k + 1|k) cannot be computed off-line as occurs in the Kalman filter.
Contrary to the Kalman filter, the EKF may diverge, if the consecutive linearizations
are not a good approximation of the linear model in all the associated
uncertainty domain. [1]