ukf matlab,matlab UKF

function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)% UKF   Unscented Kalman Filter for nonlinear dynamic systems% [x, P] = ukf(f,x,P,h,z,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%           z_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: fanction handle for h(x)%           z: 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 stateq=0.1;    %std of processr=0.1;    %std of measurementQ=q^2*eye(n); % covariance of processR=r^2;        % covariance of measurementf=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equationsh=@(x)x(1);                               % measurement equations=[0;0;1];                                % initial statex=s+q*randn(3,1); %initial state          % initial state with noiseP = eye(n);                               % initial state covraianceN=20;                                     % total dynamic stepsxV = zeros(n,N);          %estmate        % allocate memorysV = zeros(n,N);          %actualzV = zeros(1,N);for k=1:Nz = h(s) + r*randn;                     % measurmentssV(:,k)= s;                             % save actual statezV(k)  = z;                             % save measurment[x, P] = ukf(f,x,P,h,z,Q,R);            % ekfxV(:,k) = x;                            % save estimates = f(s) + q*randn(3,1);                % update processendfor k=1:3                                 % plot resultssubplot(3,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')end%}%% By Yi Cao at Cranfield University, 04/01/2008%L=numel(x);                                 %numer of statesm=numel(z);                                 %numer of measurementsalpha=1e-3;                                 %default, tunableki=0;                                       %default, tunablebeta=2;                                     %default, tunablelambda=alpha^2*(L+ki)-L;                    %scaling factorc=L+lambda;                                 %scaling factorWm=[lambda/c 0.5/c+zeros(1,2*L)];           %weights for meansWc=Wm;Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariancec=sqrt(c);X=sigmas(x,P,c);                            %sigma points around x[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);          %unscented transformation of process% X1=sigmas(x1,P1,c);                         %sigma points around x1% X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurmentsP12=X2*diag(Wc)*Z2';                        %transformed cross-covarianceK=P12*inv(P2);x=x1+K*(z-z1);                              %state updateP=P1-K*P12';                                %covariance updatefunction [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)%Unscented Transformation%Input:%        f: nonlinear map%        X: sigma points%       Wm: weights for mean%       Wc: weights for covraiance%        n: numer of outputs of f%        R: additive covariance%Output:%        y: transformed mean%        Y: transformed smapling points%        P: transformed covariance%       Y1: transformed deviationsL=size(X,2);y=zeros(n,1);Y=zeros(n,L);for k=1:LY(:,k)=f(X(:,k));y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L));P=Y1*diag(Wc)*Y1'+R;function X=sigmas(x,P,c)%Sigma points around reference point%Inputs:%       x: reference point%       P: covariance%       c: coefficient%Output:%       X: Sigma pointsA = c*chol(P)';