精华内容
下载资源
问答
我要提问

  • 平方根无迹卡尔曼滤波matlab代码

     2019-01-21 22:30:36
    前段时间用平方根无迹卡尔曼滤波做了一个观测器算法，之前也找了好多该算法的资料，在CSDN也下载过，但是并不能用，后来自己结合之前下载的代码又上各种论坛，梳理了一下。最终代码可以用了，论文也顺利投出去了。... 
    
                                    
    
                                    
    收起 
    
                                
    
                            
    
                                
    
                                    
                                        代码   
                                
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    UKF无迹卡尔曼滤波算法matlab代码
    
                                    
                                2018-09-14 21:32:16
                            
    
                            
    
                                
    
                                    UKF的matlab代码，用于无迹卡尔曼滤波算法的学习，希望大家多多交流，共同学习.
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    无迹卡尔曼滤波
    
                                    
                                2018-03-22 18:52:10
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波matlab的代码，最基础的，有需要的可以在这个基础上修改
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    无迹卡尔曼滤波-三维目标跟踪MATLAB实现.zip
    
                                    
                                2021-04-01 22:30:51
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波在目标跟踪中的应用 无迹卡尔曼滤波算法； MATLAB仿真 目标跟踪matlab仿真实现; Case: 三维目标跟踪情况 对应的理论分析和参数设置，见博文...
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    无迹卡尔曼滤波UKF-二维目标跟踪MATLAB仿真.zip
    
                                    
                                2021-04-01 22:28:45
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波在目标跟踪中的应用 无迹卡尔曼滤波算法； MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置，见博文...
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    无迹卡尔曼滤波UKF,及目标跟踪.zip
    
                                    
                                2021-03-31 14:43:06
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波算法 仿真场景为三维雷达目标跟踪 并与容积卡尔曼滤波算法比较了性能 MATLAB仿真仿真实现; 蒙特卡洛仿真实验， 输出：三维跟踪轨迹，各纬度跟踪轨迹，估计均方误差RMSE，位置RMSE,速度RMSE.  对应的...
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    UKF（Unscented Kalman Filter) 无迹卡尔曼滤波程序代码
    
                                    
                                2017-06-05 16:54:16
                            
    
                            
    
                                
    
                                    本代码实现是对UKF（无迹卡尔曼滤波）的MATLAB实现
                                
    
                                
    
                                    
    
                                        
    
                                    
    
                                    
    收起 
    
                                
    
                            
    

                        
    • 
                        
  • 
                            
    
                                
                                    
    利用matlab实现扩展卡尔曼滤波（EKF）与无迹卡尔曼滤波（UKF）
    
                                    
                                    
                                            千次阅读
                                    
                                2020-05-31 16:28:19
                            
    
                            
    
                                
    
                                    %滤波周期 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—目标跟踪中的应用(算法部分)
    
                                    
                                2021-04-01 19:22:47
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波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 滤波算法；
    
当n≤2n\leq2时即低维非线性系统， UKF 的估计精度高于 CKF，应选用 UKF 滤波
    
算法；
    
当n=2n=2时的非线性系统， UKF 及 CKF 的估计精度相同，但 CKF 的实时性更
    
好，应选用 CKF 滤波算法；
    
当 n≥3n\geq3时即高维非线性系统， CKF 的估计精度高于 UKF，应选用 CKF 滤波算法。
    

    无迹卡尔曼滤波UKF—目标跟踪中的应用
    

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

    1.1 问题描述（离散时间非线性系统描述）
    

    考虑带加性噪声的一般非线性系统模型，
    
xk=f(xk−1)+wk−1zk=h(xk)+vk(1)x_k=f(x_{k-1}) +w_{k-1} \\
z_k=h(x_k)+v_k     \tag{1}
    
其中xkx_k为kk时刻的目标状态向量。zkz_k为kk时刻量测向量（传感器数据）。这里不考虑控制器uku_k。wk{w_k}和vk{v_k}分别是过程噪声序列和量测噪声序列，并假设wkw_k和vkv_k为零均值高斯白噪声，其方差分别为QkQ_k和RkR_k的高斯白噪声，即wk∼(0,Qk)w_k\sim(0,Q_k), vk∼(0,Rk)v_k\sim(0,R_k)，且满足如下关系(线性高斯假设)为：
    
E[wivj′]=0E[wiwj′]=0i≠jE[vivj′]=0i≠j
   \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}
    
其中λ=α2（n+κ）−n\lambda=\alpha^2（{n+\kappa}）-n，决定sigma点距离xˉ\bar{x}的距离。10−4≤α<010^{-4}\leq\alpha<0，κ\kappa一般取0或3−n3-n。对于高斯分布，β=2\beta=2为最优，如果为单变量则为0。此外，Px′Px=Px\sqrt{P_x}'\sqrt{P_x}=P_x，[Px]i[\sqrt{P_x}]_i表示矩阵的第ii列元素。
    

    1.3 无迹卡尔曼滤波器（UKF）
    

    1.）  初始化
    
步骤一：
    
给定k−1k-1时刻的状态估计和协方差矩阵
    
x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1\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^0∣0=xˉ0P0∣0=P0\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0
    
2. ) 状态预测
    
步骤二： 根据x^k−1∣k−1,Pk−1∣k−1\hat{x}_{k-1|k-1},P_{k-1|k-1}，利用UT变换选取2n+12n+1个sigam点和权值
    
Xk−1∣k−10=x^k−1∣k−1,i=0Xk−1∣k−1i=x^k−1∣k−1+n+λ[Pk−1∣k−1]i,i=1,2,⋯ ,nXk−1∣k−1n+i=x^k−1∣k−1−n+λ[Pk−1∣k−1]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,wic，i=1,2,⋯ ,2nw^m_0,w^m_i, w^c_0, w^c_i，i=1,2,\cdots,2n。
    

    步骤三：  根据系统方程传播sigma点
    
Xk∣k−1i=f(Xk−1∣k−1i)，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
    
步骤四：  状态一步预测和预测方差
    
xk∣k−1=∑i=02nwmXk∣k−1iPk∣k−1=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Xk∣k−1i−xk∣k−1)′+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. ) 状态更新
    
步骤五：  将Xk∣k−1i\mathcal{X}^i_{k|k-1}点通过量测方程传播，得到量测预测sigma点
    
Zk∣k−1i=h(Xk∣k−1i)，i=0,1,2,⋯ ,2n\mathcal{Z}^i_{k|k-1}=h(\mathcal{X}^i_{k|k-1})，i=0,1,2,\cdots,2n
    
步骤六： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
    
zk∣k−1=∑i=02nwmZk∣k−1iSk=∑i=02nwc(Zk∣k−1i−zk∣k−1)(Zk∣k−1i−zk∣k−1)′+RkCk=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Zk∣k−1i−zk∣k−1)′Kk=CkSk−1   \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^k∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)Pk∣k=cov(x~k∣k)=Pk∣k−1−KkSkKk′(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~k∣k=xk−x^k∣k\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}
    

    

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

    

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

    2.1 问题描述（离散时间非线性系统描述）
    

    考虑带非加性噪声的一般非线性系统模型，
    
xk=f(xk−1，wk−1)zk=h(xk，vk)(2-1)x_k=f(x_{k-1}， w_{k-1})  \\
z_k=h(x_k， v_k)     \tag{2-1}
    
其中xkx_k为kk时刻的目标状态向量。zkz_k为kk时刻量测向量（传感器数据）。这里不考虑控制器uku_k。wk{w_k}和vk{v_k}分别是过程噪声序列和量测噪声序列，并假设wkw_k和vkv_k为零均值高斯白噪声，其方差分别为QkQ_k和RkR_k的高斯白噪声，即wk∼(0,Qk)w_k\sim(0,Q_k), vk∼(0,Rk)v_k\sim(0,R_k)，且满足如下关系(线性高斯假设)为：
    
E[wivj′]=0E[wiwj′]=0i≠jE[vivj′]=0i≠j
   \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
    
给定k−1k-1时刻的状态估计和协方差矩阵x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}，则
    
x^k−1∣k−1a=[x^k−1∣k−1′,0′,0′]′∈RnPk−1∣k−1a=[Pk−1∣k−1000Qk−1000Rk−1]   \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^0∣0a=[xˉ0′,0′,0′]′∈RnPk−1∣k−1a=[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^k−1∣k−1a,Pk−1∣k−1a\hat{x}^a_{k-1|k-1},P^a_{k-1|k-1}，利用UT变换选取2n+12n+1个sigam点和权值
    
Xk−1∣k−10=x^k−1∣k−1a,i=0Xk−1∣k−1i=x^k−1∣k−1a+n+λ[Pk−1∣k−1a]i,i=1,2,⋯ ,nXk−1∣k−1n+i=x^k−1∣k−1a−n+λ[Pk−1∣k−1a]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,wic，i=1,2,⋯ ,2nw^m_0,w^m_i, w^c_0, w^c_i，i=1,2,\cdots,2n。
    
注意：扩维后的sigma点的组成
    
Xk−1∣k−1i=[(Xk−1∣k−1i,x)′,(Xk−1∣k−1i,w)′,(Xk−1∣k−1i,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点
    
Xk∣k−1i,x=f(Xk−1∣k−1i,x,Xk−1∣k−1i,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
    
步骤四：  状态一步预测和预测方差
    
xk∣k−1=∑i=02nwmXk∣k−1i,xPk∣k−1=∑i=02nwc(Xk∣k−1i,x−xk∣k−1)(Xk∣k−1i,x−xk∣k−1)′   \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. ) 状态更新
    
步骤五：  将Xk∣k−1i\mathcal{X}^i_{k|k-1}点通过量测方程传播，得到量测预测sigma点
    
Zk∣k−1i,x=h(Xk∣k−1i,x,Xk∣k−1i,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
    
步骤六： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
    
zk∣k−1=∑i=02nwmZk∣k−1i,xSk=∑i=02nwc(Zk∣k−1i,x−zk∣k−1)(Zk∣k−1i,x−zk∣k−1)′Ck=∑i=02nwc(Xk∣k−1i,x−xk∣k−1)(Zk∣k−1i,x−zk∣k−1)′Kk=CkSk−1   \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^k∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)Pk∣k=cov(x~k∣k)=Pk∣k−1−KkSkKk′(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~k∣k=xk−x^k∣k\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—目标跟踪中的应用(仿真部分)
    
                                    
                                2021-04-14 09:44:26
                            
    
                            
    
                                
    
                                    无迹卡尔曼滤波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

    

    无迹滤波思考：
    

    无迹卡尔曼滤波UKF—目标跟踪中的应用
    踪中的应用)
    

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

    1.1 问题描述（离散时间非线性系统描述）
    

    考虑带加性噪声的一般非线性系统模型，
    
xk=f(xk−1)+wk−1zk=h(xk)+vk(1)x_k=f(x_{k-1}) +w_{k-1} \\
z_k=h(x_k)+v_k     \tag{1}
    
其中xkx_k为kk时刻的目标状态向量。zkz_k为kk时刻量测向量（传感器数据）。这里不考虑控制器uku_k。wk{w_k}和vk{v_k}分别是过程噪声序列和量测噪声序列，并假设wkw_k和vkv_k为零均值高斯白噪声，其方差分别为QkQ_k和RkR_k的高斯白噪声，即wk∼(0,Qk)w_k\sim(0,Q_k), vk∼(0,Rk)v_k\sim(0,R_k)，且满足如下关系(线性高斯假设)为：
    
E[wivj′]=0E[wiwj′]=0i≠jE[vivj′]=0i≠j
   \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}
    
其中λ=α2（n+κ）−n\lambda=\alpha^2（{n+\kappa}）-n，决定sigma点距离xˉ\bar{x}的距离。10−4≤α<010^{-4}\leq\alpha<0，κ\kappa一般取0或3−n3-n。对于高斯分布，β=2\beta=2为最优，如果为单变量则为0。此外，Px′Px=Px\sqrt{P_x}'\sqrt{P_x}=P_x，[Px]i[\sqrt{P_x}]_i表示矩阵的第ii列元素。
    

    1.3 无迹卡尔曼滤波器（UKF）
    

    1.）  初始化
    
步骤一：
    
给定k−1k-1时刻的状态估计和协方差矩阵
    
x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1\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^0∣0=xˉ0P0∣0=P0\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0
    
2. ) 状态预测
    
步骤二： 根据x^k−1∣k−1,Pk−1∣k−1\hat{x}_{k-1|k-1},P_{k-1|k-1}，利用UT变换选取2n+12n+1个sigam点和权值
    
Xk−1∣k−10=x^k−1∣k−1,i=0Xk−1∣k−1i=x^k−1∣k−1+n+λ[Pk−1∣k−1]i,i=1,2,⋯ ,nXk−1∣k−1n+i=x^k−1∣k−1−n+λ[Pk−1∣k−1]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,wic，i=1,2,⋯ ,2nw^m_0,w^m_i, w^c_0, w^c_i，i=1,2,\cdots,2n。
    

    步骤三：  根据系统方程传播sigma点
    
Xk∣k−1i=f(Xk−1∣k−1i)，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
    
步骤四：  状态一步预测和预测方差
    
xk∣k−1=∑i=02nwmXk∣k−1iPk∣k−1=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Xk∣k−1i−xk∣k−1)′+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. ) 状态更新
    
步骤五：  将Xk∣k−1i\mathcal{X}^i_{k|k-1}点通过量测方程传播，得到量测预测sigma点
    
Zk∣k−1i=h(Xk∣k−1i)，i=0,1,2,⋯ ,2n\mathcal{Z}^i_{k|k-1}=h(\mathcal{X}^i_{k|k-1})，i=0,1,2,\cdots,2n
    
步骤六： 量测预测，量测预测误差方差（新息方差），状态与量测互协方差，增益
    
zk∣k−1=∑i=02nwmZk∣k−1iSk=∑i=02nwc(Zk∣k−1i−zk∣k−1)(Zk∣k−1i−zk∣k−1)′+RkCk=∑i=02nwc(Xk∣k−1i−xk∣k−1)(Zk∣k−1i−zk∣k−1)′Kk=CkSk−1   \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^k∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)Pk∣k=cov(x~k∣k)=Pk∣k−1−KkSkKk′(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~k∣k=xk−x^k∣k\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=[0，0]′\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)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan⁡−1yk−y0xk−x0r_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=500，x^k∣ki\hat{x}_{k|k}^i为第ii次仿真得到的估计，RMSE(Root mean-squared error):
    
RMSE(x^)=1M∑i=1M(xk−x^k∣ki)(xk−x^k∣ki)′\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~k∣k\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/s，13km,20m/s,11km,21m/s]′P0=diag(105m2,102m2/s2,105m2,102m2/s2，105m2,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=[0，0，0]′\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)=(xk−x0)+(yk−y0)2)bk=hb(xk,vk)=tan⁡−1yk−y0xk−x0ek=he(xk,vk)=tan⁡−1zk−z0(xk−x0)2+(yk−y0)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}}}\\