function [x,P]=ukf(fstate,x,P,hmeas,y,Q,R) % UKF Unscented Kalman Filter for nonlinear dynamic systems % [x, P] = ukf(f,x,P,h,y,Q,R) returns state estimate, x and state covariance, P % for nonlinear dynamic system (for simplicity, noises are assumed as additive): % x_k+1 = f(x_k) + w_k % y_k = h(x_k) + v_k % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q % v ~ N(0,R) meaning v is gaussian noise with covariance R % Inputs: f: function handle for f(x) % x: "a priori" state estimate % P: "a priori" estimated state covariance % h: function handle for h(x) % y: current measurement % Q: process noise covariance % R: measurement noise covariance % Output: x: "a posteriori" state estimate % P: "a posteriori" state covariance % % Example: %{ n=3; %number of state q=0.1; %std of process r=0.1; %std of measurement Q=q^2*eye(n); % covariance of process R=r^2; % covariance of measurement f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations h=@(x)x(1); % measurement equation s=[0;0;1]; % initial state x=s+q*randn(3,1); %initial state % initial state with noise P = eye(n); % initial state covraiance N=20; % total dynamic steps xV = zeros(n,N); %estmate % allocate memory sV = zeros(n,N); %actual yV = zeros(1,N); for k=1:N y = h(s) + r*randn; % measurements sV(:,k)= s; % save actual state yV(k) = y; % save measurment [x, P] = ukf(f,x,P,h,y,Q,R); % ekf xV(:,k) = x; % save estimate s = f(s) + q*randn(3,1); % update process end for k=1:3 % plot results subplot(3,1,k) plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--') end %} % % Reference: Julier, SJ. and Uhlmann, J.K.. Unscented Filtering and % Nonlinear Estimation. Proceedings of the IEEE, Vol. 92, No. 3, % pp.401-422, 2004. % Henrique M. T. Menegaz, João Y. Ishihara, Geovany A. Borges, and % Alessandro N. Vargas. A Systematization of the Unscented Kalman Filter % Theory. IEEE TRANS. ON AUTOMATIC CONTROL, VOL. 60, NO. 10, OCTOBER 2015 % James B. Rawlings, David Q. Mayne, and Moritz M. Diehl. % Model Predictive Control: Theory, Computation, and Design. % Ebook provided online: https://sites.engineering.ucsb.edu/~jbraw/mpc/ % % Adapted from Yi Cao's work 04/01/2008 % https://de.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter % n=numel(x); %numer of states m=numel(y); %numer of measurements alpha=1; %default, tunable ki=0.1; %default, tunable beta=2; %default, tunable lambda=alpha^2*(n+ki)-n; %scaling factor c=n+lambda; %scaling factor W=[lambda/c 0.5/c+zeros(1,2*n)]; %weights c_sqrt=sqrt(c); X=sigmas(x,P,c_sqrt); %sigma points around x [x_pred,X1,P_pred,X2]=ut(fstate,X,W,n,Q); %unscented transformation of process [y_pred,Z1,P2,Z2]=ut(hmeas,X1,W,m,R); %unscented transformation of measurements PCT=X2*diag(W)*Z2'; %transformed cross-covariance Kf=PCT*inv(P2); x=x_pred+Kf*(y-y_pred); %state update P=P_pred-Kf*PCT'; %covariance update function [y,Y,P,Y1]=ut(f,X,W,n,R) %Unscented Transformation %Input: % f: nonlinear map % X: sigma points % W: weights % n: numer of outputs of f % R: additive covariance %Output: % y: transformed mean % Y: transformed sampling points % P: transformed covariance % Y1: transformed deviations L=size(X,2); % number of points y=zeros(n,1); Y=zeros(n,L); d=chol(R)*randn(n,L); % sample disturbances/noises for k=1:L Y(:,k)=f(X(:,k)) + d(:,k); % in MPC book of Rawlings, Mayne, and Diehl %Y(:,k)=f(X(:,k)); % in Julier and Ulhmann 2004 paper y=y+W(k)*Y(:,k); end Y1=Y-y(:,ones(1,L)); P=Y1*diag(W)*Y1'+R; eig(P) function Z=sigmas(x,P,c) %Sigma points around reference point %Inputs: % x: reference point % P: covariance % c: coefficient for 2n points %Output: % Z: Sigma points A = c*sqrtm(P); %A = c*chol(P)'; % using Cholesky factorization also works Y = x(:,ones(1,numel(x))); Z = [x Y+A Y-A];