精华内容
下载资源
问答
  • 前段时间用平方根无迹卡尔曼滤波做了一个观测器算法,之前也找了好多该算法的资料,在CSDN也下载过,但是并不能用,后来自己结合之前下载的代码又上各种论坛,梳理了一下。最终代码可以用了,论文也顺利投出去了。...
  • UKF的matlab代码,用于无迹卡尔曼滤波算法的学习,希望大家多多交流,共同学习.
  • 无迹卡尔曼滤波

    2018-03-22 18:52:10
    无迹卡尔曼滤波matlab的代码,最基础的,有需要的可以在这个基础上修改
  • 无迹卡尔曼滤波在目标跟踪中的应用 无迹卡尔曼滤波算法; MATLAB仿真 目标跟踪matlab仿真实现; Case: 三维目标跟踪情况 对应的理论分析和参数设置,见博文...
  • 无迹卡尔曼滤波在目标跟踪中的应用 无迹卡尔曼滤波算法; MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置,见博文...
  • 无迹卡尔曼滤波算法 仿真场景为三维雷达目标跟踪 并与容积卡尔曼滤波算法比较了性能 MATLAB仿真仿真实现; 蒙特卡洛仿真实验, 输出:三维跟踪轨迹,各纬度跟踪轨迹,估计均方误差RMSE,位置RMSE,速度RMSE. 对应的...
  • 本代码实现是对UKF(无迹卡尔曼滤波)的MATLAB实现
  • %滤波周期 T_go=10;%滤波时间 N=T_go/T;%观测次数 t=0:T:T_go-T;%假定输出序列(供画图用) x=zeros(2,N); z=zeros(2,N); x(:,1)=[1;0];%真值初始值 mu=[0;0]; Q=[0.01,0;0,0.0001]; R=[0.1,0;0,0.1]; rng(1); w=...

    例题

    将下例进行EKF和UKF代码实现,进行仿真复现。
    在这里插入图片描述

    EKF算法模型

    在这里插入图片描述

    clc;
    clear;
    %测量值模拟
    T=0.05;%滤波周期
    T_go=10;%滤波时间
    N=T_go/T;%观测次数
    t=0:T:T_go-T;%假定输出序列(供画图用)
    x=zeros(2,N);
    z=zeros(2,N);
    x(:,1)=[1;0];%真值初始值
    mu=[0;0];
    Q=[0.01,0;0,0.0001];
    R=[0.1,0;0,0.1];
    
    rng(1);
    w=mvnrnd(mu,Q,N)';
    v=mvnrnd(mu,R,N)';
    for k=1:N-1
        x1=x(1,k);x2=x(2,k);
        w1=w(1,k);w2=w(2,k);
        x(:,k+1)=[x1+T*x2+w1;-10*T*sin(x1)+(1-T)*x2+w2];
        x1=x(1,k+1);x2=x(2,k+1);
        v1=v(1,k+1);v2=v(2,k+1);
        z(:,k+1)=[2*sin(x1/2)+v1;x1/2+v2];
    end
    %EKF估计
    xk=zeros(2,N);
    xk(:,1)=[1;0];
    Pk=[1,0;0,1];
    for k=1:N-1
        x1=xk(1,k);x2=xk(2,k);
        Fai=[1,T;-10*T*cos(x1),1-T];
        xk(:,k+1)=[x1+T*x2;-10*T*sin(x1)+(1-T)*x2];
        Pk=Fai*Pk*Fai'+Q;
        x1=xk(1,k+1);x2=xk(2,k+1);
        Hk=[cos(x1/2),0;1/2,0];
        Kk=Pk*Hk'/(Hk*Pk*Hk'+R);
        xk(:,k+1)=xk(:,k+1)+Kk*(z(:,k+1)-[2*sin(x1/2);x1/2]);
        Pk=(eye(2)-Kk*Hk)*Pk*(eye(2)-Kk*Hk)'+Kk*R*Kk';
    end
    
    subplot(2,1,1);
    plot(t,x(1,:),'-r',t,z(1,:),'-g',t,xk(1,:),'-b');
    xlabel('t');
    ylabel('x1');
    subplot(2,1,2);
    plot(t,x(2,:),'-r',t,z(2,:),'-g',t,xk(2,:),'-b');
    xlabel('t');
    ylabel('x2');
    legend('状态值','测量值','EKF');
    

    输出结果:
    在这里插入图片描述

    UKF算法模型

    在这里插入图片描述
    在这里插入图片描述

    clc;
    clear;
    %测量值模拟
    T=0.05;%滤波周期
    T_go=10;%滤波时间
    N=T_go/T;%观测次数
    t=0:T:T_go-T;%假定输出序列(供画图用)
    x=zeros(2,N);
    z=zeros(2,N);
    x(:,1)=[1;0];%真值初始值
    mu=[0;0];
    Q=[0.01,0;0,0.0001];
    R=[0.1,0;0,0.1];
    
    rng(1);
    w=mvnrnd(mu,Q,N)';
    v=mvnrnd(mu,R,N)';
    for k=1:N-1
        x1=x(1,k);x2=x(2,k);
        w1=w(1,k);w2=w(2,k);
        x(:,k+1)=[x1+T*x2+w1;-10*T*sin(x1)+(1-T)*x2+w2];
        x1=x(1,k+1);x2=x(2,k+1);
        v1=v(1,k+1);v2=v(2,k+1);
        z(:,k+1)=[2*sin(x1/2)+v1;x1/2+v2];
    end
    
    %UKF估计
    alpha=0.1;
    beta=2;
    kappa=1;
    n=2;%状态维数
    lamda=alpha^2*(n+kappa)-n;
    xk=zeros(2,N);
    
    %1.初始化
    xk(:,1)=[1;0];
    Pk=[1,0;0,1];
    wm=zeros(1,5);
    wc=zeros(1,5);
    wm(1)=lamda/(n+lamda);
    wc(1)=lamda/(n+lamda)+1-alpha^2+beta;
    for i=2:2*n+1
        wm(i)=1/(2*(n+lamda));
        wc(i)=1/(2*(n+lamda));
    end
    
    for k=1:N-1
    %2.计算k-1时刻采样点和权值
        xsigma=zeros(2,5);
        xsigma(:,1)=xk(:,k);
        xsigma(:,2)=xk(:,k)+sqrt(n+lamda)*sqrt(Pk(:,1));
        xsigma(:,3)=xk(:,k)+sqrt(n+lamda)*sqrt(Pk(:,2));
        xsigma(:,4)=xk(:,k)-sqrt(n+lamda)*sqrt(Pk(:,1));
        xsigma(:,5)=xk(:,k)-sqrt(n+lamda)*sqrt(Pk(:,2));
    %3.计算k时刻的一步预测模型值
        xsigmapre=zeros(2,5);
        for i=1:5
            xsigmapre(:,i)=[xsigma(1,i)+T*xsigma(2,i);-10*T*sin(xsigma(1,i))+(1-T)*xsigma(2,i)];
        end
        xkpre=xsigmapre*wm';
        Pkpre=Q;
        for i=1:5
            Pkpre=Pkpre+wc(i)*(xsigmapre(:,i)-xkpre)*(xsigmapre(:,i)-xkpre)';
        end
    %4.计算k时刻的一步预测样本点(重新采样)    
    %     xsigma(:,1)=xkpre;
    %     xsigma(:,2)=xkpre+sqrt(n+lamda)*sqrt(Pkpre(1));
    %     xsigma(:,3)=xkpre+sqrt(n+lamda)*sqrt(Pkpre(2));
    %     xsigma(:,4)=xkpre-sqrt(n+lamda)*sqrt(Pkpre(1));
    %     xsigma(:,5)=xkpre-sqrt(n+lamda)*sqrt(Pkpre(2));
    %5.计算k时刻的预测量测值
        zsigmapre=zeros(2,5);
        for i=1:5
           zsigmapre(:,i)=[2*sin(xsigma(1,i)/2);xsigma(1,i)/2] ;  
        end
        zkpre=zsigmapre*wm';
        Pzzk=R;
        Pxzk=zeros(2);
        for i=1:5
            Pzzk=Pzzk+wc(i)*(zsigmapre(:,i)-zkpre)*(zsigmapre(:,i)-zkpre)';
            Pxzk=Pxzk+wc(i)*(xsigma(:,i)-xkpre)*(zsigmapre(:,i)-zkpre)';
        end
    %6.量测更新
        Kk=Pxzk/Pzzk;
        xkpre=xkpre+Kk*(z(:,k+1)-zkpre);
        Pkpre=Pkpre-Kk*Pzzk*Kk';
        
        xk(:,k+1)=xkpre;
        Pk=Pkpre;
    end
    
    subplot(2,1,1);
    plot(t,x(1,:),'-r',t,z(1,:),'-g',t,xk(1,:),'-b');
    xlabel('t');
    ylabel('x1');
    subplot(2,1,2);
    plot(t,x(2,:),'-r',t,z(2,:),'-g',t,xk(2,:),'-b');
    xlabel('t');
    ylabel('x2');
    legend('状态值','测量值','UKF');
    

    输出结果:
    在这里插入图片描述

    展开全文
  • 无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)分 更新中ing 仿真部分见博客: [无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分] 作者:823618313@qq.com 备注: 无迹卡尔曼滤波算法;无迹滤波;Uncented Filter 两...

    无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分)

    原创不易,路过的各位大佬请点个赞

    仿真部分见博客:
    [无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
    链接: https://blog.csdn.net/weixin_44044161/article/details/115390660.

    作者:823618313@qq.com
    备注:
    无迹卡尔曼滤波算法;无迹滤波;Uncented Filter
    两种UKF算法:加性噪声UKF和非加性噪声UKF
    matlab实现;
    目标跟踪仿真
    Case: 二维目标跟踪情况和三维目标跟踪情况
    代码下载地址如下(分别为二维情形和三维情形)

    UKF仿真代码:二维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16313950 
    

    UKF仿真代码:三维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16268338
    

    无迹滤波思考:

    CKF和UKF 总结:
    当取κ=0\kappa=0时, CKF 和 UKF 的估计精度相同,但鉴于 CKF 采样点少,实时性
    比 UKF 好,故应选用 CKF 滤波算法;
    n2n\leq2时即低维非线性系统, UKF 的估计精度高于 CKF,应选用 UKF 滤波
    算法;
    n=2n=2时的非线性系统, UKF 及 CKF 的估计精度相同,但 CKF 的实时性更
    好,应选用 CKF 滤波算法;
    n3n\geq3时即高维非线性系统, CKF 的估计精度高于 UKF,应选用 CKF 滤波算法。

    1、带加性噪声的无迹卡尔曼滤波算法UKF

    1.1 问题描述(离散时间非线性系统描述)

    考虑带加性噪声的一般非线性系统模型,
    xk=f(xk1)+wk1zk=h(xk)+vk(1)x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1}
    其中xkx_kkk时刻的目标状态向量。zkz_kkk时刻量测向量(传感器数据)。这里不考虑控制器uku_kwk{w_k}vk{v_k}分别是过程噪声序列和量测噪声序列,并假设wkw_kvkv_k为零均值高斯白噪声,其方差分别为QkQ_kRkR_k的高斯白噪声,即wk(0,Qk)w_k\sim(0,Q_k), vk(0,Rk)v_k\sim(0,R_k),且满足如下关系(线性高斯假设)为:
    E[wivj]=0E[wiwj]=0ijE[vivj]=0ij \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned}

    1.2 无极变换UT

    无迹变换的目的:通过确定性采样得到随机变量x(xˉ,Px)x\sim(\bar{x},P_x)2n+12n+1个sigma点X\mathcal{X},将这些sigma点通过非线性函数传播后得到随机变量yy的sigma点,进而通过加权平均求得随机变量yy的一二阶矩(i.e.,均值和方差)。也可以理解为,根据x的统计特性,利用确定性采样法获得非线性函数y=f(x)y=f(x)传播后的y的统计特性的。UT变换如下:
    X0=xˉXi=xˉ+n+λ[Px]i,i=1,2,,nXn+i=xˉn+λ[Px]i, \begin{aligned} &\mathcal{X}^0=\bar{x}\\ &\mathcal{X}^i=\bar{x}+\sqrt{n+\lambda}[\sqrt{P_x}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}=\bar{x}-\sqrt{n+\lambda}[\sqrt{P_x}]_i, \end{aligned}
    w0m=λn+λw0c=λn+λ+(1α2+β)wim=wic=12n+2λ,i=1,2,,2n \begin{aligned} &w^m_0=\frac{\lambda}{n+\lambda}\\ &w^c_0=\frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)\\ &w^m_i=w^c_i=\frac{1}{2n+2\lambda},i=1,2,\cdots,2n \end{aligned}
    其中λ=α2n+κn\lambda=\alpha^2({n+\kappa})-n,决定sigma点距离xˉ\bar{x}的距离。104α<010^{-4}\leq\alpha<0κ\kappa一般取0或3n3-n。对于高斯分布,β=2\beta=2为最优,如果为单变量则为0。此外,PxPx=Px\sqrt{P_x}'\sqrt{P_x}=P_x[Px]i[\sqrt{P_x}]_i表示矩阵的第ii列元素。

    1.3 无迹卡尔曼滤波器(UKF)

    1.) 初始化
    步骤一:
    给定k1k-1时刻的状态估计和协方差矩阵
    x^k1k1,Pk1k1,Qk1,Rk1\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}
    当为k=0k=0时刻时,假设x0(xˉ0,P0),Q0,R0x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0},则滤波器最优初始化为
    x^00=xˉ0P00=P0\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0

    2. ) 状态预测
    步骤二: 根据x^k1k1,Pk1k1\hat{x}_{k-1|k-1},P_{k-1|k-1},利用UT变换选取2n+12n+1个sigam点和权值
    Xk1k10=x^k1k1,i=0Xk1k1i=x^k1k1+n+λ[Pk1k1]i,i=1,2,,nXk1k1n+i=x^k1k1n+λ[Pk1k1]i,i=1,2,,n \begin{aligned} &\mathcal{X}^0_{k-1|k-1}=\hat{x}_{k-1|k-1},i=0\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}_{k-1|k-1}+\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}_{k-1|k-1}=\hat{x}_{k-1|k-1}-\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ \end{aligned}
    和权值w0m,wim,w0c,wici=1,2,,2nw^m_0,w^m_i, w^c_0, w^c_i,i=1,2,\cdots,2n

    步骤三: 根据系统方程传播sigma点
    Xkk1i=f(Xk1k1i)i=0,1,2,,2n\mathcal{X}^i_{k|k-1}=f(\mathcal{X}^i_{k-1|k-1}),i=0,1,2,\cdots,2n
    步骤四: 状态一步预测和预测方差
    xkk1=i=02nwmXkk1iPkk1=i=02nwc(Xkk1ixkk1)(Xkk1ixkk1)+Qk \begin{aligned} &x_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{X}^i_{k|k-1}\\ &P_{k|k-1}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{X}^i_{k|k-1}-x_{k|k-1})'+Q_k \end{aligned}
    3. ) 状态更新
    步骤五:Xkk1i\mathcal{X}^i_{k|k-1}点通过量测方程传播,得到量测预测sigma点
    Zkk1i=h(Xkk1i)i=0,1,2,,2n\mathcal{Z}^i_{k|k-1}=h(\mathcal{X}^i_{k|k-1}),i=0,1,2,\cdots,2n
    步骤六: 量测预测,量测预测误差方差(新息方差),状态与量测互协方差,增益
    zkk1=i=02nwmZkk1iSk=i=02nwc(Zkk1izkk1)(Zkk1izkk1)+RkCk=i=02nwc(Xkk1ixkk1)(Zkk1izkk1)Kk=CkSk1 \begin{aligned} &z_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{Z}^i_{k|k-1}\\ &S_{k}=\sum_{i=0}^{2n}w^c(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'+R_k\\ &C_{k}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned}
    步骤七:

    x^kk=E[xkZk]=x^kk1+Kz(zkz^kk1)Pkk=cov(x~kk)=Pkk1KkSkKk(5)\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5}
    其中估计误差为
    x~kk=xkx^kk\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}

    以上公式(2-5)及为带加性噪声非线性系统的无迹卡尔曼滤波算法。

    二、带非加性噪声的无迹卡尔曼滤波算法(UKF)

    2.1 问题描述(离散时间非线性系统描述)

    考虑带非加性噪声的一般非线性系统模型,
    xk=f(xk1wk1)zk=h(xkvk)(2-1)x_k=f(x_{k-1}, w_{k-1}) \\ z_k=h(x_k, v_k) \tag{2-1}
    其中xkx_kkk时刻的目标状态向量。zkz_kkk时刻量测向量(传感器数据)。这里不考虑控制器uku_kwk{w_k}vk{v_k}分别是过程噪声序列和量测噪声序列,并假设wkw_kvkv_k为零均值高斯白噪声,其方差分别为QkQ_kRkR_k的高斯白噪声,即wk(0,Qk)w_k\sim(0,Q_k), vk(0,Rk)v_k\sim(0,R_k),且满足如下关系(线性高斯假设)为:
    E[wivj]=0E[wiwj]=0ijE[vivj]=0ij \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned}

    2.2 带非加性噪声的无迹卡尔曼滤波器(UKF)

    1.) 初始化
    步骤一: 将状态变量扩维 xa=[x,w,v],n=nx+nw+nvx^a=[x', w', v']', n=n_x+n_w+n_v
    给定k1k-1时刻的状态估计和协方差矩阵x^k1k1,Pk1k1,Qk1,Rk1\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1},则
    x^k1k1a=[x^k1k1,0,0]RnPk1k1a=[Pk1k1000Qk1000Rk1] \begin{aligned} &\hat{x}_{k-1|k-1}^a=[\hat{x}_{k-1|k-1}', 0', 0']'\in R^n\\ &P_{k-1|k-1}^a=\begin{bmatrix} P_{k-1|k-1}&0&0\\ 0&Q_{k-1}&0\\0&0&R_{k-1}\end{bmatrix} \end{aligned}
    当为k=0k=0时刻时,假设x0(xˉ0,P0),Q0,R0x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0},则滤波器最优初始化为
    x^00a=[xˉ0,0,0]RnPk1k1a=[P0000Q0000R0] \begin{aligned} &\hat{x}_{0|0}^a=[\bar{x}_0', 0', 0']'\in R^n\\ &P_{k-1|k-1}^a=\begin{bmatrix} P_0&0&0\\ 0&Q_0&0\\0&0&R_0\end{bmatrix} \end{aligned}

    2. ) 状态预测
    步骤二
    根据x^k1k1a,Pk1k1a\hat{x}^a_{k-1|k-1},P^a_{k-1|k-1},利用UT变换选取2n+12n+1个sigam点和权值
    Xk1k10=x^k1k1a,i=0Xk1k1i=x^k1k1a+n+λ[Pk1k1a]i,i=1,2,,nXk1k1n+i=x^k1k1an+λ[Pk1k1a]i,i=1,2,,n \begin{aligned} &\mathcal{X}^0_{k-1|k-1}=\hat{x}^a_{k-1|k-1},i=0\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}^a_{k-1|k-1}+\sqrt{n+\lambda}[\sqrt{P^a_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}_{k-1|k-1}=\hat{x}^a_{k-1|k-1}-\sqrt{n+\lambda}[\sqrt{P^a_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ \end{aligned}
    和权值w0m,wim,w0c,wici=1,2,,2nw^m_0,w^m_i, w^c_0, w^c_i,i=1,2,\cdots,2n
    注意:扩维后的sigma点的组成
    Xk1k1i=[(Xk1k1i,x),(Xk1k1i,w),(Xk1k1i,v)],i=0,1,2,,2n\mathcal{X}^i_{k-1|k-1}=[(\mathcal{X}_{k-1|k-1}^{i,x})', (\mathcal{X}_{k-1|k-1}^{i,w})', (\mathcal{X}_{k-1|k-1}^{i,v})']',i=0,1,2,\cdots,2n

    步骤三: 根据系统方程传播sigma点
    Xkk1i,x=f(Xk1k1i,x,Xk1k1i,w)i=0,1,2,,2n\mathcal{X}^{i,x}_{k|k-1}=f(\mathcal{X}_{k-1|k-1}^{i,x}, \mathcal{X}_{k-1|k-1}^{i,w}),i=0,1,2,\cdots,2n
    步骤四: 状态一步预测和预测方差
    xkk1=i=02nwmXkk1i,xPkk1=i=02nwc(Xkk1i,xxkk1)(Xkk1i,xxkk1) \begin{aligned} &x_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{X}^{i,x}_{k|k-1}\\ &P_{k|k-1}=\sum_{i=0}^{2n}w^c(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})' \end{aligned}
    3. ) 状态更新
    步骤五:Xkk1i\mathcal{X}^i_{k|k-1}点通过量测方程传播,得到量测预测sigma点
    Zkk1i,x=h(Xkk1i,x,Xkk1i,v)i=0,1,2,,2n\mathcal{Z}^{i,x}_{k|k-1}=h(\mathcal{X}^{i,x}_{k|k-1}, \mathcal{X}^{i,v}_{k|k-1}),i=0,1,2,\cdots,2n
    步骤六: 量测预测,量测预测误差方差(新息方差),状态与量测互协方差,增益
    zkk1=i=02nwmZkk1i,xSk=i=02nwc(Zkk1i,xzkk1)(Zkk1i,xzkk1)Ck=i=02nwc(Xkk1i,xxkk1)(Zkk1i,xzkk1)Kk=CkSk1 \begin{aligned} &z_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{Z}^{i,x}_{k|k-1}\\ &S_{k}=\sum_{i=0}^{2n}w^c(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})'\\ &C_{k}=\sum_{i=0}^{2n}w^c(\mathcal{X}^{i,x}_{k|k-1}-x_{k|k-1})(\mathcal{Z}^{i,x}_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned}
    步骤七:

    x^kk=E[xkZk]=x^kk1+Kz(zkz^kk1)Pkk=cov(x~kk)=Pkk1KkSkKk(5)\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5}
    其中估计误差为
    x~kk=xkx^kk\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}

    以上公式(2-5)及为带非加性噪声非线性系统的无迹卡尔曼滤波算法。

    三、仿真实验

    仿真部分见博客:
    无迹卡尔曼滤波UKF在目标跟踪中的应用—仿真部分

    https://blog.csdn.net/weixin_44044161/article/details/115385918           
    

    3.1 仿真场景(三维)

    UKF仿真代码:三维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16239480       
    

    3.2 仿真场景(二维)

    UKF仿真代码:二维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16239356 
    

    3.3 二维UKF跟踪仿真结果

    四、二维UKF跟踪参考代码

    说明:
    1.二维仿真代码也可以在上面的连接中直接下载,
    2.将UKF函数保存,文件名“fun_2UKF.m”
    3.将量测函数保存,文件名“measurements.m”
    4. 运行下面的主函数
    5. 注意将这三个文件保存在一个文件夹下

    4.1 主函数

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % created by: D. Zhang
    % date: 2020/4
    % 无迹卡尔曼滤波,目标跟踪  
    % 二维目标跟踪问题
    % 线性CV目标模型
    % 单雷达
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    clear all; close all; clc;
    %% initial parameter
    n=4; %状态维数 ;
    T=1; %采样时间
    M=1; %雷达数目
    N=200; %运行总时刻
    MC=100; %蒙特卡洛次数
    chan=1; %滤波器通道,这里只有一个滤波器
    w_mu=[0,0]'; % mean of process noise 
    v_mu=[0,0]'; % mean of measurement noise
    %% target model
    %covariance of process noise
    q_x=0.01; %m/s^2
    q_y=q_x;
    Qk=diag([q_x^2,q_y^2]); 
    % state matrix
    Fk= [1 T 0 0
         0 T 0 0
         0 0 1 T
         0 0 0 T]; %
     Gk= [ T^2/2  0
           T      0
           0      T^2/2
           0      T ]; %
    %量测模型
    sigma_r(1)=80;  sigma_b(1)=60e-3; % covariance of measurement noise (radar)
    Rk=diag([sigma_r(1)^2, sigma_b(1)^2]);
    xp=[0,0,0,0];%雷达位置
    %% 定义存储空间
    sV=zeros(n,N,MC); % 状态
    eV=zeros(n,N,MC,chan); %估计
    PV=zeros(n,n,N,MC,chan);%协方差
    rV=zeros(2,N,MC,M); % %量测
    for i=1:MC
        sprintf('rate of process:%3.1f%%',(2*i)/(4*MC)*100)
        % 初始状态的均值和方差
        x=[100,15,100,15]';
        P_0=diag([1e3,10^1,1e3,10^1]); 
        xk_ukf=x;    Pk_ukf=P_0;   % P0|0 x0|0 
        x0=mvnrnd(x,P_0); % 初始状态
        %x0=(x+normrnd(0,0.001)')';
        x=x0';
        for k=1:N
           %% %%%%%%% 目标模型和雷达量测模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % 目标模型CV 
            w=mvnrnd(w_mu',Qk)';
            x=Fk*x+Gk*w;
            sV(:,k,i)=x;
            
            % 雷达量测模型,M=1,表示一个雷达
            for m=1:M
                v=normrnd(v_mu,[sigma_r(m); sigma_b(m)]);
                [r,b] = measurements(x,xp);%r=距离,b=角度
                rm=r+v(1);
                bm=b+v(2);
                rV(:,k,i,m)=[rm,bm]';   
            end
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            %% %%%%%%%%% UKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%
            [xk_ukf,Pk_ukf] = fun_2UKF(xk_ukf,Pk_ukf,Fk,Gk,rV(:,k,i,1),Qk,Rk, xp); 
            PV(:,:,k,i,1)=Pk_ukf;
            eV(:,k,i,1)=xk_ukf;
            %%%%%%%%% end centralized fusion with non-concentric radars%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            
        end    
        
    end
        
    figure
    plot(sV(1,:,1),sV(3,:,1),'--k',eV(1,:,1,1),eV(3,:,1,1),'-r')
    xlabel('m');ylabel('m');
    legend('State','UKF')
    title('跟踪轨迹')
    figure;
    ii=1:N;
    plot(ii,sV(1,:,1),'--k',ii,eV(1,:,1,1),'r');
    xlabel('t/s');ylabel('m');%X轴的跟踪轨迹
    legend('X-轴','UKF')
    title('X轴的跟踪轨迹')
    
    %P_real=cell(MC,chan);
    P_r=0;
    for i=1:MC
        sprintf('rate of process:%3.1f%%',(3*MC+i)/(4*MC)*100)
        for k=1:N
            for c=1:chan
                error(:,c)=sV(:,k,i,1)-eV(:,k,i,c); 
                % RMSE
                error2(:,c)=error(:,c).^2;               
                error2_dis(c)=error2(1,c)+error2(3,c);
                error2_vel(c)=error2(2,c)+error2(4,c);
                position(k,i,c)=error2_dis(c);     
                velocity(k,i,c)=error2_vel(c); 
                            
            end
        end
    end
    %% RMSE
    for c=1:chan
        rms_position(:,c)=sqrt(sum(position(:,:,c),2)./MC);  
        rms_velocity(:,c)=sqrt(sum(velocity(:,:,c),2)./MC);  
    end
    figure;%position
    plot(ii,rms_position(ii,1),'.-r','LineWidth',0.7);
    legend('UKF')
    xlabel('t/s');ylabel('RMSE');
    title('position-RMS analyze')
    figure;%position
    plot(ii,rms_velocity(ii,1),'.-r','LineWidth',0.7);
    legend('UKF')
    xlabel('t/s');ylabel('RMSE');
    title('velocity-RMS analyze')    
    
    

    4.2 UKF函数

    function [xk,Pk] = fun_2UKF(xk,Pk,Fk,Gk,Zm,Qk,Rk, xp);
    %UKF Fusion
    %%
    zk=Zm(:,:); %stacked measurement:rm bm em
    % xkk=Fk*xk;
    % Pkk=Fk*Pk*Fk'+Gk*Qk*Gk';
    %UT transformation
    alpha=0.01;
    kk=0;
    beta=2; 
    n=4; %dimension of state x
    lambda=alpha^2*(n+kk)-n;
    Wm=[lambda/(lambda+n),  (0.5/(lambda+n))+zeros(1,2*n)];%权值确定
    Wc=[lambda/(lambda+n)+1-alpha^2+beta,   (0.5/(lambda+n))+zeros(1,2*n)];
    %产生xk的Sigma点
    SPk=sqrt(n+lambda)*(chol(Pk))';
    Xsigma0=xk;
    for i=1:n 
        Xsigma1(:,i)=xk+SPk(:,i);
        Xsigma2(:,i)=xk-SPk(:,i); 
    end
    Xsigma=[Xsigma0,Xsigma1,Xsigma2];
    %产生xkk的Gama点
    for i=1:2*n+1
        Xgama(:,i)=Fk*Xsigma(:,i);
    end
    xkk=Xgama*Wm';
    Pkk=zeros(n,n);for i=1:2*n+1; Pkk=Pkk+Wc(i)*((Xgama(:,i)-xkk)*(Xgama(:,i)-xkk)');end; 
    Pkk=Pkk+Gk*Qk*Gk';
    
    %产生xkk的Sigma点
    SPkk=sqrt(n+lambda)*(chol(Pkk))';
    Zsigma0=xkk;
    for i=1:n 
        Zsigma1(:,i)=xkk+SPkk(:,i);
        Zsigma2(:,i)=xkk-SPkk(:,i);
    end
    Zsigma=[Zsigma0,Zsigma1,Zsigma2];
    % zkk
    for i=1:2*n+1
        [z1,z2] = measurements(Zsigma(:,i), xp); Zgama(:,i)=[z1,z2]';
    end
    
    zkk=Zgama*Wm';
    Sk=zeros(2,2);for i=1:2*n+1; Sk=Sk+Wc(i)*((Zgama(:,i)-zkk)*(Zgama(:,i)-zkk)');end; 
    Sk=Sk+Rk;
    Ck=zeros(4,2);for i=1:2*n+1; Ck=Ck+Wc(i)*((Zsigma(:,i)-xkk)*(Zgama(:,i)-zkk)');end;
    %
    xk=xkk+Ck*inv(Sk)*(zk-zkk);
    Pk=Pkk-Ck*inv(Sk)*Ck';
    
    
    end
    

    4.3 量测函数

    function [fz1,fz2] = measurements(fx,fxp)
    %极坐标量测,二维雷达量测
    %距离量测
    fz1=sqrt((fx(1)-fxp(1))^2+((fx(3)-fxp(3))^2));
    %方位角
    fz2=atan2((fx(3)-fxp(3)),(fx(1)-fxp(1)));
    end
    
    展开全文
  • 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 算法部分见博客: [无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分] ... ...备注: 无迹卡尔曼滤波算法;无迹滤波;...

    无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)

    原创不易,路过的各位大佬请点个赞

    算法部分见博客:
    [无迹卡尔曼滤波UKF—目标跟踪中的应用(算法部分]

    https://blog.csdn.net/weixin_44044161/article/details/115381848

    作者:823618313@qq.com
    备注:
    无迹卡尔曼滤波算法;无迹滤波;Uncented Filter
    两种UKF算法:加性噪声UKF和非加性噪声UKF
    matlab实现;
    目标跟踪仿真
    Case: 二维目标跟踪情况和三维目标跟踪情况
    代码下载地址如下(分别为二维情形和三维情形)

    UKF仿真代码:二维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16313950 
    

    UKF仿真代码:三维目标跟踪

    https://download.csdn.net/download/weixin_44044161/16268338
    

    无迹滤波思考:

    踪中的应用)

    1、带加性噪声的无迹卡尔曼滤波算法UKF

    1.1 问题描述(离散时间非线性系统描述)

    考虑带加性噪声的一般非线性系统模型,
    xk=f(xk1)+wk1zk=h(xk)+vk(1)x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1}
    其中xkx_kkk时刻的目标状态向量。zkz_kkk时刻量测向量(传感器数据)。这里不考虑控制器uku_kwk{w_k}vk{v_k}分别是过程噪声序列和量测噪声序列,并假设wkw_kvkv_k为零均值高斯白噪声,其方差分别为QkQ_kRkR_k的高斯白噪声,即wk(0,Qk)w_k\sim(0,Q_k), vk(0,Rk)v_k\sim(0,R_k),且满足如下关系(线性高斯假设)为:
    E[wivj]=0E[wiwj]=0ijE[vivj]=0ij \begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned}

    1.2 无极变换UT

    无迹变换的目的:通过确定性采样得到随机变量x(xˉ,Px)x\sim(\bar{x},P_x)2n+12n+1个sigma点X\mathcal{X},将这些sigma点通过非线性函数传播后得到随机变量yy的sigma点,进而通过加权平均求得随机变量yy的一二阶矩(i.e.,均值和方差)。也可以理解为,根据x的统计特性,利用确定性采样法获得非线性函数y=f(x)y=f(x)传播后的y的统计特性的。UT变换如下:
    X0=xˉXi=xˉ+n+λ[Px]i,i=1,2,,nXn+i=xˉn+λ[Px]i, \begin{aligned} &\mathcal{X}^0=\bar{x}\\ &\mathcal{X}^i=\bar{x}+\sqrt{n+\lambda}[\sqrt{P_x}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}=\bar{x}-\sqrt{n+\lambda}[\sqrt{P_x}]_i, \end{aligned}
    w0m=λn+λw0c=λn+λ+(1α2+β)wim=wic=12n+2λ,i=1,2,,2n \begin{aligned} &w^m_0=\frac{\lambda}{n+\lambda}\\ &w^c_0=\frac{\lambda}{n+\lambda} + (1-\alpha^2+\beta)\\ &w^m_i=w^c_i=\frac{1}{2n+2\lambda},i=1,2,\cdots,2n \end{aligned}
    其中λ=α2n+κn\lambda=\alpha^2({n+\kappa})-n,决定sigma点距离xˉ\bar{x}的距离。104α<010^{-4}\leq\alpha<0κ\kappa一般取0或3n3-n。对于高斯分布,β=2\beta=2为最优,如果为单变量则为0。此外,PxPx=Px\sqrt{P_x}'\sqrt{P_x}=P_x[Px]i[\sqrt{P_x}]_i表示矩阵的第ii列元素。

    1.3 无迹卡尔曼滤波器(UKF)

    1.) 初始化
    步骤一:
    给定k1k-1时刻的状态估计和协方差矩阵
    x^k1k1,Pk1k1,Qk1,Rk1\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}
    当为k=0k=0时刻时,假设x0(xˉ0,P0),Q0,R0x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0},则滤波器最优初始化为
    x^00=xˉ0P00=P0\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0

    2. ) 状态预测
    步骤二: 根据x^k1k1,Pk1k1\hat{x}_{k-1|k-1},P_{k-1|k-1},利用UT变换选取2n+12n+1个sigam点和权值
    Xk1k10=x^k1k1,i=0Xk1k1i=x^k1k1+n+λ[Pk1k1]i,i=1,2,,nXk1k1n+i=x^k1k1n+λ[Pk1k1]i,i=1,2,,n \begin{aligned} &\mathcal{X}^0_{k-1|k-1}=\hat{x}_{k-1|k-1},i=0\\ &\mathcal{X}^i_{k-1|k-1}=\hat{x}_{k-1|k-1}+\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ &\mathcal{X}^{n+i}_{k-1|k-1}=\hat{x}_{k-1|k-1}-\sqrt{n+\lambda}[\sqrt{P_{k-1|k-1}}]_i,i=1,2,\cdots,n\\ \end{aligned}
    和权值w0m,wim,w0c,wici=1,2,,2nw^m_0,w^m_i, w^c_0, w^c_i,i=1,2,\cdots,2n

    步骤三: 根据系统方程传播sigma点
    Xkk1i=f(Xk1k1i)i=0,1,2,,2n\mathcal{X}^i_{k|k-1}=f(\mathcal{X}^i_{k-1|k-1}),i=0,1,2,\cdots,2n
    步骤四: 状态一步预测和预测方差
    xkk1=i=02nwmXkk1iPkk1=i=02nwc(Xkk1ixkk1)(Xkk1ixkk1)+Qk \begin{aligned} &x_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{X}^i_{k|k-1}\\ &P_{k|k-1}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{X}^i_{k|k-1}-x_{k|k-1})'+Q_k \end{aligned}
    3. ) 状态更新
    步骤五:Xkk1i\mathcal{X}^i_{k|k-1}点通过量测方程传播,得到量测预测sigma点
    Zkk1i=h(Xkk1i)i=0,1,2,,2n\mathcal{Z}^i_{k|k-1}=h(\mathcal{X}^i_{k|k-1}),i=0,1,2,\cdots,2n
    步骤六: 量测预测,量测预测误差方差(新息方差),状态与量测互协方差,增益
    zkk1=i=02nwmZkk1iSk=i=02nwc(Zkk1izkk1)(Zkk1izkk1)+RkCk=i=02nwc(Xkk1ixkk1)(Zkk1izkk1)Kk=CkSk1 \begin{aligned} &z_{k|k-1}=\sum_{i=0}^{2n}w^m\mathcal{Z}^i_{k|k-1}\\ &S_{k}=\sum_{i=0}^{2n}w^c(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'+R_k\\ &C_{k}=\sum_{i=0}^{2n}w^c(\mathcal{X}^i_{k|k-1}-x_{k|k-1})(\mathcal{Z}^i_{k|k-1}-z_{k|k-1})'\\ &K_k=C_{k}S_{k}^{-1} \end{aligned}
    步骤七:

    x^kk=E[xkZk]=x^kk1+Kz(zkz^kk1)Pkk=cov(x~kk)=Pkk1KkSkKk(5)\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5}
    其中估计误差为
    x~kk=xkx^kk\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}

    以上公式(2-5)及为带加性噪声非线性系统的无迹卡尔曼滤波算法。

    2、带非加性噪声的无迹卡尔曼滤波算法(UKF)

    2.1 问题描述(离散时间非线性系统描述)

    2.2 带非加性噪声的无迹卡尔曼滤波器(UKF)

    3、仿真实验一:无迹卡尔曼滤波—二维雷达目标跟踪

    3.1 二维目标仿真

    目标模型
    为了方便,考虑一个二维匀速运动目标,即CV 模型:
    xk+1=Fkxk+Gkwkx_{k+1}=F_kx_k+G _kw_k
    其中状态xk=[xk,x˙k,yk,y˙k]x_k=[x_k,\dot{x}_k,y_k,\dot{y}_k]'[xk,yk[x_k,y_k'为目标位置,[dotxk,y˙k][dot{x}_k,\dot{y}_k]'为目标速度;噪声为wk=[wx,wy]w_k=[w_x,w_y]';状态转移矩阵FkF_k和噪声驱动矩阵GkG_k如下
    Fk=[1T000100001T0001]Gk=[1/2T20T001/2T20T]F_k=\begin{bmatrix}1 & T & 0 & 0 \\0 & 1 & 0 & 0 \\0 & 0 & 1 & T\\0 & 0 & 0 & 1\end{bmatrix} \qquad G_k=\begin{bmatrix}1/2T^2 & 0 \\T & 0 \\0 & 1/2T^2 \\0 & T \end{bmatrix}
    初始状态为 x0(xˉ0,P0)xˉ0=[100m,15m/s,100m,15m/s]P0=diag(103m2,10m2/s2,103m2,102m/s2)x_0\sim(\bar{x}_0,P_0)\\\bar{x}_{0}=[100m, 15\text{m/s} ,100m, 15\text{m/s}]'\\P_{0}=\text{diag}(10^3\text{m}^2, 10\text{m}^2/\text{s}^2, 10^3\text{m}^2, 10^2\text{m}/\text{s}^2)
    过程噪声噪声均值为0,即wˉk=[00]\bar{w}_k=[0,0]'。方差分别为
    Qk=[0.12000.12]Q_k=\begin{bmatrix}0.1^2 & 0 \\0 & 0.1^2 \end{bmatrix}
    采样时间 T=1sT=1s.

    尽管这里的目标模型为线性,在使用UKF预测时,可以用两种方法:xkk=Fkxk,Pkk=FkPkFk’+GkQk*Gk’;或者采用sigma取点来计算该线性模型前两阶矩(线性系统是特殊的非线性系统)

    雷达量测模型
    在二维情况下,雷达量测为距离,方位角
    rkm=rk+r~kbkm=bk+b~k{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k
    其中
    rk=hr(xk,vk)=(xkx0)+(yky0)2)bk=hb(xk,vk)=tan1yky0xkx0r_k=h_r(x_k,v_k)=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=h_b(x_k,v_k)=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\
    [x0,y0][x_0,y_0]为传感器(雷达)坐标,一般情况为0。雷达量测为zk=[rk,bk]z_k=[r_k,b_k]'。雷达量测方差为
    Rk=cov(vk)=[σr200σb2]R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}σr=80m\sigma_r=80mσb=60mrad\sigma_b=60mrad
    性能指标
    蒙塔卡罗次数M=500M=500x^kki\hat{x}_{k|k}^i为第ii次仿真得到的估计,RMSE(Root mean-squared error):
    RMSE(x^)=1Mi=1M(xkx^kki)(xkx^kki)\text{RMSE}(\hat{x})=\sqrt{\frac{1}{M}\sum_{i=1}^{M}(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k}^i)(\mathbf{x}_k-\hat{\mathbf{x}}_{k|k}^i)'}
    其中位置RMSE和速度RMSE分别取对应状态x~kk\tilde{x}_{k|k}的位置项和速度项,加和,具体公式见博客《扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分》

    https://blog.csdn.net/weixin_44044161/article/details/115329181

    3.2 二维UKF跟踪仿真结果

    UKF仿真代码:二维目标仿真

    https://download.csdn.net/download/weixin_44044161/16313950

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    四、仿真实验二:无迹卡尔曼滤波—三维雷达目标跟踪

    4.1 三维目标仿真

    目标模型
    为了方便,考虑一个二维匀速运动目标,即CV 模型:
    xk+1=Fkxk+Gkwkx_{k+1}=F_kx_k+G _kw_k
    其中状态向量xk=[xk,x˙k,yk,y˙k,zk,z˙k]x_k=[x_k,\dot{x}_k,y_k,\dot{y}_k,z_k,\dot{z}_k]'[xk,yk,zk][x_k,y_k,z_k]'为目标位置,[dotxk,y˙k,z˙k][dot{x}_k,\dot{y}_k,\dot{z}_k]'为目标速度;;噪声为wk=[wx,wy,wz]w_k=[w_x,w_y,w_z]';状态转移矩阵FkF_k和噪声驱动矩阵GkG_k如下
    Fk=[1T0000010000001T0000010000001T000001]Γk=[1/2T200T0001/2T200T001/2T200T]F_k=\begin{bmatrix}1 & T & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 \\0 & 0 & 1 & T & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 1 & T\\0 & 0 & 0 & 0 & 0 & 1\end{bmatrix} \qquad\varGamma_k=\begin{bmatrix}1/2T^2 & 0 & 0 \\T & 0 & 0 \\0 & 1/2T^2 & 0 \\0 & T & \\0 & 0 & 1/2T^2 \\0 & 0 & T\end{bmatrix}
    初始状态为 x0(xˉ0,P0)xˉ0=[12km,31m/s13km,20m/s,11km,21m/s]P0=diag(105m2,102m2/s2,105m2,102m2/s2105m2,102m2/s2)x_0\sim(\bar{x}_0,P_0)\\\bar{x}_{0}=[12\text{km}, 31\text{m/s} ,13\text{km}, 20\text{m/s} ,11\text{km}, 21\text{m/s}]'\\P_{0}=\text{diag}(10^5\text{m}^2, 10^2\text{m}^2/\text{s}^2, 10^5\text{m}^2, 10^2\text{m}^2/\text{s}^2, 10^5\text{m}^2, 10^2\text{m}^2/\text{s}^2)
    过程噪声均值为0,即wˉk=[000]\bar{w}_k=[0,0, 0]'。方差为
    Qk=[0.010000.010000.01]Q_k=\begin{bmatrix}0.01 & 0& 0 \\0 & 0.01 & 0\\0&0 & 0.01 \end{bmatrix}
    采样时间 T=1sT=1s.

    尽管这里的目标模型为线性,在使用UKF预测时,可以用两种方法:xkk=Fkxk,Pkk=FkPkFk’+GkQk*Gk’;或者采用sigma取点来计算该线性模型前两阶矩(线性系统是特殊的非线性系统)

    雷达量测模型
    在二维情况下,雷达量测为距离,方位角,俯仰角
    rkm=rk+r~kbkm=bk+b~kekm=ek+e~k{r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k\\ e^m_k=e_k+\tilde{e}_k
    其中
    rk=hr(xk,vk)=(xkx0)+(yky0)2)bk=hb(xk,vk)=tan1yky0xkx0ek=he(xk,vk)=tan1zkz0(xkx0)2+(yky0)2r_k=h_r(x_k,v_k)=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=h_b(x_k,v_k)=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\ e_k=h_e(x_k,v_k)=\tan^{-1}{\frac{z_k-z_0}{\sqrt{(x_k-x_0)^2+(y_k-y_0)^2}}}\\