unscented_transform.m

通用的unscented 卡尔曼滤波器是现在应用最广泛的非线性卡尔曼滤波器 又叫做sp 卡尔曼滤波器

% Time update equations

function [xe,P] = unscented_transform(func,dfunc,xe,P,varargin)


% Set up dimensions(general UKF)
D = length(xe);  
N = D*2 + 1;    
scale = 3;      
kappa = scale-D;

% Create sigma points 
Ps = sqrt_posdef(P) * sqrt(scale);
x_sp = [xe, repvec(xe,D)+Ps, repvec(xe,D)-Ps];

if isempty(dfunc), dfunc = @default_dfunc; end
xs = feval(func,x_sp, varargin{:});  % compute (possibly discontinuous) transform
base = repvec(xs(:,1), N);          % set first transformed sample as the base
delta = feval(dfunc, base, xs);     % compute correct residual
xs = base - delta;                  % offset ys from base according to correct residual

% Calculate the priori estimation
idx = 2:N;
xe = (2*kappa*xs(:,1) + sum(xs(:,idx),2)) / (2*scale);

% Calculate the priori covariance
dx = xs - repvec(xe,N);
P  = (2*kappa*dx(:,1)*dx(:,1)' + dx(:,idx)*dx(:,idx)') / (2*scale);

function x = repvec(x,N)
x = x(:, ones(1,N));

function e = default_dfunc(y1, y2)
e = y1 - y2;