Learning the Unscented Kalman Filter

An implementation of Unscented Kalman Filter for nonlinear state estimation.

Description

Nonlinear state estimation is a challenge problem. The well-known Kalman Filter is only suitable for linear systems. The Extended Kalman Filter (EKF) has become a standarded formulation for nonlinear state estimation. However, it may cause significant error for highly nonlinear systems because of the propagation of uncertainty through the nonlinear system.

The Unscented Kalman Filter (UKF) is a novel development in the field. The idea is to produce several sampling points (Sigma points) around the current state estimate based on its covariance. Then, propagating these points through the nonlinear map to get more accurate estimation of the mean and covariance of the mapping results. In this way, it avoids the need to calculate the Jacobian, hence incurs only the similar computation load as the EKF.

For tutorial purpose, this code implements a simplified version of UKF formulation, where we assume both the process and measurement noises are additive to avoid augment of state and also to simplify the assumption on nonlinear maps.

The code is heavily commented with an example to use the function. Hence, it is sutiable for beginners to learn the UKF. For comparison, the EKF code can be found from http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=18189&objectType=FILE

Acknowledgements

The author wishes to acknowledge the following in the creation of this submission:
Learning the Kalman Filter, Learning the Extended Kalman Filter
This submission has inspired the following:
Neural Network training using the Unscented Kalman Filter, Nonlinear least square optimization through parameter estimation using the Unscented Kalman Filter

• MATLAB release: MATLAB 7.4 (R2007a)

[edwin de Vries]

??? Maximum recursion limit of 500 reached. Use set(0,'RecursionLimit',N)
to change the limit. Be aware that exceeding your available stack space can
crash MATLAB and/or your computer.

Error in ==> ukf>create@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))] at 25
 f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations

[Yi Cao]

Dear Edwin,

This is not a recursive code. Hence, this error should not happen. I believe this is due to the way you run the example. When you selected the example and pressed control-t to uncomment the selection, you must have saved the change so that the ukf function is recursively called. The right way to run the example, after you uncomment the selection, you should not save the change, just right-click to run the selection. To help other users may come with the same error, I modified the example with block-comments. Now, you can select the example, right-click to run the selection without accidently saving the change.

Hope this help.

[Taher DERBEL]

 

[Hao Li]

where's the function "[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R)"

[Terry Zeng]

Hi,in your code. you write

'x=x1+K*(z-z1); %state update'

but the procedure calculating 'z1' has not been given.

[Yi Cao]

Hi, Hao Li
The function "[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R)" is the subfunction included in the file from Line 72 to Line 95.

Hi, Terry Zeng,

The line you mentioned is line 69. 'z1' is calculated in line 66:
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);
i.e. the line mentioned by Hao Li.

HTH.

[Yi Cao]

Dear Atilla Aydogdu,

Thank you for your comments. The augmented state variables are only applable if the process noise and measurement noise are non-additive, i.e. they are in the nonlinear functions:

x(k+1)=f(x(k),u(k),w(k))
y(k+1)=h(x(k+1),u(k+1),v(k+1))

In this case, we have to propagate w and v through the nonlinear functions, hence have to have extra augmented dimensions in state space to evaluate these nonlinear functions. That is why the state space dimention becomes 2L+1.

As I stated in the description of my UKF submission, for tutorial purpose, we only consider a simple case, i.e. the noises are additive, where the equarions are:

x(k+1)=f(x(k),u(k))+w(k)
y(k+1)=h(x(k),u(k))+v(k)

In this case, both w and v are not a part of these nonlinear functions, hence, do not need to propagate through these functions. Hence, we do not need the state space augmentation. We can prove, in this case, the non-augment state space formulation is equivalent to the augmented state space formulation.

The reason to assume additive noises is that normally, we do not know how exactly noises influence a system, hence do not realy know how to represent them in nonlinear functions. In this case, it is sensible to assume noises are additive. In the Julier's paper, since it is an academic article, certainly, it makes sense to discuss a more general case, that is to include noises within these nonlinear functions.

Conclusion: if we know how to represent noises in nonlinear functions, then use augmented formulation. Other wise, we can assume additive noises and use the simplified formulation without the state space augmentation.

hth.

[Loki Inc]

Hi; i tried your function with this, f=@(x)[-x(2);-exp(-a*x(1))*x(2)^2*x(3);0]; % nonlinear state equations
h=@(x)[sqrt(m^2+(x(1)-Z)^2)]; % measurement equation
s=[3*10^5 2*10^4 1*10^-3]';
but the state xV seems to do not evolve after the first step.
Any suggestion? Did i make something wrong?

[Yi Cao]

Loki

States is not evolved by the UKF. You should have another simulation model to evolve states, then send output of the model to UKF to estimate the states. If you send me you model through email I may be able to see what is you problem.

[kavin trivedi]

hey Yi cao,why do u use chol function instead of sqrt ,what is the advantage of doing so,pls clarify chol function u use.

[Yi Cao]

Hi, Kavin

Thanks for comments. chol is more efficient and robust than sqrtm.

[ben ouissem]

I'm student in france and i have seen your program about UKF (unscented kalman filter) in the page : http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=18217&objectType=file . I don't understand why the function UKF nead the covariance R and Q coz in the algorithm UKF we can find in the paper : http://mi.eng.cam.ac.uk/~cipolla/publications/inproceedings/2001-BMVC-Stenger-kalman.pdf (page 4) the UKF just need the covariance P and the state x.
Sorry for my english if it was difficult to understand my question.
Can you please help to understand the UKF.
Thank you

[Yi Cao]

There are a few different versions of UKF. I used unaugmented one, where Q and R have to be explicitly used. There are some augmented versions, where Q and R are included in P.

hth
Yi

[? ?]

????????????

[Adam Attarian]

Greetings,

While I understand it is no longer necessary to augment the states when you consider additive noise, it is also apparent that you then only have to use the first L weights, and not the 2L+1 weights. Can you comment on this? Is anything lost or gained by using L weights or 2L+1 weights in the additive noise case?

I ask only because I saw degraded performance when I switched from using all 2L+1 weights to using only the first L weights in my program (I am not augmenting the states).

Thanks,
Adam
NC State Univ

[fengtset tsai]

Hi, Dr. Cao,
Is the covariance update correct?

P=P1-K*P12'; %covariance update

Some paper wrote:

P=P1-K*P12*K';

fengtset

[fengtset tsai]

The same question as Loki,
if the measurement equation is nonlinear in state variable, the estimated state variable does not change with actural (simulated) state variable. Any suggestion?

[Ramya Gangishetty]

well i'm doing my research project and the topic is comparison of EKF and UKF in non-linear state estimation. The non-linear model which i have used gives correct results for EKF but i'm not able to get the correct results for UKF. I don't know what the problem is. I was wondering if you could look at my model and suggest a solution to it.

[Nan Luo]

Hi,Dr.Cao
The same question as Adam.

Thank you

[V. Poor]

 

[Dapat Chawah]

hello Dr.Yi
This code is working good for N<=150
but when N exceeds this limit, a nonsense happens
Is there any improvement to the code considering this error?

[Yi Cao]

It seems that your model is not stable. You may wish to adjust P, Q and R matrices to see if this helps.

Yi

[Erik Robers]

I think the covariance updat should be:

P=P1-K*P2*K'

Erik

[Yi Cao]

Right. However, K=P12*inv(P2). Hence, K*P2 = P12.
This leads to K*P2*K' = P12*K'. Therefore, P=P1-P12*K'.

HTH
Yi

[Peter Schenkel]

Hi Yi Cao,

First of all, thanks for your contribution here. I do have a question though, I do get for some parameter combinations a complex covariance matrix, the parameters look like this :

z = -78
c = 1.7321e-004
P =
   1.2500 + 0.0000i 0.0000 - 0.0000i 0.0000 - 0.0000i
   0.0000 + 0.0000i 0.4438 + 0.0000i 0.0000 + 0.0000i
   0.0000 + 0.0000i 0.0000 - 0.0000i 1.2500 + 0.0000i

x =
   0.5807 - 0.0000i
  -5.5018 + 3.7078i
   0.7954 - 0.0000i

Then I get this error :
??? Error using ==> chol
Matrix must be positive definite with real diagonal.

I assume that this is due to the complex covariance matrix.
I have no idea how this matrix can become complex as in my oppinion the only way it can become complex is if c would be negative which it isn't here...

Additionally, I would like to measure distances using radio signal strength, therefore I have actually the distances from RSSI values and additional velocity from the last step to the current step, is it possible to process these information with this implementation as well ?

Thanks for any help!

Best
 Peter

[Yi Cao]

Peter,

This means the iteration of ukf is unstable. You have to adjust P, Q, etc to make it stable.

Yi

[Peter Schenkel]

sorry...ekf should be ukf in the previous posting

[Peter Schenkel]

ok for some reasion my previous posting got lost , so once again :

I am using UKF to estimate distances from radio signal strength. Eventhough the RSSI error (measurement equation) is gauss distributed UKF performs very poorly and I cannot understand why as it seems the perfect choice for this kind of problem. I set the measurment nois to the std I got from the training data. My system equations are

f=@(x)[abs(x(1)+x(2));abs(x(3)-x(1));x(1)] ;
h=@(x)[-log10(x(1))*10*pl-A];

for f :
x1: new distance = old distance + velocity
x2: velocity = difference(old distance, new distance)
x3: old distance

h is simply a given transformation from distance to radio singal strength

I cannot find any reason for the poor performance as it should be the best filter for this kind of application. Am I missing some important issues ?

[Gauri Patil]

Hi,
Is it possible to use the UFK when the non-linear function 'f' is unknown. But instead there is a 'map' (non deterministic) which is known. I want to filter the measurement signal using this non-deterministic 'map' which is only a set of samples and is seen periodically in the measurement signal.

Thanks.

[tim Heights]

Hello Dr Yi Cao

are you planing to post square root UKF for parameter estimation?

[Guapa]

Hi Yi Cao,
i still can't turn the program, please can you tell me how may i do it, since the dowloading of the file to the right execution. matlab always returns errors

[holland]

Hi Dr Cao

why we can not use this UKF algorithm for parameter and state estimation both just like EKF algorithm. What needs to be done to play with this UKF algorithm for state-parameter estimation.
It seems this UKF algorithm is useless and much touted advantage over EKF is not true.

[Yi Cao]

Hi holland,

Yes, we can. Please check the following two FEX entries for details.

Yi

http://www.mathworks.com/matlabcentral/fileexchange/18356
http://www.mathworks.com/matlabcentral/fileexchange/18355

[Haijun Shen]

Hi Yi,

In propagating the process by

[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);

You include Q in the covariance P1, but the propagated states in matrix X1 does not include any process noise because you are assuming additive noise and your f function does not account for process noise. In turn, when you feed X1 into

[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);

to get the measurement matrix Z1, Z1 does not include the effect of any process noise. Therefore, when you use Z1 and z1 to calculate P2, even though you add R onto P2, P2 is not a true representation of Pyy. In your example, Pyy is off by Q.

In linear terms, your X1 consists of Ak*xkhat instead of Ak*xkhat+wk even though your P1 is Ak*Pkhat*Ak'+Qk. And your P2 is C_{k+1}*Ak*Pkhat*AK'*C_{k+1}' + R. The correct P2 should be C_{k+1}*(Ak*Pkhat*AK'+Q)*C_{k+1}' + R.

By the way, I think the augmented version is still applicable to cases with additive noises, although one may choose not to use it because of added complexity.

Looking forward to your opinion.

Thanks.

Haijun Shen

[Big Andy]

 

[Jordan Malof]

Dr. Cao,

I really appreciate your submission, it was a great help. I would only suggest listing a reference or two in your m-file, e.g. (R van der Merwe and EA Wan, 2002). I didn't know about the square-root implementation of the ukf and was, just at first, a bit confused about your implementation. Otherwise everything was very clear and helpfull.

Thanks again!
Jordan

[Yi Cao]

Hi Jordan,

Thanks for the comments. A reference has been added to the updated code. It should be available within a few days.

Yi

[Paul Mellor]

I have an input function also ("u"). How can this be added to the UKF code? Do i simply pass "u" through to the fstate() function via the ut() function?

[miftahuddin miftahuddin]

hi Dr.Cao

i
have some problem with my dynamic model. would you help me to apply my model in your "UKF".
could i get your email addres.

Best,

Miftahuddin
student
Sepuluh Nopember Institute of technology
Surabaya
Indonesia

[Yvonne]

I have implemented a highly non-linear aircraft tracking app with both an EKF and your UKF. The initial state and state error covariance matrices are the identical as are the observation and process errors. However, I get a decent result with the EKF, but NOT with your UKF... it should be the reverse... Any suggestion?

[Sourav]

Please send your comments to [e-mail]..as I am not very frequent visitor.Thanks again

[xiaogang]

what does the x(1),x(2),x(3) represent
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state,I don't konw how does the f function come out?

[Sally Thompson]

Take note of the point made by Haijun Shen if you are planning on using this filter as a basis for an augmented system (where the noise is part of the state vector). Not including the process noise in the function "f" will cause significant bias in the filter results if your noise is not additive or your state vector is augmented.

Otherwise, thanks so much for a great way to learn about unscented filtering!