# Understanding the Extended Kalman Filter

#### 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

Related Astronomy and Astrophysics News on Phys.org

#### enigma

Staff Emeritus
Gold Member
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.

#### enigma

Staff Emeritus
Gold Member
Eh.... maybe Celestial Mechanics.

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

#### 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:

#### 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 =)

S

#### 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.

S

#### SGT

enigma said:
Eh.... maybe Celestial Mechanics.

Sorry for the lack of response, Ultima. It's a tricky subject.
I would put it in Electrical Engineering or in Engineering Systems and Design.

#### 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:
S

#### SGT

Ultimâ said:
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)$$.

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?
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:

#### 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:
S

#### 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:

#### 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.

S

#### 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.

#### 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:
S

#### 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:

#### 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?

S

#### SGT

Ultimâ said:
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?
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.

### Physics Forums Values

We Value Quality
• Topics based on mainstream science
• Proper English grammar and spelling
We Value Civility
• Positive and compassionate attitudes
• Patience while debating
We Value Productivity
• Disciplined to remain on-topic
• Recognition of own weaknesses
• Solo and co-op problem solving