UKF的MATLAB程序

function [xcp]=UKFfiter（systemfunmeasurefunxc0ycp0sysparaIMU）
%------------------------------------------
%此程序用于描述UKF（无迹kalman滤波）算法
%作者: Jiangfeng
%日期：2012.3.17
%------------------------------------------
global Qf Rf n;
%----------------参数注解-------------------
% xc0---状态初值（列向量）  yc---系统测量值
% p0----状态误差协方差    n----系统状态量数
%systemfun---系统方程 measurefun--测量方程
%---------------滤波初始化------------------
alp=0.01;                                 %default tunable
kap=-1;                                   %default tunable
beta=2;                                   %default tunable
lamda=alp^2*（n+kap）-n;                    %scaling factor
nc=n+lamda;                               %scaling factor
Wm=[lamda/nc 0.5/nc+zeros（12*n）];        %weights for means
Wc=Wm;
Wc（1）=Wc（1）+（1-alp^2+beta）;               %weights for covariance
ns=sqrt（nc）;
t=syspara（1）;h=syspara（2）;
%-------------------------------------------

  sxk=0;spk=0;syk=0;pyy=0;pxy=0; p=p0;     
    
%--------------构造si