Understanding the Extended Kalman Filter

1. Jun 27, 2005

Ultimâ

Hi there,

I'm wasn't sure whether I should post this question in the maths section or here, but since this technique is used a lot in aerospace I thought I'd try here first.

I've grasped the idea of an ordinary Kalman Filter and created a program that tracks a noisy signal. However when it comes to modifying the program to an EKF I'm at a total loss. I've spent weeks reading up on this and searching for examples, but I'm still unable to figure how to calculate the modified equations (especially the Jacobian terms). So I have decided to ask if anyone on these forums have any expertise in this area and could possibly help me out.

Can anyone suggest how I calculate the Jacobians A_k (sometimes described as F_k) and H_k for a simple case of tracking an x-coordinate over time? I'm assuming the noise has no gain.

Thanks,
Charles

2. Jul 5, 2005

3. Jul 5, 2005

enigma

Staff Emeritus
Ultima, I'm going to move this thread to General Math in the hopes you get a response.

I have seen the Kalman filter used, but I certainly don't have a good enough grasp on it to explain the nuts and bolts.

4. Jul 6, 2005

enigma

Staff Emeritus
Eh.... maybe Celestial Mechanics.

Sorry for the lack of response, Ultima. It's a tricky subject.

5. Jul 6, 2005

goavs4

Hey Ultima,

I took some graduate level courses in Statistical Orbit Determination in college unfortunately we only worked with generic Kalman filtering (and that was over 4 years ago :P ). Extended Kalman filtering was covered in another course I never had the fortune of taking. However, my former professor, George Born at the University of Colorado Boulder, co-authored a book "Statistical Orbit Determination" which covers some applications of EKF and he also wrote a paper on variations a while back:

https://www.amazon.com/exec/obidos/tg/detail/-/0126836302/
http://www.space-flight.org/AAS_meetings/1996_winter/abstracts/96-108.html

So in short while I can't help you directly perhaps you could try contacting Prof. Born via e-mail:

He might be willing to answer your questions directly or perhaps provide you with a place to look for examples of the EKF implemented by others. Papers or perhaps other books or programs written which would be accessible online.

Hope that helps!

Last edited by a moderator: May 2, 2017
6. Jul 7, 2005

Ultimâ

I was beginning to lose hope.

Enigma, thanks for your help in finding the best place for this thread.

Goavs4, I've now put in a request with the British Library and should have the book in a week or so. I may send Prof. Born an email and see if he can suggest any good references (don't like to inconvenience people though).

From all that I've read it would appear that the step from the Kalman filter to the EKF is fairly straightforward...I think I must be just missing some fundamental issue that might be rectified through a good example, but sadly, simple examples of the EKF seem a bit like the needle in the haystack senario.

Thanks for you input Goavs4 =)

7. Jul 8, 2005

SGT

Consider the process:
$$x_{k+1} = f(x_{k},k) + w_k$$
with noisy measurements:
$$z_k = h(x_{k},k) + v_k$$
Where f and h are nonlinear functions of the state x and $$w_k$$ and $$v_k$$ are gaussian noises.
If we know an unbiased estimation $$\hat x_{k|k}$$ of the state at the instant k we can develop $$f(x_{k},k)$$ in a Taylor series around $$\hat x_{k|k}$$.
$$x_{k+1} = f(\hat x_{k|k},k) + f_x(\hat x_{k|k},k)e_{k|k} + \frac{1}{2}f_{xx}(\hat x_{k|k},k)e_{k|k} (x) e_{k|k} + ...$$
Where:
$$e_{k|j} = x_k - \hat x_{k|j}$$
$$f_x = \frac {\partial f}{\partial x^T}$$
$$f_{xx} = \frac {\partial ^2 f}{\partial x^T \partial x^T}$$
and (x) is the Kronecker matrix product.
If the errors are small, we can add the terms of second and higher orders to the noise and write:
$$x_{k+1} = f(\hat x_{k|k},k) + f_x(\hat x_{k|k},k)e_{k|k} + w'_k$$
Before the arrival of the measurement $$z_{k+1}$$ the best estimate of $$x_{k+1}$$ is its mathematical expectation:
$$\hat x_{k+1|k} = E[x_{k+1}] = f(\hat x_{k|k},k)$$
We can now develop $$h(x_k,k)$$ in series around the new estimate:
$$z_{k+1} = h(\hat x_{k+1|k},k+1) + h_x(\hat x_{k+1|k+1},k)e_{k+1|k} + \frac{1}{2}h_{xx}(\hat x_{k+1|k},k+1)e_{k+1|k} (x) e_{k+1|k} + ...$$
again, supposing $$e_{k+1|k}$$ is small we can write:
$$z_{k+1} = h(\hat x_{k+1|k},k) + h_x(\hat x_{k+1|k},k)e_{k+1|k} + v'_{k+1}$$
After the arrival of the measurement $$z_{k+1}$$ the best estimate of $$x_{k+1}$$ is:
$$\hat x_{k+1|k+1} = \hat x_{k+1|k} + K_{k+1}[z_{k+1} - h(\hat x_{k+1|k},k+1)]$$
Where
$$K_{k+1} = P_{k+1|k}.h_x^T[h_x.P_{k+1|k}.h_x^T + R_{k+1}]^{-1}$$
$$P_{k+1|k} = E[e_{k+1|k} . e^T_{k+1|k} = f_x.P_{k|k}.f_x^T + Q_k$$
The new error covariance matrix is:
$$P_{k+1|k+1} = E[e_{k+1|k+1} . e^T_{k+1|k+1} = [I - K_{k+1}.h_x].P_{k+1|k}.[I - K_{k+1}.h_x]^T + K_{k+1}.R_{k+1}.K^T_{k+1}$$
Where $$Q_k$$ and $$R_k$$ are the covariances of the process and measurement noises respectively.

8. Jul 8, 2005

SGT

I would put it in Electrical Engineering or in Engineering Systems and Design.

9. Jul 11, 2005

Ultimâ

Thanks SGT for that derivation. I was also amazed to learn that this forum supports Latex, which will help me express myself far better now and in the future. It really does put a smile on my face when I find I've had a reply.

Anyway, specifically I'm asking for help with calculating

$$f_x$$, $$h_x$$ and $$h(\hat{x}_{k+1|k}, k+1)$$ (from 2 posts above for any new comers).

So if we are tracking a noisy sinusoid (for example), let's say the first part of the data stream for for the observation of the input ($$z$$) is:

0.044, 0.088, 0.143, 0.188, 0.261, 0.355, 0.352, 0.287

and the corresponding estimate of the input ($$\hat{x}$$) is:

0.004, 0.017, 0.043, 0.077, 0.123, 0.184, 0.229, 0.244

how would the three above terms be calculated? Are the first two simply looking at the rate of change of $$z$$ and $$\hat{x}$$ and the last term ($$h(\hat{x}_{k+1|k}, k+1)$$) simply equal to $$\hat{x}$$ for this example?

Last edited: Jul 11, 2005
10. Jul 11, 2005

SGT

Could you elaborate a little more? What is your state vector $$x$$? Are its components position and velocity? And what are your observations $$z$$? Is $$z$$ the measurement of the position?
What are the functions $$f(x_k,k)$$ and $$h(x_k,k)$$ ?

Last edited by a moderator: Jul 11, 2005
11. Jul 11, 2005

Ultimâ

Right, we're just dealing with an x-position with respect to k, $$x$$ and $$z$$ can be treated as scalers. So for the first k, the observation is 0.004, $$z=h(x_k,k)+v_k=Hx_k$$+ $$v_k$$, in this case the observation is of the state directly, so H is unity) and the state is $$x_{k+1}+w_k=f(x_k,k)=Ax_k+w_k$$

Last edited: Jul 11, 2005
12. Jul 11, 2005

SGT

For such a simple system you don't need an EKF. A linear Kalman Filter will do. Your state is simply:
$$\vec x = \left [ \begin{array}{cc} x \\ \dot x \end{array} \right ]$$
$$\vec x_{k+1} = A.\vec x_k + w_k$$
where
$$A = \left [ \begin{array}{cc} 1 & T \\ 0 & 1 \end{array} \right ]$$
and T is the time interval between observations.
For the initial position $$x_0$$ you take the first observation $$z_0$$ and for the initial velocity you take the difference of the first two measurements divided by T.
$$\vec x_0 = \left [ \begin{array}{cc} 0.044 \\ \frac {0.044}{T} \end{array} \right ]$$
For the initial value of the error covariance matrix you can use :
$$P_{0|0} = \left [ \begin{array}{cc} 100\sigma_x^2 & 0 \\ 0 & 100\sigma_v^2 \end{array} \right ]$$
The matrix R is simply the scalar $$\sigma_z^2$$

Last edited by a moderator: Jul 12, 2005
13. Jul 12, 2005

Ultimâ

Although, I'll probably find this bit of information (working with vectors in KF) beneficial in the long term, in my first post I mention that I've implemented a scaler version of the Kalman Filter that works fine (In my model I assume $$x_0$$=0 and $$P_0$$=1). The idea was to amend this so it acted as an extended kalman filter. I realise that the KF is better for such a simple system, but my aim is to understand the EKF. When I tried to apply the EKF, I found I was unsure how to calculate the three variables mentioned in post #9.

14. Jul 12, 2005

SGT

You cannot use an EKF with a linear system. And a scalar linear Kalman filter is inefficient. Since the KF can provide you with velocity information, why not use it? The abandon of this information will provide you with a smoothed estimation. You said your data belonged to a sinusoid. If you don't use the velocity information you will end with the mean value of the sinusoid: zero.

15. Jul 12, 2005

Ultimâ

My KF model comes from the example in the following paper
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

I then changed the input signal from a constant voltage to a sinewave. The programmed filter tracks it reasonably well provided R and Q are choosen wisely.

Velocity isn't used in the example so I didn't incorporate it in my initial program model.

My understanding of the EKF is that, yes it is designed to handle non-linear systems, but this is through linearising the model at its current position, which suggests that it can be applied to linear models, but will not be as accurate as the normal kf.

So we are effectively tracking the position, with only the observation of a single x-coordinate available to us. I'd expect the ekf should produce better results for such a situation...

Last edited: Jul 12, 2005
16. Jul 12, 2005

SGT

The example from the paper deals with the estimation of a constant, so there is no velocity involved. If you want to estimate the position of a mobile you must either know its velocity or to estimate it through the filter.
You cannot use an EKF with a linear system, because the functions $$f$$ and $$h$$ are linear functions of the state $$x$$, so their derivatives $$f_x$$ and $$h_x$$ are simply constants.

Last edited by a moderator: Jul 12, 2005
17. Jul 13, 2005

Ultimâ

This makes sense, $$f_x$$ and $$h_x$$ seemed to become zero when I tried to calculate them and may be the fundamental reason why I am having difficulty understanding the EKF. So what would be the simplest non-linear model to apply an EKF too?

18. Jul 13, 2005

SGT

Suppose you have a stationary point in a plane, with coordinates $$x_0$$ and $$y_0$$. You measure the distance and the azimuth from the origin to the point:
$$d = \sqrt{x_0^2 + y_0^2} + \nu_d$$
$$az = tan^{-1}\frac{y}{x} + \nu_{az}$$
Now your $$h$$ is a nonlinear function of the state and you can calculate $$h_x$$.
The function $$f$$ is still linear, but if one of the two functions is nonlinear you must use an EKF.