unscented kalman filter in matlab

matlab, kalman filtering, unscented transform, state estimation, time series analysis

%Sample code for implementing Unscented Kalman Filter in Matlab

%Initialization x0=[0; 0]; %initial state vector P0=[0.1 0; 0 0.1]; %initial state covariance matrix Q=[0.1 0;0 0.1]; %process noise covariance R=0.2; %measurement noise covariance y=[1.2 0.95 0.86 0.66 0.57 0.42 0.35 0.2 0.14 0.1]; %measurement vector N=length(y);

%Step 1 - Calculate Sigma points alpha=0.3; %scaling parameter for sigma points distribution k=2; %secondary scaling parameter for sigma points distribution n=length(x0); Lambda=(alpha^2)*(n+k)-n; sigmaPoints=[x0 sqrt((n+Lambda)*P0) -sqrt((n+Lambda)*P0)];

%Step 2 - Predict Sigma points through Dynamic model sigmaPointsPred=zeros(size(sigmaPoints)); dT=1; %sampling time for i=1:size(sigmaPoints,2) sigmaPointsPred(:,i)=dynamicModel(sigmaPoints(:,i), dT); end

%Step 3 - Calculate mean and covariance of Predicted Sigma Points [meanPred,covPred]=meanCovariance(sigmaPointsPred,alpha,n,Lambda);

%Step 4 - Calculate Sigma points from Predicted Mean and Covariance sigmaPoints=[meanPred sqrt((n+Lambda)*covPred) -sqrt((n+Lambda)*covPred)];

%Step 5 - Update Sigma points through Measurement model sigmaPointsUpdate=zeros(size(sigmaPoints)); for i=1:size(sigmaPoints,2) sigmaPointsUpdate(:,i)=measurementModel(sigmaPoints(:,i)); end

%Step 6 - Calculate mean and covariance of Updated Sigma Points [meanUpdate,covUpdate]=meanCovariance(sigmaPointsUpdate,alpha,n,Lambda);

%Step 7 - Calculate Kalman Gain and state estimation K=covPredinv(covPred+covR); xEst=meanPred+K(y-meanUpdate);

%Step 8 - Update Covariance of State P=covPred-KcovUpdateK';

function xNext=dynamicModel(x, dT) %Dynamic model xNext=[x(1)+x(2)*dT; x(2)]; end

function [meanVal,covVal]=meanCovariance(sigmaPoints,alpha,n,Lambda) %Calculate mean and covariance identityM=eye(2n+1,2n+1); Wm=[Lambda/(n+Lambda) repmat(1/(2*(n+Lambda)),1,2n)]'; Wc=Wm; Wc(1)=Wc(1)+(1-alpha^2+n); meanVal=sigmaPointsWm; covVal=(sigmaPoints-meanVal)diag(Wc)(sigmaPoints-meanVal)'+Q; end

function y=measurementModel(x) %Measurement model y=x(1); end

main.m
0 chars
1 lines