An implementation of Unscented Kalman Filter for nonlinear state estimation.
Nonlinear state estimation is a challenge problem. The wellknown 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
1.2  Add a reference. 

1.1  update the code. 

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

correct weights 

a bug fix 
Dean Carhoun (view profile)
I'm looking for a lucid (understandable) implementation of a quaternionbased UKF for 6 DOF gyro/accelerometer Attitude estimation (pitch and roll). Any advice?
olbond (view profile)
Raola ra (view profile)
Mattias Wallin (view profile)
Have got the code to run, howerver something I don't really understand is in the computation of the sigma points. In all literature I've seen (for example the main documentation cited in the code, from Julier and Uhlmann) you are supposed to calculate the sigma points with the help of the square root of (n+kappa)*Pxx.
In this code it is only done as the square root of the constant, not the covariance matrix? Am I missing something?
Zhiwen Chen (view profile)
Mark Spiller (view profile)
Estimating the frequency of a cosine works for me. Perhaps try to set the initialization value close to the real value.
Nathan Zimmerman (view profile)
Unless i'm missing something, this code does not work for parameter estimation?
My state transition is simply the identity matrix while my my measurement matrix would be nonlinear.
Trying to identify a cos wave of unknown frequency & amplitude. This is easy with EKF but I can't figure out how w/ the UKF.
xue lee (view profile)
Cu Lo (view profile)
asaf eilam (view profile)
hi there,
I'm new with kalman filtering and need your help. I'm using the ukf script for solving the next problem: my measurement record the next function h(t) = x_{3}^2/((X_{1}(t)X0)^2+X_{2}^2).X_{1,2,3}are my unknowns and X0 in known. my state is [X_{1} X_{2} X_{3}]. X_{1} is periodic signal and X_{2} and X_{3} are parameters that are not change but should be estimated. the algorithm is not even close to give a result! can someone help with this.
Yang Luo (view profile)
it seems good
Fabian Gomez Barrero (view profile)
I'm trying to run the code and I'm having the following error
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.5*x(1)*(x(2)+x(3)))]
Not very good with MATLAB here, how can I fix this! Thanks!
iyad raf3 (view profile)
wang qq (view profile)
Jeremy (view profile)
Matlab's rounding errors were causing my covariance matrix (P) to become non positive definite. However, I have found a solution.
The function nearestSPD was able to correct for errors in P very nicely.
http://www.mathworks.com/matlabcentral/fileexchange/42885nearestspd
Only thing to watch out for is that nearestSPD can convert any matrix into a symmetric positive definite matrix, not just matrices that are close.
Hope this helps others with the same problem!
Hesam Khazraj (view profile)
Hi after ruining example its shows me this error :
Error: File: ukf.m Line: 21 Column: 2
The expression to the left of the equals sign is not a valid target for an assignment.
would you please tell me whats wrong ?
alex xue (view profile)
It's great help for beginners in researching!
zhanghao zhanghao (view profile)
good
Anik Islam (view profile)
Anik Islam (view profile)
I recently came across this code on the unscented Kalman filter (and it's great!) but I'm wondering if it can work when the state and observation variables are complex valued.
I was under the impression that the only difference between the UKF and the Complex UKF (CUKF) is to replace transpose operations with conjugate/hermitian transpose. However, when I attempt to use this code for a complex valued system, the cholesky factorization fails.
More details regarding my application are here:
https://www.reddit.com/r/ECE/comments/3cmrlo/anyone_familiar_with_unscented_kalman_filtering/
Any advice is appreciated. Thanks!
Mark (view profile)
@ Matthew (Jun 28)
I had the same problem (with P growing exponentially). Like you said: this has to do with the Alpha parameter. It has to do with how the Unscented Transform calculates its transformed mean. In certain cases (I think when measurement covariance is very low, and process covariance is a few orders of magnitude greater), there can be some rounding errors in Matlab, which causing the transformed mean to come up short.
To fix this, I changed the UT function to be like this:
~~~~~~~~~~
function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R)
L=size(X,2);
% y=zeros(n,1); % LINE COMMENTED OUT HERE
Y=zeros(n,L);
for k=1:L
Y(:,k)=f(X(:,k));
% y=y+Wm(k)*Y(:,k); % LINE COMMENTED OUT HERE
end
y = mean([Y(:,1)'; mean(Y(:,2:end)')]); % LINE ADDED HERE
Y1=Yy(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;
Matthew (view profile)
I found that for my system, the covariance matrix was growing like crazy (P_k~10^8*P_k1) and was getting complex.
Try adjusting the alpha parameter. 10e3 was way too small for my system, and 0.1 was the bare minimum that I could avoid the covariance issues. 0.15 seemed to work best.
in general, alpha is recommended to be between 10e3 and 1.
Hien (view profile)
Hi everybody!
I really have not understood this code yet. In my case, I also study on EKF for GPS data that I want to apply EKF to due with noise and missing data in GPS data. I have one GPS data columm with more than 2000 of length. Who could show me how to do it?
Thank you so much for your kinds
Jan (view profile)
Very good example!
Ghulam Rasool (view profile)
Ghulam Rasool (view profile)
Emmanuel Farhi (view profile)
This contribution can be used as an optimizer but is not very efficient then.
Chinedum (view profile)
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};"
when i run this instead:
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.
GMark (view profile)
Dear Yi Cao,
I have a problem with the correlation matrix of the measurement. It becomes nonpositive 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.
zhang blue (view profile)
thara (view profile)
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.
xiao (view profile)
Dear Yi Cao,
According to the paper'performance evaluation of UKFbased nonlinear filtering',choose:f=@(x)[x(1)+tao*x(2);x(2)tao*x(1)+tao*(x(1)^2+x(2)^21)*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?
Scott Levinson (view profile)
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/18217learningtheunscentedkalmanfilter
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 timevarying features, like a battery’s stateofcharge.
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=1e3; %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
Scott Levinson (view profile)
grge (view profile)
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*X1b*Y1c*H1d) / (a*(XLX1)+b*(YLY1)+c*(ZLH1)); // a, b, c, d, constants
P2=(a*X2b*Y2c*H2d) / (a*(XLX2)+b*(YLY2)+c*(ZLH2));
P3=(a*X3b*Y3c*H3d) / (a*(XLX3)+b*(YLY3)+c*(ZLH3));
A1[0] = XLX1; // XL, YL, ZL, constants
A1[1] = YLY1;
A1[2] = ZLH1;
A2[0] = XLX2;
A2[1] = YLY2;
A2[2] = ZLH2;
A3[0] = XLX3;
A3[1] = YLY3;
A3[2] = ZLH3;
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?
Xile Kang (view profile)
Great works!
Sally Thompson (view profile)
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!
xiaogang (view profile)
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?
Sourav (view profile)
Please send your comments to majordavuramus@gmail.com..as I am not very frequent visitor.Thanks again
Yvonne (view profile)
I have implemented a highly nonlinear 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?
miftahuddin miftahuddin (view profile)
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
Paul Mellor (view profile)
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?
Yi Cao (view profile)
Hi Jordan,
Thanks for the comments. A reference has been added to the updated code. It should be available within a few days.
Yi
Jordan Malof (view profile)
Dr. Cao,
I really appreciate your submission, it was a great help. I would only suggest listing a reference or two in your mfile, e.g. (R van der Merwe and EA Wan, 2002). I didn't know about the squareroot 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
Big Andy (view profile)
Haijun Shen (view profile)
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
Yi Cao (view profile)
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
holland (view profile)
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 stateparameter estimation.
It seems this UKF algorithm is useless and much touted advantage over EKF is not true.
Guapa (view profile)
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
tim Heights (view profile)
Hello Dr Yi Cao
are you planing to post square root UKF for parameter estimation?
Gauri Patil (view profile)
Hi,
Is it possible to use the UFK when the nonlinear function 'f' is unknown. But instead there is a 'map' (non deterministic) which is known. I want to filter the measurement signal using this nondeterministic 'map' which is only a set of samples and is seen periodically in the measurement signal.
Thanks.
Peter Schenkel (view profile)
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*plA];
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 ?
Peter Schenkel (view profile)
sorry...ekf should be ukf in the previous posting
Yi Cao (view profile)
Peter,
This means the iteration of ukf is unstable. You have to adjust P, Q, etc to make it stable.
Yi
Peter Schenkel (view profile)
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.7321e004
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 (view profile)
Right. However, K=P12*inv(P2). Hence, K*P2 = P12.
This leads to K*P2*K' = P12*K'. Therefore, P=P1P12*K'.
HTH
Yi
Erik Robers (view profile)
I think the covariance updat should be:
P=P1K*P2*K'
Erik
Yi Cao (view profile)
It seems that your model is not stable. You may wish to adjust P, Q and R matrices to see if this helps.
Yi
Dapat Chawah (view profile)
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?
V. Poor (view profile)
Nan Luo (view profile)
Hi,Dr.Cao
The same question as Adam.
Thank you
Ramya Gangishetty (view profile)
well i'm doing my research project and the topic is comparison of EKF and UKF in nonlinear state estimation. The nonlinear 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.
fengtset tsai (view profile)
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?
fengtset tsai (view profile)
Hi, Dr. Cao,
Is the covariance update correct?
P=P1K*P12'; %covariance update
Some paper wrote:
P=P1K*P12*K';
fengtset
Adam Attarian (view profile)
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
????????????
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
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/2001BMVCStengerkalman.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
Hi, Kavin
Thanks for comments. chol is more efficient and robust than sqrtm.
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.
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.
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?
Dear Atilla Aydogdu,
Thank you for your comments. The augmented state variables are only applable if the process noise and measurement noise are nonadditive, 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 nonaugment 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.
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.
Hi,in your code. you write
'x=x1+K*(zz1); %state update'
but the procedure calculating 'z1' has not been given.
where's the function "[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R)"
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 controlt 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 rightclick to run the selection. To help other users may come with the same error, I modified the example with blockcomments. Now, you can select the example, rightclick to run the selection without accidently saving the change.
Hope this help.
??? 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