simp_unscented.m

来自「抖动的存在」· M 代码 · 共 27 行

function [X_sigma, Weight] = Simp_Unscented(n, W0)
%Spherical Simplex Unscented Transform
%n, 状态的维数
%W0, 权值的初值
%X_sigma(n,n+2), Weight(1,n+2)

X_sigma = zeros(n,n+2);
Weight = zeros(1, n+2);
W1 = (1-W0)/(n+1);
Weight = [W0, cvecrep(W1, n+1)];
X_sigma(1,1) = 0;
X_sigma(1,2) = -1/((2*W1)^0.5);
X_sigma(1,3) = 1/((2*W1)^0.5);

for j = 2:n
    for i = 0:(j+1)
        if(i == 0)
            X_sigma(1:j,1) = [X_sigma(1:(j-1),1); 0];
        elseif(i == (j+1))
            X_sigma(1:j,i+1) = [zeros(j-1,1); j/((j*(j+1)*W1)^0.5)];
        else
            X_sigma(1:j,i+1) = [X_sigma(1:(j-1),i+1);-1/((j*(j+1)*W1)^0.5)];
        end
    end
end