ffun.m

无忌卡尔曼粒子滤波

%/upf_demos/ffun.m
function [y] = ffun(x,w,t,dt);
% PURPOSE : Process model function.
% INPUTS  : - x:  The evaluation point in the domain.: 9x1
%           - w:  角速度 3xt
%           - dt: 采样间隔
% OUTPUTS : - y: The value of the function at x.  9x1
% AUTHORS  : 
% DATE     : 

if nargin < 3, error('Not enough input arguments.'); 
    
else 

y = [ x(1:3,i,t-1)+0.5*dt*((w(:,t-1)-x(4:6,i,t-1)-x(7:9,i,t-1))+cross_product(x(1:3,i,t-1))*(w(:,t-1)-x(4:6,i,t-1)-x(7:9,i,t-1))+x(1:3,i,t-1)'*(w(:,t-1)-x(4:6,i,t-1)-x(7:9,i,t-1))*x(1:3,i,t-1));
                             exp(-dt/tal)*x(4:6,i,t-1);
                             x(7:9,i,t-1)              ] ; 
                         
end