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.

[Atilla Aydogdu]

1) Your UCF algorithm is wrong. You no where mention the augmented state variables as derived in the article of 'Julier and Uhlmann (2004), Unscented Filtering and Nonlinear Estimation' Equation 22.

2) The number of weights is incorrect in your algorithm. In your example you have 3 state variables, which give you 3 dimensions. And if we follow the literature(not only the one I mentioned under comment 1) ), this must give us 2*L+1, which is 2*3 + 1 = 7 weights and sigmapoints.

Please clearify your intuition behind your method.

Greets,

Atilla Aydogdu
Quantitative Econometrics
Erasmus University Rotterdam
The Netherlands

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