UKF和EKF算法在非线性系统中的应用比较

参考内容：书籍《卡尔曼滤波原理及应用------matlab仿真》这本书对kalman算法的解析很清晰，MATLAB程序很全，适合初学者（如有侵权，请联系删除(qq:1491967912)）

之前学习了EKF算法和UKF算法，接下来对于两种算法在目标跟踪中的效果进行一个比较。那么首先我们先大致回顾一下EKF算法和UKF算法，具体内容可参考扩展KALMAN滤波（EKF）基本知识和相应的MATLAB仿真和无迹KALMAN滤波原理、算法实现：

1、EKF和UKF基础知识回顾

1）EKF算法：

 　　EKF滤波算法是建立在KF滤波算法的基础上，核心思想是，对于非线性系统，首先对滤波值的非线性函数展开成泰勒级数但只保存一阶及以下部分（舍去二阶和高阶部分），得到近似的线性化模型。然后就是利用KF算法完成对目标的滤波估计等处理。

       EKF的优点是不需提前计算标准轨迹（过程噪声W（k）与观测噪声V（k）为0时的非线性方程的解），但它的缺陷是只能在滤波误差和一步预测误差较小的时候才能使用。

EKF算法存在的问题

　　EKF算法对非线性的系统方程或者观测方程及进行泰勒展开并保留其一阶项，由于仅保留了一阶项，将引入一些线性误差。另外当线性化假设不成立时，该方法会造成跟踪性能下降，最终可能导致跟踪发散。除此之外，通常计算系统的状态方程和观测方程的雅可比矩阵是一个很复杂的问题，这样会增加算法的复杂度。不利于实际的应用。

2）UKF算法：

　　无迹kalman滤波（UKF）不是采用的将非线性函数线性化的做法。无迹kalman仍然采用的是线性kalman滤波的架构，对于一步预测方程，使用无迹变换（UT）来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对雅可比矩阵进行求导。UKF没有高阶项忽略，因此对于非线性分布的统计量有较高的精度，有效克服了扩展Kalman滤波的估计精度低、稳定性差的缺陷。

2、UKF和EKF算法的应用比较

　　为了方便比较选用一维非线性系统，

 

　　该系统中，过程噪声W（k），他的方差为Q，观测方程的噪声为V(k),其方差为R。

　　在EKF中，对状态方程和观测方程要进行线性化求雅可比矩阵的方法如下：

 

 

　　在UKF的UT变换中由于是一维的所以维数为1，取相应的数值具体数值和含义请参考无迹KALMAN滤波原理、算法实现

，来进一步做系统仿真。

　　为了更好的进行对比，我们取真实值和估计值之间的距离误差做对比。具体代码如下。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序说明：对比UKF与EKF在非线性系统中应用的算法性能
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear all
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=100;
L=1;
Q=16;             %状态噪声均方值
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);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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);
end

Xukf=zeros(L,N);    %UKF初始化
Xukf(:,1)=X(:,1);
Pukf=eye(L);        %ukf协方差矩阵初始化
Xekf=zeros(L,N);    %EKF初始化
Xekf(:,1)=X(:,1);
Pekf=eye(L);        %ekf协方差矩阵初始化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%                  EKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N   
    Xpre=0.5*Xekf(:,k-1)+2.5*Xekf(:,k-1)/(1+Xekf(:,k-1)^2)+8*cos(1.2*k);    %状态预测
    F=[0.5+(2.5*(1+Xpre^2)-2.5*Xpre*2*Xpre)/(1+Xpre^2)^2];   %雅可比矩阵处理后的观测矩阵
    Ppre=F*Pekf*F'+Q;               %协方差矩阵预测      
    Zpre=Xpre^2/20;                 %观测预测
    H=[Xpre/10];                    %雅可比矩阵处理后的观测矩阵
    K=Ppre*H'/(H*Ppre*H'+R);        %卡尔曼增益
    Xekf(:,k)=Xpre+K*(Z(k)-Zpre);   %EKF状态更新
    Pekf=(eye(1)-K*H)*Ppre;         %EKF协方差矩阵更新
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
%                  UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%UKF参数配置
L=1;
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);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;
for k=2:N
    xestimate= Xukf(:,k-1);
    P=Pukf;
    %第一步：获得一组sigma点集
    cho=(chol(P*(L+ramda)))';
    for j=1:L
        xgamaP1(:,j)=xestimate+cho(:,j);
        xgamaP2(:,j)=xestimate-cho(:,j);
    end
    Xsigma=[xestimate,xgamaP1,xgamaP2];
    %第二步：对sigma点集进一步预测
    for j=1:2*L+1
        Xsigmapre(:,j)=0.5*Xsigma(:,j)+2.5*Xsigma(:,j)/(1+Xsigma(:,j)^2)+8*cos(1.2*k);
    end
    Xpred=0;
    for j=1:2*L+1
        Xpred=Xpred+Wm(j)*Xsigmapre(:,j);
    end
    Ppred=0;
    for j=1:2*L+1
        Ppred=Ppred+Wc(j)*(Xsigmapre(:,j)-Xpred)*(Xsigmapre(:,j)-Xpred)';
    end
    Ppred=Ppred+Q;
    %第四步：根据预测值，再一次使用UT变换，得到新的sigma点集
    chor=(chol((L+ramda)*Ppred))';
    for j=1:L
        XaugsigmaP1(:,j)=Xpred+chor(:,j);
        XaugsigmaP2(:,j)=Xpred-chor(:,j);
    end
    Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
    %第五步：观测预测
    for j=1:2*L+1
        Zsigmapre(1,j)=Xaugsigma(:,j)^2/20;
    end
    %第六步：计算观测预测均值和协方差
    Zpred=0;
    for j=1:2*L+1
        Zpred=Zpred+Wm(j)*Zsigmapre(1,j);
    end
    Pzz=0;
    for j=1:2*L+1
        Pzz=Pzz+Wc(j)*(Zsigmapre(1,j)-Zpred)*(Zsigmapre(1,j)-Zpred)';
    end
    Pzz=Pzz+R;
    
    Pxz=0;
    for j=1:2*L+1
        Pxz=Pxz+Wc(j)*(Xaugsigma(:,j)-Xpred)*(Zsigmapre(1,j)-Zpred)';
    end
    %第七步：计算kalman增益
    K=Pxz*inv(Pzz);
    %第八步：状态和协方差更新
    xestimate=Xpred+K*(Z(k)-Zpred);
    P=Ppred-K*Pzz*K';
    Pukf=P;
    %第九步：状态更新
    Xukf(:,k)=xestimate;
%    [Xukf(:,k),Pukf]=ukf1(Xukf(:,k-1),Pukf,Z(k),Q,R,k);
end

err_ekf=zeros(1,N);
err_ukf=zeros(1,N);
for k=1:N
    err_ekf(k)=abs(Xekf(1,k)-X(1,k));
    err_ukf(k)=abs(Xukf(1,k)-X(1,k));
end
XX=X-W;
err_ave_ekf=sum(err_ekf)/N
err_ave_ukf=sum(err_ukf)/N
figure
hold on;box on;
plot(X,'-r*');
plot(Xekf,'-ko');
plot(Xukf,'-b+');
legend('真实状态','EKF估计','UKF估计')
xlabel('时间k/s')
ylabel('状态值')
figure
hold on;box on;
plot(err_ekf,'-ro');
plot(err_ukf,'-b+');
xlabel('时间k/s')
ylabel('偏差绝对值')
legend('EKF估计','UKF估计')

 

　　

 

 

 　　从上图结果来看1~100时间点内，UKF的结果整体上要比EKF算法的估计值更准确。另外从下图误差平均值可以更直观的看出UKF的估计偏差会比EKF小。

 

 

 3、总结

　　UKF算法以贝叶斯理论和UT变换为基础，在很大程度上克服了算法的线性化误差。但UKF算法最大的缺点在于其参数的选择问题，并且UKF的滤波效果与EKF算法一样也受到滤波初值的影响。