【滤波跟踪】基于matlab扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF比较【含Matlab源码 1933期】

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【滤波跟踪】基于matlab扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF比较【含Matlab源码 1933期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、EKF算法简介

在SLAM问题解决方法中，EKF算法是比较常用的经典算法。机器人的自定位过程是一个非线性化的过程，EKF是为了解决Kalman滤波器不能应用于非线性系统而产生的，该滤波算法的主要过程是预测和更新。在预测和更新过程中，EKF算法对原有的系统方程和观测方程进行线性化并得到一个高估计的结果。如果系统中的非线性很弱，EKF也能给出很好的估计结果。

预测时，使用系统模型如下：

式中，z(k+1）表示的是第k+1步中获得的观测量，W （k+1）表示Kalman增益，其中还包括了前向估计在实际中的权重。

实现EKF-SLAM需要以下几方面内容：系统动态方程以及相应的参量，使用的传感器类型和观测方程，根据这些可以得到EKF的相应形式。二维平面中的EKF-SLAM需要知道机器人在X、Y方向的值，还需要知道二维平面下机器人头部朝向与X轴正方向的夹角。

三、部分源代码

%% 主函数功能：EKF与UKF误差分析，性能比较
%图一：真实状态和EKF滤波状态比较
%图二：真实状态和UKF滤波状态比较
%图三：EKF滤波误差浮动范围
%图四：UKF滤波误差浮动范围
N=150; %仿真时间 
L=1;  
Q=6; %噪声方差 
R=1;  
W=sqrtm(Q)*randn(L,N);  
V=sqrt(R)*randn(1,N);  
X=zeros(L,N);  
X(:,1)=[0.1]';  
Z=zeros(1,N);  
Z(1)=X(:,1)^2/20+V(1);  
Xukf=zeros(L,N);  
Xukf(:,1)=X(:,1)+sqrtm(Q)*randn(L,1);  
Pukf=eye(L);  
Xekf=zeros(L,N);  
Xekf(:,1)=X(:,1)+sqrtm(Q)*randn(L,1);  
Pekf=eye(L);  
%算法仿真验证
for k=2:N  
    X(:,k)=0.5*X(:,k-1)+2.5*X(:,k-1)/(1+X(:,k-1)^2)+8*cos(1.2*k)+W(k);  
    Z(k)=X(:,k)^2/20+V(k);  
    [Xekf(:,k),Pekf]=EKF(Xekf(:,k-1),Pekf,Z(k),Q,R,k);  
    [Xukf(:,k),Pukf]=UKF(Xukf(:,k-1),Pukf,Z(k),Q,R,k);  
end

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.
[4]林志东.基于扩展卡尔曼滤波算法的SLAM问题分析[J].城市建筑. 2020,17(11)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除