unscented_update.m

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

% Measurement update equations

function [xe,ye,P] = unscented_update(yfunc,dyfunc, xe,y,P,R)

% 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];

% Calculate transformed measurements
if isempty(dyfunc), dyfunc = @default_dfunc; end
ys = feval(yfunc, x_sp); % compute (possibly discontinuous) transform
yy = repvec(y,N);
dy = feval(dyfunc, yy, ys); % compute correct residual
ys = yy - dy;               % offset zs from z according to correct residual
              
% Calculate predicted measurement
ye = (kappa*ys(:,1) + 0.5*sum(ys(:,2:end), 2)) / scale; 

% Calculate observation covariance and the state-observation correlation matrix
dx = x_sp - repvec(xe,N);
dy = ys - repvec(ye,N);
Pxy = (2*kappa*dx(:,1)*dy(:,1)' + dx(:,2:end)*dy(:,2:end)') / (2*scale);
Py = (2*kappa*dy(:,1)*dy(:,1)' + dy(:,2:end)*dy(:,2:end)') / (2*scale);

% Compute Kalman gain
S = Py + R;
Sc  = chol(S);  
Sci = inv(Sc);
Wc = Pxy * Sci;
W  = Wc * Sci';

% Perform update
xe = xe + W*(y - ye);
P = P - Wc*Wc';


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


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