sigma.m

自己写的UKF滤波程序

function [Sigma_point,Weight]=Sigma(Xmean,P,kappa)
%
%      [Sigma_point,Weight]=Sigma(Xmean,P,kappa)
%
% This program produce the sigma points in UKF
%      Xmean is the mean of variable X , n*1
%      P is the variance of variable X
%      kappa is the tune parameter , 
%            n+kappa != 0 , n is the dimension of X
%
%      Sigma_point is the sampled sigma points
%      Weight is weights of the sigma points 
%

n=size(Xmean,1);
Sigma_point = zeros(n,2*n+1);
Weight = zeros(2*n+1,1);

Sigma_point(:,1)=Xmean;
Weight(1)=kappa/(n+kappa);

nkP=((n+kappa)*P)^(1/2);
for i=1:n
    Sigma_point(:,1+i)   = Xmean + (nkP(i,:))';
    Sigma_point(:,n+1+i) = Xmean - (nkP(i,:))';
    Weight(1+i)   = 1/(2*(n+kappa));
    Weight(n+1+i) = 1/(2*(n+kappa));
end