扩展滤波和无迹卡尔曼滤波优秀PPT.ppt

《扩展滤波和无迹卡尔曼滤波优秀PPT.ppt》由会员分享，可在线阅读，更多相关《扩展滤波和无迹卡尔曼滤波优秀PPT.ppt（28页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、扩展滤波和无迹卡尔曼滤波你现在浏览的是第一页，共28页一、背景一、背景 普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标的动态估计，适应于过程和测量都属于线性系统过程和测量都属于线性系统，且误差符合高斯分误差符合高斯分布布的系统。但是实际上很多系统都存在一定的非线性，表现在过程方过程方程程 （状态方程）是非线性（状态方程）是非线性的，或者观测与状态之间的关系（测量方程）观测与状态之间的关系（测量方程）是非线性的是非线性的。这种情况下就不能使用一般的卡尔曼滤波了。解决的方法是将非线性关系进行线性线性近似，将其转化成线性问题。对于非线性问题线性化常用的两大途径：（1）将非线性环节线性化

2、，对高阶项采用忽略或逼近措施；（EKFEKF）（2）用采样方法近似非线性分布.(UKFUKF)你现在浏览的是第二页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法 l EKF算法是一种近似方法，它将非线性模型在状态估计值附近作泰勒级数展开，并在一阶截断，用得到的一阶近似项作为原状态方程和测量方程近似表达形式，从而实现线性化同时假定线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。采用局部线性化技术，能得到问题局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点的选择。你现在浏览的是第三页，共28页二、扩展

3、二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法l假定定位跟踪问题的非线性状态方程和测量方程如下:l 在最近一次状态估计的时刻，对以上两式进行线性化处理，首先构造如下2个矩阵:你现在浏览的是第四页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法l 将线性化后的状态转移矩阵和观测矩阵代入到标准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。l 因为EKF忽略了非线性函数泰勒展开的高阶项，仅仅用了一阶项，是非线性函数在局部线性化的结果，这就给估计带来了很大误差，所以只有当系统的状态方程和观测方程都接近线性且连续时，EKF的滤波结果才有可能接近真实值

4、。EKF滤波结果的好坏还与状态噪声和观测噪声的统计特性有关，在EKF的递推滤波过程中，状态噪声和观测噪声的协方差矩阵保持不变，如果这两个噪声协方差矩阵估计的不够准确，那就容易产生误差累计，导致滤波器发散。EKF的另外一个缺点是初始状态不太好确定，如果假设的初始状态和初始协方差误差较大，也容易导致滤波器发散。你现在浏览的是第五页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法lMatlab程序：lfunction test_ekfl kx=.01;ky=.05;%阻尼系数l g=9.8;%重力l t=10;%仿真时间l Ts=0.1;%采样周期l len=f

5、ix(t/Ts);%仿真步数l%真实轨迹模拟l dax=1.5;day=1.5;%系统噪声l X=zeros(len,4);X(1,:)=0,50,500,0;%状态模拟的初值l for k=2:lenlx=X(k-1,1);vx=X(k-1,2);y=X(k-1,3);vy=X(k-1,4);l x=x+vx*Ts;l vx=vx+(-kx*vx2+dax*randn(1,1)*Ts;l y=y+vy*Ts;你现在浏览的是第六页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法lvy=vy+(ky*vy2-g+day*randn(1)*Ts;l X(k,:

6、)=x,vx,y,vy;l endl figure(1),hold off,plot(X(:,1),X(:,3),-b),grid onl%figure(2),plot(X(:,2:2:4)l%构造量测量l mrad=0.001;l dr=10;dafa=10*mrad;%量测噪声l for k=1:lenl r=sqrt(X(k,1)2+X(k,3)2)+dr*randn(1,1);l a=atan(X(k,1)/X(k,3)+dafa*randn(1,1);l Z(k,:)=r,a;l end你现在浏览的是第七页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)

7、算法算法l figure(1),hold on,plot(Z(:,1).*sin(Z(:,2),Z(:,1).*cos(Z(:,2),*)l%ekf 滤波l Qk=diag(0;dax;0;day)2;l Rk=diag(dr;dafa)2;l Xk=zeros(4,1);l Pk=100*eye(4);l X_est=X;l for k=1:lenl Ft=JacobianF(X(k,:),kx,ky,g);l Hk=JacobianH(X(k,:);l fX=fff(X(k,:),kx,ky,g,Ts);l hfX=hhh(fX,Ts);l Xk,Pk,Kk=ekf(eye(4)+Ft*T

8、s,Qk,fX,Pk,Hk,Rk,Z(k,:)-hfX);l X_est(k,:)=Xk;l end你现在浏览的是第八页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法l figure(1),plot(X_est(:,1),X_est(:,3),+r)l xlabel(X);ylabel(Y);title(ekf simulation);l legend(real,measurement,ekf estimated);ll%子程序%lfunction F=JacobianF(X,kx,ky,g)%系统状态雅可比函数l vx=X(2);vy=X(4);l F

9、=zeros(4,4);l F(1,F(2,2)=-2*kx*vx;l F(3,4)=1;l F(4,4)=2*ky*vy;l 2)=1;你现在浏览的是第九页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法lfunction H=JacobianH(X)%量测雅可比函数l x=X(1);y=X(3);l H=zeros(2,4);l r=sqrt(x2+y2);l H(1,1)=1/r;H(1,3)=1/r;l xy2=1+(x/y)2;l H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/y2);lfunction fX=fff(X

10、,kx,ky,g,Ts)%系统状态非线性函数l x=X(1);vx=X(2);y=X(3);vy=X(4);l x1=x+vx*Ts;l vx1=vx+(-kx*vx2)*Ts;l y1=y+vy*Ts;l vy1=vy+(ky*vy2-g)*Ts;l fX=x1;vx1;y1;vy1;你现在浏览的是第十页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法lfunction hfX=hhh(fX,Ts)%量测非线性函数l x=fX(1);y=fX(3);l r=sqrt(x2+y2);l a=atan(x/y);l hfX=r;a;llfunction Xk

11、,Pk,Kk=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf 滤波函数l Pkk_1=Phikk_1*Pk_1*Phikk_1+Qk;l Pxz=Pkk_1*Hk;Pzz=Hk*Pxz+Rk;Kk=Pxz*Pzz-1;l Xk=fXk_1+Kk*Zk_hfX;lPk=Pkk_1-Kk*Pzz*Kk;你现在浏览的是第十一页，共28页二、扩展二、扩展KalmanKalman滤波滤波(EKF)(EKF)算法算法l图2 仿真结果你现在浏览的是第十二页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)l 为了改善对非线性问题进行滤波的效果，

12、Julier 等人提出了采用基于unscented变换的UKF方法UKF不是和EKF一样去近似非线性模型，而是对后验概率密度进行近似来得到次优的滤波算法。l UKF算法的核心是UT变换，UT是一种计算非线性变换中的随机变量的统计特征的新方法，是UKF的基础。你现在浏览的是第十三页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)l 假设n维随机向量 ,x通过非线性函数y=f(x)变换后得到n维的随机变量y。通过UT变换可以以较高的精度和较低的计算复杂度求得y的均值 和方差 。UT的具体过程可描述如下:l(1)计算2n+1个Sigma点及其权值:你现在浏览的是第十四页，共

13、28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)l式中:,决定Sigma点的散布程度，通常取一小的正值,k通常取0;用来描述x的分布信息了高斯情况下，的最优值为2);l 为矩阵平方根第i列；为均值的权值，为方差的权值。l l(2)计算Sigma点通过非线性函数 的传播结果:l从而可知：你现在浏览的是第十五页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)由于x的均值和方差都精确到二阶，计算得到y的均值和方差也精确到二阶，比线性化模型精度更高。在卡尔曼框架内应用UT技术就得到了UKF算法。你现在浏览的是第十六页，共28页三、无迹卡尔曼滤波算法三、无

14、迹卡尔曼滤波算法(UKF)(UKF)l UKF是用确定的采样来近似状态的后验PDF,可以有效解决由系统非线性的加剧而引起的滤波发散问题，但UKF仍是用高斯分布来逼近系统状态的后验概率密度，所以在系统状态的后验概率密度是非高斯的情况下，滤波结果将有极大的误差。你现在浏览的是第十七页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)lMatlab程序：l%-function UKFmain%-清屏-close all;clear all;clc;tic;global Qf n;%定义全局变量%-初始化-stater0=220;1;55;-0.5;%标准系统初值state0=

15、200;1.3;50;-0.3;%测量状态初值%-系统滤波初始化p=0.005 0 0 0;0 0.005 0 0;0 0 0.005 0;0 0 0 0.005;%状态误差协方差初值 n=4;T=3;Qf=T2/2 0;0 T;T2/2 0;0 T;%-stater=stater0;state=state0;xc=state;staterout=;stateout=;xcout=;errorout=;tout=;t0=1;h=1;tf=1000;%仿真时间设置你现在浏览的是第十八页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)%-滤波算法-for t=t0:h:t

16、f state,stater,yc=track(state,stater);%轨迹发生器：标准轨迹和输出 xc,p=UKFfiter(systemfun,measurefun,xc,yc,p);error=xc-stater;%滤波处理后的误差 staterout=staterout,stater;stateout=stateout,state;errorout=errorout,error;xcout=xcout,xc;tout=tout,t;end%-状态信息图像-figure;plot(tout,xcout(1,:),r,tout,staterout(1,:),g,.tout,state

17、out(1,:),black);legend(滤波后,真实值,无滤波);grid on;xlabel(时间 t（s）);ylabel(系统状态A);title(无迹卡尔曼滤波);figure;你现在浏览的是第十九页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)lplot(tout,xcout(2,:),r,tout,staterout(2,:),g,.tout,stateout(2,:),black);grid on;legend(滤波后,真实值,无滤波);xlabel(时间 t（s）);ylabel(系统状态B);title(无迹卡尔曼滤波);figure;plo

18、t(tout,xcout(3,:),r,tout,staterout(3,:),g,.tout,stateout(3,:),black);grid on;legend(滤波后,真实值,无滤波);xlabel(时间 t（s）);ylabel(系统状态C);title(无迹卡尔曼滤波);figure;plot(tout,xcout(4,:),r,tout,staterout(4,:),g,.tout,stateout(4,:),black);你现在浏览的是第二十页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)lgrid on;legend(滤波后,真实值,无滤波);xl

19、abel(时间 t（s）);ylabel(系统状态D);title(无迹卡尔曼滤波);figure;plot(tout,errorout(1,:),r,tout,errorout(2,:),g,.tout,errorout(3,:),black,tout,errorout(4,:),b);grid on;legend(A,B,C,D);xlabel(时间 t（s）);ylabel(滤波后的状态误差);title(无迹卡尔曼滤波误差);%-toc;%计算仿真程序运行时间end)你现在浏览的是第二十一页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)lfunction s

20、tate,stater,yout=track(state0,stater0%-%轨迹发生函数%-T=3;F=1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1;G=T2/2 0;0 T;T2/2 0;0 T;V=0.005*randn(2,1);W=0.008*randn(1,1);state=F*state0+G*V;stater=F*stater0;yout=atan(stater0(3)/stater0(1)+W;%用真实值得到测量值，在滤波时结果才会与真实值重合。endfunctionstate=systemfun(state0)%-%系统方程%-T=3;F=1 T 0 0

21、;0 1 0 0;0 0 1 T;0 0 0 1;state=F*state0;end你现在浏览的是第二十二页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)lfunctionyout=measurefun(state0)%-%测量方程%-yout=atan(state0(3)/state0(1);endfunction xc,p=UKFfiter(systemfun,measurefun,xc0,yc,p0)%-%此程序用于描述UKF（无迹kalman滤波）算法%-global Qf n;%-参数注解-%xc0-状态初值(列向量)yc-系统测量值%p0-状态误差协方

22、差 n-系统状态量数%systemfun-系统方程 measurefun-测量方程%-滤波初始化-alp=1;%default,tunablekap=-1;%default,tunablebeta=2;%default,你现在浏览的是第二十三页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)ltunablelamda=alp2*(n+kap)-n;%scaling factornc=n+lamda;%scaling factorWm=lamda/nc 0.5/nc+zeros(1,2*n);%weights for meansWc=Wm;Wc(1)=Wc(1)+(1-

23、alp2+beta);%weights for covariancens=sqrt(nc);%-sxk=0;spk=0;syk=0;pyy=0;pxy=0;p=p0;%-构造sigma点-pk=ns*chol(p);%B=chol(A);meant:A*A=B;sigma=xc0;for k=1:2*n if(k=n)sigma=sigma,xc0+pk(:,k);else sigma=sigma,xc0-pk(:,k-n);endend你现在浏览的是第二十四页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)l%-时间传播方程-for ks=1:2*n+1 sigma

24、(:,ks)=systemfun(sigma(:,ks);%利用系统方程对状态预测 sxk=Wm(ks)*sigma(:,ks)+sxk;end%-完成对Pk的估计for kp=1:2*n+1 spk=Wc(kp)*(sigma(:,kp)-sxk)*(sigma(:,kp)-sxk)+spk;end spk=spk+Qf*0.005*Qf;%-for kg=1:2*n+1 gamma(kg)=measurefun(sigma(:,kg);endfor ky=1:2*n+1 syk=syk+Wm(ky)*gamma(ky);end你现在浏览的是第二十五页，共28页三、无迹卡尔曼滤波算法三、无迹

25、卡尔曼滤波算法(UKF)(UKF)l%-测量更新方程-for kpy=1:2*n+1 pyy=Wc(kpy)*(gamma(kpy)-syk)*(gamma(kpy)-syk)+pyy;end pyy=pyy+0.008;for kxy=1:2*n+1 pxy=Wc(kxy)*(sigma(:,kxy)-sxk)*(gamma(kxy)-syk)+pxy;end kgs=pxy/pyy;%修正系数 xc=sxk+kgs*(yc-syk);%测量信息修正状态 p=spk-kgs*pyy*kgs;%误差协方差阵更新%-end你现在浏览的是第二十六页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)你现在浏览的是第二十七页，共28页三、无迹卡尔曼滤波算法三、无迹卡尔曼滤波算法(UKF)(UKF)你现在浏览的是第二十八页，共28页