无迹卡尔曼滤波

无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是卡尔曼滤波的一种扩展，它不需要对非线性函数进行线性化，而是通过选取一组称为sigma点的采样点来近似非线性函数的期望和协方差。

下面给出UKF二维平面仿真。观测为单个雷达模型。

clc;close all;clear;
bushu=100;
T=0.1;
fai=[1 , T , 0 , 0;
     0 , 1 , 0 , 0;
     0 , 0 , 1 , T;
     0 , 0 , 0 , 1];
gama=[0.5*T^2  ,   0   ;
         T     ,   0   ;
         0     ,0.5*T^2;
         0     ,   T   ];
%% 模拟数据
X(:,1)=[1;1;1;1];%%x Vx y Vy
Q=1e-2*diag([1,1]);
w=sqrt(Q)*randn(2,bushu);
for i=2:bushu
    X(:,i)=fai*X(:,i-1)+gama*w(:,i);
end
%% 测站位置& measurement equation
x0=2;y0=3;
R1=0.002;R2=0.003;R=diag([R1,R2]);
V1=sqrtm(R1)*randn(1,bushu);
V2=sqrtm(R2)*randn(1,bushu);
for i=1:bushu
    Z1(:,i)=sqrtm((x0-X(1,i))^2+(y0-X(3,i))^2)+V1(i);
    Z2(:,i)=atan((y0-X(3,i))/(x0-X(1,i)))+V2(i);
end
Z=[Z1;Z2];
%% CMF-UKF
n=4;
alpha=0.01;beta=2;kk=0;
lanmega=1;
%% UT变换
for j=1:2*n+1
    Wm(j)=lanmega/(2*(n+lanmega));
    Wc(j)=lanmega/(2*(n+lanmega));
end
Wm(1)=lanmega/(n+lanmega);
Wc(1)=lanmega/(n+lanmega)+3;
Xukf(:,1)=[1;1;1;1];
P(:,:,1)=0.01*eye(4);
for i=2:bushu
    Xestimate0(:,i-1)=Xukf(:,i-1);
    chl_P(:,:,i-1)=(chol((n+lanmega)*P(:,:,i-1)))';
    for k=1:n
        Xestimate1(:,k,i-1)=Xestimate0(:,i-1)+chl_P(:,k,i-1);
        Xestimate2(:,k,i-1)=Xestimate0(:,i-1)-chl_P(:,k,i-1);
    end
    Xsigma(:,:,i-1)=[Xestimate0(:,i-1),Xestimate1(:,:,i-1),Xestimate2(:,:,i-1)];
    Xsigma_jian(:,:,i)=fai*Xsigma(:,:,i-1);
    Xjian(:,i)=zeros(4,1);
    for k=1:2*n+1
        Xjian(:,i)=Xjian(:,i)+Wm(k)*Xsigma_jian(:,k,i);
    end
    Pxx(:,:,i)=zeros(4,4);
    for k=1:2*n+1
        Pxx(:,:,i)=Pxx(:,:,i)+Wc(k)*(Xjian(:,i)-Xsigma_jian(:,k,i))*(Xjian(:,i)-Xsigma_jian(:,k,i))';
    end
    Pxx(:,:,i)=Pxx(:,:,i)+gama*Q*gama';
    cho_P0(:,:,i)=(chol((n+lanmega)*Pxx(:,:,i)))';
    for k=1:n
        Xaugsigma1(:,k,i)=Xjian(:,i)+cho_P0(:,k,i);
        Xaugsigma2(:,k,i)=Xjian(:,i)-cho_P0(:,k,i);
    end
    Xaugsigma(:,:,i)=[Xjian(:,i),Xaugsigma1(:,:,i),Xaugsigma2(:,:,i)];
    for k=1:2*n+1
        Zsigma(1,k,i)=sqrtm((Xaugsigma(1,k,i)-x0)^2+(Xaugsigma(3,k,i)-y0)^2);
        Zsigma(2,k,i)=atan((Xaugsigma(3,k,i)-y0)/(Xaugsigma(1,k,i)-x0));
    end
    Zjian(:,i)=zeros(2,1);
    for k=1:2*n+1
        Zjian(:,i)=Zjian(:,i)+Wm(k)*Zsigma(:,k,i);
    end
    Pzz(:,:,i)=zeros(2,2);
    for k=1:2*n+1
        Pzz(:,:,i)=Pzz(:,:,i)+Wc(k)*(Zjian(:,i)-Zsigma(:,k,i))*(Zjian(:,i)-Zsigma(:,k,i))';
    end
    Pzz(:,:,i)=Pzz(:,:,i)+R;
    Pxz(:,:,i)=zeros(4,2);
    for k=1:2*n+1
        Pxz(:,:,i)=Pxz(:,:,i)+Wc(k)*(Xjian(:,i)-Xaugsigma(:,k,i))*(Zjian(:,i)-Zsigma(:,k,i))';
    end
    K(:,:,i)=Pxz(:,:,i)*pinv(Pzz(:,:,i));
    Xukf(:,i)=Xjian(:,i)+K(:,:,i)*(Z(:,i)-Zjian(:,i));
    P(:,:,i)=Pxx(:,:,i)-K(:,:,i)*Pzz(:,:,i)*K(:,:,i)';
end

figure(1)
plot(Xukf(1,:),Xukf(3,:),'r',X(1,:),X(3,:),'b')
legend('UKF','True');