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

 

[Ultimâ]

Here's a link describing the EKF : http://en.wikipedia.org/wiki/Kalman_filter#Extended_Kalman_filter

 

[enigma]

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]

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:


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:

http://aerospace.colorado.edu/frameset.php/facstaff/facultyHTMLfiles/born.html


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!

 

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

 

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

 

[SGT]

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?

 

[SGT]

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)$$ ?

 

[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$$

 

[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 ]$$
Your state equation is linear:
$$\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$$

 

[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 realize 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.

 

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

 

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

 

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

 

[SGT]

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.