unscented_transform.asv

来自「通用的unscented 卡尔曼滤波器是现在应用最广泛的非线性卡尔曼滤波器 又叫」· ASV 代码 · 共 28 行

% Time update equations

function [xe,P] = unscented_transform(func,xe,P,u,Q,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];

xs = feval(func,x_sp,u,varargin{:}); 

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