UKFUnscented Kalman Filter） 无迹卡尔曼滤波程序代码

function [XP] = UKF_func（ ffun X P hfun Z Q R ）
%UKF_func：UKF滤波算法
%   此处显示详细说明
    x_dim = numel（X）;  %状态维数
    o_dim = numel（Z）;  %观测维数
    alpha = 0.01;      %决定mean（x）周围sigma点的分布状态
    ki = 0;
    beta = 2;
    lambda = alpha^2*（x_dim + ki） - x_dim; %比例因子
    c = x_dim + lambda;
    Wm = [lambda/c 0.5/c+zeros（1 2*x_dim）];  %权值
    Wc = Wm;
    Wc（1） = Wc（1） + （1 - alpha^2 + beta）;   %权值
    c = sqrt（c）;

    %% 获得一组采样点（Sigma点集）
    A = c*chol（P）‘; %Cholesky分解
    Y = X（:ones（1numel（X）））;
    XsigmaSet = [X Y+A Y-A];  %Sigmada点集

    %% 计算2*x_dim+1个Sigma点集的一步预测，得到均值X1means和方差P1和新Sigma点集X1
    %对状态UT变换
    SamNum = size（XsigmaSet2）;   %得到Xsigma样本个数
    X1_pre = zeros（x_dim1）;      %系统状态量的一步预测（均值）
    X1 = zeros（x_dimSamNum）;
    for k=1:SamNum