ukf_ffun.m

来自「unsented kalman filter算法在惯性导航系统中的应用」· M 代码 · 共 34 行

function [y] = ukf_ffun(x,u,n,t);

% PURPOSE : Process model function for UKF
% INPUTS  : - x:  prior state
%           - u:  control vector  
%           - n:  process noise    
%           - t:  time index
% OUTPUTS : - y:  state after time update
% Copyright  (c) Gao Yanan (2004)
% Novel approach to nonlinear/non-Gaussian Bayesian state estimation
%   Bearings-only tracking example
if nargin < 3, error('Not enough input arguments.'); end
%y =x+ n;
A=[1 1 0 0;
   0 1 0 0;
   0 0 1 1;
   0 0 0 1];
B=[0.5 0;
    1  0;
    0 0.5;
    0  1];
[dim,np] = size(x);
y=zeros(dim,np);
for i=1:np
y(:,i)=A*x(:,i) +B*n(:,i);
end