无迹卡尔曼滤波UKF的理解与应用（附Matlab实例）

如有错误，欢迎指正，向大家学习！

1 从卡尔曼滤波说起

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

无迹卡尔曼滤波（Unscented Kalman Filter，UKF），是无损变换（Unscented Transform，UT变换）与标准卡尔曼滤波体系的结合，通过无损变换变换使非线性系统方程适用于线性假设下的标准卡尔曼体系。

也就是说UKF是在KF的基础上加入了UT变换而已。

2 UKF有什么用？

2.1 UKF使用背景

（1）对于某个系统，你拥有准确的数学模型（状态方程和观测方程），也就是说，给出这个系统的输入，你必然能算出这个系统的输出；但是，在现实生活中往往拿到的第一手测试数据并不是你想要的最直观的数据（而且数据还有噪声），这时候使用UKF可以依靠相对准确的数值模型和结合测试数据来得到你想要的信息。（重点讲这个）

举例：某个飞行器在空中做匀速直线运动，你已经推出了其k+1步的位置与k步位置的数学关系（状态方程），但是在现实中你拥有一个雷达，这个雷达只能实时地反馈该飞行器每一步距离雷达的直线距离，但你却想要飞行器的每一步的具体位置（x,y坐标），于是你又建立了雷达与飞行器的每一步距离方程（直接用雷达坐标与飞行器当前步坐标即可求得，观测方程）。UKF的神奇之处在于它允许你用这个雷达实测距离数据来反推得到飞行器的坐标。

（2）数值模型的参数相对不准确，但实测数据可信度更好，这时候使用UKF可以依靠相对准确的实测数据和结合数值模型来还原较真实的参数。

2.2 UKF的核心思想

逼近概率分布要比逼近任意的非线性函数要容易的多，如下图所示。

UT变换图示

2.3 UKF的原理与实现流程

1）首先，你得有状态方程与观测方程

其中，是非线性状态方程函数，即飞行器第步的位置与第步的数学关系；是非线性观测方程函数，即飞行器与雷达在第k步的直线距离关系；和分别是状态方程与观测方程的当前噪声。

2）获得2n+1个采样组合（即Sigma点集）及其权值（n为需要估计的参数数量）

式中, ，表示矩阵方根的第 列，其中 P 指的是当前状态的协方差矩阵（每步会实时更新）

整理成如下顺序：

（向右滑动还有公式）

3）把这2n+1个点代进状态方程(1)，获得了这些点的步预测

4）根据3）得到的2n+1个预测结果，计算系统状态量的步预测均值和协方差矩阵

先算权值：

式中，m表均值，c表协方差，上标为第几个采样点。参数 是一个缩放比例参数，用来降低总的预测误差， 的选取控制了采样点的分布状态， 为待选参数，其具体取值虽然没有界限，但通常需要保证矩阵 为半正定矩阵。待选参数 是一个非负的权系数，它可以合并方程中高阶项的动差，这样就可以把高阶项影响包括在内。*注意：这一段来自书本，这些参数的确定通常有经验公式，但具体怎么来的我目前是不懂的。

得到权值后就代入下式得到系统状态量的步预测均值 和协方差矩阵

（向右滑动还有公式）

5） 根据4）的预测均值 和协方差矩阵，再次使用UT变换，产生新的2n+1个Sigma点集。

（向右滑动还有公式）

6）将5）的点集代入观测方程（式(2)），得到步的预测观测量

7）根据6）得到的2n+1个预测结果，计算系统观测量的步预测均值和协方差矩阵

其中权值还是继续用回4）中的权值

（向右滑动还有公式）

8）计算Kalman增益矩阵

9）最后，更新系统的状态与协方差

*注意： 就是步雷达站实测数据，即步飞行器与雷达站直线实测距离。

2.4 个人对Kalman增益的理解

UKF用UT变换近似逼近了系统非线性变换后的分布，得到的Kalman增益可以类比成：状态变量X的在当前步的线性修正率，有了这个线性修正率再去修正量（实测距离-距离的预测均值，即)，从而更新了步的状态量，实现了追踪。

3 Matlab代码实例

*注意：代码来自书籍

[1]黄小平, 王岩. 卡尔曼滤波原理及应用:MATLAB仿真[M]. 电子工业出版社, 2015.

%%%
% UKF在目标跟踪中的应用
%%%
clc;clear
T=1;
N=60/T;
X=zeros(4,N);%目标真实位置、速度，
X(:,1)=[-100,2,200,20];
Z=zeros(1,N);
delta_w=1e-3;
Q=delta_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];%状态转移矩阵
x0=200;%观测站位置
y0=300;
Xstation=[x0,y0];%雷达站位置
%%%%%%%%%%%%%%
v=sqrtm(R)*randn(1,N);
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹，含噪声
end
for t=1:N
    Z(t)=Dist(X(:,t),Xstation)+v(t);%对目标观测，目标与雷达站的距离，含噪声
end
%%%%%%%%%%%%%
%UKF滤波，UT变换
L=4;%4个变量
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2*L+1
    Wm(j)=1/(2*(L+ramda));
    Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定
%%%%%%%%%%%%%%%%
Xukf=zeros(4,N);
Xukf(:,1)=X(:,1);%把X真值的初次数据赋给Xukf
P0=eye(4);%协方差阵初始化
for t=2:N
    xestimate=Xukf(:,t-1);%获取上一步的UKF点
    P=P0;%协方差阵
    %第一步：获得一组Sigma点集
    cho=(chol(P*(L+ramda)))';
    for k=1:L
        xgamaP1(:,k)=xestimate+cho(:,k);
        xgamaP2(:,k)=xestimate-cho(:,k);
    end
    Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点
    %第二步：对Sigma点集进行一步预测
    Xsigmapre=F*Xsigma;%F是状态转移矩阵
    %第三步：利用第二步的结果计算均值和协方差
    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+ramda)*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)=hfun(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;
    
    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*inv(Pzz);
    %第八步：状态和方差更新
    xestimate=Xpred+K*(Z(t)-Zpred);
    P=Ppred-K*Pzz*K';
    P0=P;
    Xukf(:,t)=xestimate;
end

%误差分析
for i=1:N
    Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));
end
%%%%%%%%%%%
%画图
figure
hold on ;box on
plot(X(1,:),X(3,:),'-k.');
plot(Xukf(1,:),Xukf(3,:),'-r+');
legend('真实轨迹','UKF轨迹')
xlabel('x坐标/m')
ylabel('y坐标/m')
figure
hold on ; box on
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
xlabel('时间/s')
ylabel('位置估计偏差RMS/m')
%%%%%%%%%%%%%
%子函数

function d=Dist(X1,X2)
if length(X2)<=2
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
end
function [y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
%%%%%%%%%%%%%


基于UKF算法的跟踪轨迹图

UKF算法误差曲线图