ukf_hfun.m

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

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

% PURPOSE : Measurement model function fpr UKF.
% INPUTS  : - x:  Hidden state
%         : - u:  control vector  
%           - n:  Measurement noise
%           - t:  time index or itegration constant
% OUTPUTS : - y:  Observation of state
% % 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
[dim,np] = size(x);
y=zeros(1,np);
for i=1:np
 y(:,i) = atan2(x(3,i), x(1,i))+n(:,i); 
 idx = find(abs(y(:,i)) > pi);
 temp = rem(y(:,idx),2*pi);
 y(:,idx) = temp - sign(temp).*(2*pi);
end