1. Simple understanding (not rigorous)
The essence of UKF is to replace the mean with the conditional mean of random sampling, and calculate the variance matrix of the estimated quantity and the reproduced sample points measured by the quantity. Usually, (2n+1) Sigma points are selected to fit the mean and variance, and then the obtained variance is used to update the Kalman gain coefficient and state error.
Assume the existence of a nonlinear system:
1) First select the Sigma point (also known as UT transformation) to satisfy:
2) Multiply the obtained Sigma point matrix by the corresponding state transition matrix to obtain the predicted point Xi (or expressed as Xk|k-1), repeat step 1 again, and use UT transformation again according to the predicted value to obtain a new sigma point In the same way, the measurement points can be obtained. The new mean covariance can be obtained by multiplying by the corresponding weights.
2. Code simulation
The article is reprinted here: UKF Study Notes: Uniform Linear Motion Target Tracking_Blog to Escape with Soldier-CSDN Blog Code
function UKF
clc;
clear;
T=1; % 采样周期
N=60/T; % 采样次数
X=zeros(4,N); % 初始化真实轨迹矩阵
X(:,1)=[100 2 200 20]; % 目标初始位置、速度
Z=zeros(1,N); % 初始化 观测距离矩阵
w =1e-5;
Q = w*diag([0.5,1]); % 过程噪声协方差矩阵
G=[T^2/2, 0;
T, 0;
0,T^2/2;
0, T]; % 过程噪声驱动矩阵
R=5; % 观测噪声协方差
F=[1 T 0 0;
0 1 0 0;
0 0 1 T;
0 0 0 1]; % 状态转移矩阵
Xstation=[200,300]; % 观测位置
V = sqrt(R)*randn(1,N); % v均值为0，协方差为R的高斯白噪声 %sqrt(A)矩阵每个元素分别开方,与矩阵点乘有关;sqrtm(A)矩阵为整体,与矩阵相乘有关
W = sqrt(Q)*randn(2,1); % W均值为0，协方差为Q的高斯白噪声
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N
X(:,t)=F*X(:,t-1)+G*W; % 目标的真实轨迹
end
for t=1:N
Z(t)=Distance(X(:,t),Xstation)+V(t); % 真实观测目标位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%% UKF滤波 %%%%%%%%%%%%%%%%%%
L=4; % L为状态维度
a=1; % a控制采样点的分布状态
k=1; % k为待选参数 没有界限，但是要保证（n+lamda）*P为半正定矩阵
b=2; % b非负的权系数
lambda=a*a*(L+k)-L; % lambda为缩放比列参数，用于降低总的预测误差
%%%%%%%%%%%%%%%%%%%%%% sigma点的权值 %%%%%%%%%%%%%%%%%
Wm=zeros(1,2*L+1);
Wc=zeros(1,2*L+1);
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-a*a+b; % 第一个采样点的均值和协方差的权值
for j=2:2*L+1 % 剩下2n个sigma点的均值和协方差的权值
Wm(j)= lambda/(2*(L+lambda)); % 下标m为均值的权值
Wc(j)= lambda/(2*(L+lambda)); % 下标c为协方差的权值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf = zeros(4,N);
Xukf(:,1)=X(:,1); %无迹Kalman滤波状态初始化
P0=eye(4); %协方差阵初始化
for t=2:N
xestimate = Xukf(:,t-1);
P=P0;
%%%%%%% 第一步:获得一组采样点，Sigma点集 %%%%%%%%%%%%%%
cho=(chol(P*(L+lambda)))'; % cho=chol(X)用于对矩阵X进行Cholesky分解，产生一个上三角阵cho，使cho'cho=X。若X为非对称正定，则输出一个出错信息。
for k=1:L
xgamaP1(:,k)=xestimate+cho(:,k);
xgamaP2(:,k)=xestimate-cho(:,k);
end
Xsigma=[xestimate xgamaP1 xgamaP2]; % 获得 2L+1 个Sigma点
%第二步:对Sigme点集进行一步预测
Xsigmapre=F*Xsigma;
%第三步：利用第二部结果计算均值和协方差
Xpred=zeros(4,1);
for k=1:2*L+1
Xpred=Xpred+Wm(k)*Xsigmapre(:,k);
end
Ppred=zeros(4,4); %协力差阵预测
for k=1:2*L+1
Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';
end
Ppred=Ppred+G*Q*G';
%第四步:根据预测值，再次使用UT变换，得到新的sigma点集
chor=(chol((L+lambda)*Ppred))';
for k=1:L
XaugsigmaP1(:,k)=Xpred+chor(:,k);
XaugsigmaP2(:,k)=Xpred-chor(:,k);
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
%第五步:观测预测
for k=1:2*L+1 %观测预测
Zsigmapre(1,k)=h(Xaugsigma(:,k),Xstation);
end
%第六步:计算观测预测均值和协方差
Zpred=0; %观测预测的均值
for k=1:2*L+1
Zpred=Zpred+ Wm(k)*Zsigmapre(1,k);
end
Pzz=0;
for k=1:2*L+1
Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
end
Pzz=Pzz+R; %得到协方差Pzz
Pxz= zeros(4,1);
for k=1:2*L+1
Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
end
%第七步:计算Kalman增益
K=Pxz/Pzz; % Kalman
%第八步:状态和方差更新
Xukf(:,t)=Xpred+K*(Z(t)-Zpred);% 状态更新
P=Ppred-K*Pzz*K'; %方差更新
P0=P;
end
%6误差分析
Err_KalmanFilter=zeros(2,N);
for i=1:N
Err_KalmanFilter(1,i)=X(1,i)-Xukf(1,i); %滤波后的误差
Err_KalmanFilter(2,i)=X(3,i)-Xukf(3,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),'-B.'); %真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-rs'); %无迹Kalman滤波轨迹
legend('真实轨迹','UKF轨迹')
xlabel('横向坐标-X/m')
ylabel('纵向坐标-Y');
figure
hold on;
box on;
plot(Err_KalmanFilter(1,:),'-kd','MarkerFace','r');
xlabel('时间/s');
ylabel('横向坐标误差/m');
figure
plot(Err_KalmanFilter(2,:),'-bd','MarkerFace','g');
xlabel('时间/s');
ylabel('纵向坐标误差/m');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%子函数:求两点间的距离
function d=Distance(X1,X2)
d=sqrt((X1(1)- X2(1))^2 + (X1(3)-X2(2))^2);
end
%观测子函数:观测距离
function [y]=h(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
end 
