[filter tracking] comparison between EKF and UKF based on MATLAB extended Kalman filter [including Matlab source code 1933]

One 、 How to get the code

How to get the code 1：
The complete code has been uploaded to my resources ：【 Filter tracking 】 be based on matlab Extended Kalman filter EKF And unscented Kalman filter UKF Compare 【 contain Matlab Source code 1933 period 】

How to get the code 2：
By subscribing to Ziji Shenguang blog Paid column , With proof of payment , Private Blogger , This code is available .

remarks ： Subscribe to Ziji Shenguang blog Paid column , Free access to 1 Copy code （ The period of validity From the Subscription Date , Valid for three days ）;

Two 、EKF Introduction to the algorithm

stay SLAM Problem solving method ,EKF Algorithm is a commonly used classical algorithm . The self localization process of robot is a nonlinear process ,EKF To solve the problem Kalman Filters cannot be applied to nonlinear systems , The main process of the filtering algorithm is prediction and update . In the process of prediction and update ,EKF The algorithm linearizes the original system equation and observation equation and obtains a high estimation result . If the nonlinearity in the system is weak ,EKF It can also give good estimation results .

When predicting , The system model used is as follows ：

In style ,z(k+1） It means No k+1 The observation obtained in step ,W （k+1） Express Kalman gain , It also includes the weight of forward estimation in practice .

Realization EKF-SLAM The following aspects are required ： System dynamic equation and corresponding parameters , Type of sensor used and observation equation , According to these, we can get EKF The corresponding form of . In two-dimensional plane EKF-SLAM Need to know that robots are X、Y The value of the direction , We also need to know the orientation of the robot's head in the two-dimensional plane and X The angle in the positive direction of the axis .

3、 ... and 、 Partial source code

%%  Main function function ：EKF And UKF error analysis , Performance comparison 
% Figure 1 ： True state and EKF Filter state comparison 
% Figure 2 ： True state and UKF Filter state comparison 
% Figure 3 ：EKF Floating range of filtering error 
% Figure 4 ：UKF Floating range of filtering error 
N=150; % Simulation time  
L=1;  
Q=6; % Noise variance  
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);  
% Algorithm simulation verification 
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

Four 、 Running results

5、 ... and 、matlab Edition and references

1 matlab edition
2014a

2 reference
[1] Shen Zaiyang . Master MATLAB signal processing [M]. tsinghua university press ,2015.
[2] Gao Baojian , Peng Jinye , Wang Lin , Pan Jianshou . Signals and systems —— Use MATLAB Analysis and Implementation [M]. tsinghua university press ,2020.
[3] Wang Wenguang , Wei Shaoming , Ren Xin . Signal processing and system analysis MATLAB Realization [M]. Electronic industry press ,2018.
[4] Lin Zhidong . Based on extended Kalman Filter Algorithm SLAM Problem analysis [J]. City Building . 2020,17(11)

3 remarks
This part of the introduction is taken from the Internet , For reference only , If infringement , Contact deletion