Code covered by the BSD License

### Highlights fromLearning the Unscented Kalman Filter

4.13333

4.1 | 16 ratings Rate this file 234 Downloads (last 30 days) File Size: 2.5 KB File ID: #18217

# Learning the Unscented Kalman Filter

by Yi Cao

04 Jan 2008 (Updated 12 Dec 2010)

An implementation of Unscented Kalman Filter for nonlinear state estimation.

File Information
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

Learning The Kalman Filter and Learning The Extended Kalman Filter inspired this file.

MATLAB release MATLAB 7.4 (R2007a)
Tags for This File
Everyone's Tags
Tags I've Applied
24 Apr 2013

Thank you Prof. Yi Cao.

I have a problem at this point:
X=sigma(x,P,c) %sigma points around x

It keeps saying:

"Index exceeds matrix dimensions.
Error in sigma (line 97)
a=varargin{1}; b=varargin{2}; c=varargin{3}; d=varargin{4};"

X=sigma(x,P,c,[],1), it says:

"Error using sigma (line 107)
The A matrix must be square"

Please does anyone have a solution to this? I can't seem to get past this point.

Thank you.

25 Mar 2013

Dear Yi Cao,
I have a problem with the correlation matrix of the measurement. It becomes non-positive definite for some parameters.
How can I handle this problem?
I have already tried same matrix validations but they do not work.
I have to calibrate model's parameters with MLE.
Any comment is appreciated
Thanks
G.

13 Feb 2013
16 Nov 2012

Dear Prof. Yi Cao,
Thank you very much for the posted Matlab code. I tried to modify the process function of this code as I want.

for example:
s=[1;2;3];
f=@(x)[x(2);x(3);2*x(1)*(x(2)+x(3))];
h=@(x)x(1) ;
But when I run the program it shows an error of computing sigma points. It says that the matrix P shoud me positive definite. How we can modify this UKF program for any kind of defined function?

Thank you.

29 Jul 2012

Dear Yi Cao,
According to the paper'performance evaluation of UKF-based nonlinear filtering',choose:f=@(x)[x(1)+tao*x(2);x(2)-tao*x(1)+tao*(x(1)^2+x(2)^2-1)*x(2)];
h=@(x)x(1),with covariance of the process noise w(k)given as:Q=0.003^2I,the covaiance of the noise v(k) is given by:R=0.001^2I,initial state:x0=[2.3;2.2],P0=I,the true value of the initial state:x=[0.8;0.2]. The paper proof that when given all these,UKF tends to be divergent.However,based on this code,it seems that the estimator is stable.Does it owe to the weights chosen when doing the prediction?

07 May 2012

Dear Dr. Cao,
I am relatively new to Kalman filtering, and I am very happy to have found your Excellent, heavily commented UKF function and example “ beginners”:
http://www.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter
It seems that your nonlinear function “f” in this code - that you use as an example could be modified from
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
relatively easily to a nonlinear function that describes different nonlinear or time-varying features, like a battery’s state-of-charge.

I would most grateful if you could direct me to further literature, that might further guide me, e.g.,
• notes from your class that gives more background on your unscented Kalman filter example, above .
• how to generally select
 ki=0; %default, tunable
 beta=2; %default, tunable
 alpha=1e-3; %default, tunable
• Do state variables x(1:3) in your example above represent states of an actual physical process or is x used purely a as a numerical example ?
• I understand that your Matlab function UKF.m, describes a simplified unscented KF with added process noise and measurement noise:
% x_k+1 = f(x_k) + w_k
% z_k = h(x_k) + v_k
Do you have a more general unscented KF, UKF2.m, that that propagates the nonlinear noises, e.g.,:
% x_k+1 = f(x_k,w_k)
% z_k = h(x_k,v_k) ?

Thanks again for hour excellent work!
Scott

07 May 2012
24 Apr 2012

i need help. i have the above system, but i dont know how to fuse the equations into the filter. can anyone help me??

the system model has three states: X, Y, Th. the equations are:

Xk+1 = Xk + cos(Thk)*u*Dt
Yk+1 = Yk + sin(Thk)*u*Dt
Thk+1 = Thk + w*Dt

the measurement model is:

X1=Xk+1+r*cos(Thk+1 - 90); //r is a constant
Y1=Yk+1+r*sin(Thk+1 - 90);

X2=Xk+1+m*cos(Thk+1); //m is a constant
Y2=Yk+1+m*sin(Thk+1);

X3=Xk+r*cos(Thk+1 + 90);
Y3=Yk+r*sin(Thk+1 + 90);

H1=H2=H3=0;

P1=(-a*X1-b*Y1-c*H1-d) / (a*(XL-X1)+b*(YL-Y1)+c*(ZL-H1)); // a, b, c, d, constants
P2=(-a*X2-b*Y2-c*H2-d) / (a*(XL-X2)+b*(YL-Y2)+c*(ZL-H2));
P3=(-a*X3-b*Y3-c*H3-d) / (a*(XL-X3)+b*(YL-Y3)+c*(ZL-H3));

A1[0] = XL-X1; // XL, YL, ZL, constants
A1[1] = YL-Y1;
A1[2] = ZL-H1;

A2[0] = XL-X2;
A2[1] = YL-Y2;
A2[2] = ZL-H2;

A3[0] = XL-X3;
A3[1] = YL-Y3;
A3[2] = ZL-H3;

Z1[0] = A1[0]*P1 + X1;
Z1[1] = A1[1]*P1 + Y1;
Z1[2] = A1[2]*P1 + H1;

Z2[0] = A2[0]*P2 + X2;
Z2[1] = A2[1]*P2 + Y2;
Z2[2] = A2[2]*P2 + H2;

Z3[0] = A3[0]*P3 + X3;
Z3[1] = A3[1]*P3 + Y3;
Z3[2] = A3[2]*P3 + H3;

Z[0] = Z1[0] + Z3[0]; // Z[i], are the measurements needed for the UKF!!!
Z[1] = Z1[1] + Z3[1];
Z[2] = atan((Z2[1]-Z[1])/(Z2[0]-Z[0]));

please can you help me implement the UKF with these equations?

13 Feb 2012

Great works!

05 Jul 2011

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!

27 Feb 2011

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?

04 Feb 2011

04 Jan 2011

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?

28 Dec 2010

hi Dr.Cao

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

Best,

Miftahuddin
student
Sepuluh Nopember Institute of technology
Surabaya
Indonesia

23 Dec 2010

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?

12 Dec 2010

Hi Jordan,

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

Yi

11 Dec 2010

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

11 Aug 2010
08 Jul 2010

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.

Thanks.

Haijun Shen

09 Jun 2010

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

04 Jun 2010

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.

02 May 2010

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

19 Apr 2010

Hello Dr Yi Cao

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

15 Mar 2010

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.

23 Jun 2009

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 ?

23 Jun 2009

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

21 Jun 2009

Peter,

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

Yi

20 Jun 2009

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

06 Jun 2009

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

05 Jun 2009

I think the covariance updat should be:

P=P1-K*P2*K'

Erik

20 Apr 2009

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

Yi

20 Apr 2009

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?

05 Apr 2009
12 Mar 2009

Hi,Dr.Cao

Thank you

03 Dec 2008

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.

26 Nov 2008

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?

26 Nov 2008

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

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

Some paper wrote:

P=P1-K*P12*K';

fengtset

09 Nov 2008

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,
NC State Univ

31 Jul 2008

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

21 May 2008

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

21 May 2008

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.
Thank you

30 Apr 2008

Hi, Kavin

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

28 Apr 2008

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.

03 Apr 2008

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.

03 Apr 2008

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?

04 Feb 2008

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.

31 Jan 2008

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.

30 Jan 2008

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

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

30 Jan 2008

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

24 Jan 2008
23 Jan 2008

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.

23 Jan 2008

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

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

07 Jan 2008

a bug fix

07 Jan 2008

correct weights

23 Jan 2008

Update the example with block comments so that uses can run the example by selecting the example and right clicking to run the selection.

24 Jan 2008

update example

29 Mar 2010

update the code.

12 Dec 2010