擴(kuò)展濾波和無(wú)跡卡爾曼濾波

擴(kuò)展濾波和無(wú)跡卡爾曼濾波第1頁(yè)，共28頁(yè)，2023年，2月20日，星期一一、背景普通卡爾曼濾波是在線性高斯情況下利用最小均方誤差準(zhǔn)則獲得目標(biāo)的動(dòng)態(tài)估計(jì)，適應(yīng)于過(guò)程和測(cè)量都屬于線性系統(tǒng)，且誤差符合高斯分布的系統(tǒng)。但是實(shí)際上很多系統(tǒng)都存在一定的非線性，表現(xiàn)在過(guò)程方程（狀態(tài)方程）是非線性的，或者觀測(cè)與狀態(tài)之間的關(guān)系（測(cè)量方程）是非線性的。這種情況下就不能使用一般的卡爾曼濾波了。解決的方法是將非線性關(guān)系進(jìn)行線性近似，將其轉(zhuǎn)化成線性問(wèn)題。對(duì)于非線性問(wèn)題線性化常用的兩大途徑：（1）

將非線性環(huán)節(jié)線性化，對(duì)高階項(xiàng)采用忽略或逼近措施；（EKF）（2）用采樣方法近似非線性分布.(UKF)第2頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法EKF算法是一種近似方法，它將非線性模型在狀態(tài)估計(jì)值附近作泰勒級(jí)數(shù)展開，并在一階截?cái)?，用得到的一階近似項(xiàng)作為原狀態(tài)方程和測(cè)量方程近似表達(dá)形式，從而實(shí)現(xiàn)線性化同時(shí)假定線性化后的狀態(tài)依然服從高斯分布，然后對(duì)線性化后的系統(tǒng)采用標(biāo)準(zhǔn)卡爾曼濾波獲得狀態(tài)估計(jì)。采用局部線性化技術(shù)，能得到問(wèn)題局部最優(yōu)解，但它能否收斂于全局最優(yōu)解，取決于函數(shù)的非線性強(qiáng)度以及展開點(diǎn)的選擇。第3頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法假定定位跟蹤問(wèn)題的非線性狀態(tài)方程和測(cè)量方程如下:

在最近一次狀態(tài)估計(jì)的時(shí)刻，對(duì)以上兩式進(jìn)行線性化處理，首先構(gòu)造如下2個(gè)矩陣:第4頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法

將線性化后的狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣代入到標(biāo)準(zhǔn)卡爾曼濾波框架中，即得到擴(kuò)展卡爾曼濾波。

因?yàn)镋KF忽略了非線性函數(shù)泰勒展開的高階項(xiàng)，僅僅用了一階項(xiàng)，是非線性函數(shù)在局部線性化的結(jié)果，這就給估計(jì)帶來(lái)了很大誤差，所以只有當(dāng)系統(tǒng)的狀態(tài)方程和觀測(cè)方程都接近線性且連續(xù)時(shí)，EKF的濾波結(jié)果才有可能接近真實(shí)值。EKF濾波結(jié)果的好壞還與狀態(tài)噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性有關(guān)，在EKF的遞推濾波過(guò)程中，狀態(tài)噪聲和觀測(cè)噪聲的協(xié)方差矩陣保持不變，如果這兩個(gè)噪聲協(xié)方差矩陣估計(jì)的不夠準(zhǔn)確，那就容易產(chǎn)生誤差累計(jì)，導(dǎo)致濾波器發(fā)散。EKF的另外一個(gè)缺點(diǎn)是初始狀態(tài)不太好確定，如果假設(shè)的初始狀態(tài)和初始協(xié)方差誤差較大，也容易導(dǎo)致濾波器發(fā)散。第5頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法Matlab程序：functiontest_ekfkx=.01;ky=.05;%阻尼系數(shù)g=9.8;%重力t=10;%仿真時(shí)間Ts=0.1;%采樣周期len=fix(t/Ts);%仿真步數(shù)%真實(shí)軌跡模擬dax=1.5;day=1.5;%系統(tǒng)噪聲X=zeros(len,4);X(1,:)=[0,50,500,0];%狀態(tài)模擬的初值fork=2:lenx=X(k-1,1);vx=X(k-1,2);y=X(k-1,3);vy=X(k-1,4);x=x+vx*Ts;vx=vx+(-kx*vx^2+dax*randn(1,1))*Ts;y=y+vy*Ts;第6頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法vy=vy+(ky*vy^2-g+day*randn(1))*Ts;X(k,:)=[x,vx,y,vy];endfigure(1),holdoff,plot(X(:,1),X(:,3),'-b'),gridon%figure(2),plot(X(:,2:2:4))%構(gòu)造量測(cè)量mrad=0.001;dr=10;dafa=10*mrad;%量測(cè)噪聲fork=1:lenr=sqrt(X(k,1)^2+X(k,3)^2)+dr*randn(1,1);a=atan(X(k,1)/X(k,3))+dafa*randn(1,1);Z(k,:)=[r,a];end第7頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法figure(1),holdon,plot(Z(:,1).*sin(Z(:,2)),Z(:,1).*cos(Z(:,2)),'*')%ekf濾波Qk=diag([0;dax;0;day])^2;Rk=diag([dr;dafa])^2;Xk=zeros(4,1);Pk=100*eye(4);X_est=X;fork=1:lenFt=JacobianF(X(k,:),kx,ky,g);Hk=JacobianH(X(k,:));fX=fff(X(k,:),kx,ky,g,Ts);hfX=hhh(fX,Ts);[Xk,Pk,Kk]=ekf(eye(4)+Ft*Ts,Qk,fX,Pk,Hk,Rk,Z(k,:)'-hfX);X_est(k,:)=Xk';end第8頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法figure(1),plot(X_est(:,1),X_est(:,3),'+r')xlabel('X');ylabel('Y');title('ekfsimulation');legend('real','measurement','ekfestimated');

%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%functionF=JacobianF(X,kx,ky,g)%系統(tǒng)狀態(tài)雅可比函數(shù)vx=X(2);vy=X(4);F=zeros(4,4);F(1,F(2,2)=-2*kx*vx;F(3,4)=1;F(4,4)=2*ky*vy;2)=1;第9頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法functionH=JacobianH(X)%量測(cè)雅可比函數(shù)x=X(1);y=X(3);H=zeros(2,4);r=sqrt(x^2+y^2);H(1,1)=1/r;H(1,3)=1/r;xy2=1+(x/y)^2;H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/y^2);functionfX=fff(X,kx,ky,g,Ts)%系統(tǒng)狀態(tài)非線性函數(shù)x=X(1);vx=X(2);y=X(3);vy=X(4);x1=x+vx*Ts;vx1=vx+(-kx*vx^2)*Ts;y1=y+vy*Ts;vy1=vy+(ky*vy^2-g)*Ts;fX=[x1;vx1;y1;vy1];第10頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法functionhfX=hhh(fX,Ts)%量測(cè)非線性函數(shù)x=fX(1);y=fX(3);r=sqrt(x^2+y^2);a=atan(x/y);hfX=[r;a];

function[Xk,Pk,Kk]=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf濾波函數(shù)Pkk_1=Phikk_1*Pk_1*Phikk_1'+Qk;Pxz=Pkk_1*Hk';Pzz=Hk*Pxz+Rk;Kk=Pxz*Pzz^-1;Xk=fXk_1+Kk*Zk_hfX;Pk=Pkk_1-Kk*Pzz*Kk';第11頁(yè)，共28頁(yè)，2023年，2月20日，星期一二、擴(kuò)展Kalman濾波(EKF)算法圖2仿真結(jié)果第12頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)

為了改善對(duì)非線性問(wèn)題進(jìn)行濾波的效果，Julier等人提出了采用基于unscented變換的UKF方法UKF不是和EKF一樣去近似非線性模型，而是對(duì)后驗(yàn)概率密度進(jìn)行近似來(lái)得到次優(yōu)的濾波算法。UKF算法的核心是UT變換，UT是一種計(jì)算非線性變換中的隨機(jī)變量的統(tǒng)計(jì)特征的新方法，是UKF的基礎(chǔ)。第13頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)

假設(shè)n維隨機(jī)向量,x通過(guò)非線性函數(shù)y=f(x)變換后得到n維的隨機(jī)變量y。通過(guò)UT變換可以以較高的精度和較低的計(jì)算復(fù)雜度求得y的均值和方差。UT的具體過(guò)程可描述如下:(1)計(jì)算2n+1個(gè)Sigma點(diǎn)及其權(quán)值:第14頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)式中:,決定Sigma點(diǎn)的散布程度，通常取一小的正值,k通常取0;用來(lái)描述x的分布信息了高斯情況下，的最優(yōu)值為2);

為矩陣平方根第i列；為均值的權(quán)值，為方差的權(quán)值。

(2)計(jì)算Sigma點(diǎn)通過(guò)非線性函數(shù)的傳播結(jié)果:從而可知：第15頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)

由于x的均值和方差都精確到二階，計(jì)算得到y(tǒng)的均值和方差也精確到二階，比線性化模型精度更高。在卡爾曼框架內(nèi)應(yīng)用UT技術(shù)就得到了UKF算法。第16頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)UKF是用確定的采樣來(lái)近似狀態(tài)的后驗(yàn)PDF,可以有效解決由系統(tǒng)非線性的加劇而引起的濾波發(fā)散問(wèn)題，但UKF仍是用高斯分布來(lái)逼近系統(tǒng)狀態(tài)的后驗(yàn)概率密度，所以在系統(tǒng)狀態(tài)的后驗(yàn)概率密度是非高斯的情況下，濾波結(jié)果將有極大的誤差。第17頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)Matlab程序：%---------------------------------------

functionUKFmain

%------------------清屏----------------

closeall;clearall;

clc;tic;

globalQfn;

%定義全局變量

%------------------初始化--------------

stater0=[220;1;55;-0.5];

%標(biāo)準(zhǔn)系統(tǒng)初值

state0=[200;1.3;50;-0.3];

%測(cè)量狀態(tài)初值

%--------系統(tǒng)濾波初始化

p=[0.005000;00.00500;

000.0050;0000.005];

%狀態(tài)誤差協(xié)方差初值

n=4;T=3;

Qf=[T^2/20;0T;T^2/20;0T];

%--------------------------------------

stater=stater0;state=state0;xc=state;

staterout=[];stateout=[];xcout=[];

errorout=[];tout=[];

t0=1;h=1;tf=1000;

%仿真時(shí)間設(shè)置第18頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)%---------------濾波算法----------------

fort=t0:h:tf

[state,stater,yc]=track(state,stater);%軌跡發(fā)生器：標(biāo)準(zhǔn)軌跡和輸出

[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

%---------------狀態(tài)信息圖像---------------

figure;

plot(tout,xcout(1,:),'r',tout,staterout(1,:),'g',...

tout,stateout(1,:),'black');

legend('濾波后','真實(shí)值','無(wú)濾波');

gridon;

xlabel('時(shí)間t（s）');

ylabel('系統(tǒng)狀態(tài)A');

title('無(wú)跡卡爾曼濾波');

figure;

第19頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)plot(tout,xcout(2,:),'r',tout,staterout(2,:),'g',...

tout,stateout(2,:),'black');

gridon;

legend('濾波后','真實(shí)值','無(wú)濾波');

xlabel('時(shí)間t（s）');

ylabel('系統(tǒng)狀態(tài)B');

title('無(wú)跡卡爾曼濾波');

figure;

plot(tout,xcout(3,:),'r',tout,staterout(3,:),'g',...

tout,stateout(3,:),'black');

gridon;

legend('濾波后','真實(shí)值','無(wú)濾波');

xlabel('時(shí)間t（s）');

ylabel('系統(tǒng)狀態(tài)C');

title('無(wú)跡卡爾曼濾波');

figure;

plot(tout,xcout(4,:),'r',tout,staterout(4,:),'g',...

tout,stateout(4,:),'black');第20頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)

gridon;

legend('濾波后','真實(shí)值','無(wú)濾波');

xlabel('時(shí)間t（s）');

ylabel('系統(tǒng)狀態(tài)D');

title('無(wú)跡卡爾曼濾波');

figure;

plot(tout,errorout(1,:),'r',tout,errorout(2,:),'g',...

tout,errorout(3,:),'black',tout,errorout(4,:),'b');

gridon;

legend('A','B','C','D');

xlabel('時(shí)間t（s）');

ylabel('濾波后的狀態(tài)誤差');

title('無(wú)跡卡爾曼濾波誤差');

%---------------------------------------------

toc;

%計(jì)算仿真程序運(yùn)行時(shí)間

end

)

第21頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)function[state,stater,yout]=track(state0,stater0%-----------------------------------

%軌跡發(fā)生函數(shù)

%-----------------------------------

T=3;

F=[1T00;0100;001T;0001];

G=[T^2/20;0T;T^2/20;0T];

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;

%用真實(shí)值得到測(cè)量值，在濾波時(shí)結(jié)果才會(huì)與真實(shí)值重合。

end

function

state=systemfun(state0)

%-------------------------

%系統(tǒng)方程

%-------------------------

T=3;

F=[1T00;0100;001T;0001];

state=F*state0;

end

第22頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)function

yout=measurefun(state0)

%----------------------------

%測(cè)量方程

%----------------------------

yout=atan(state0(3)/state0(1));

end

function[xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0)

%------------------------------------------

%此程序用于描述UKF（無(wú)跡kalman濾波）算法

%------------------------------------------

globalQfn;

%----------------參數(shù)注解-------------------

%xc0---狀態(tài)初值(列向量)

yc---系統(tǒng)測(cè)量值

%p0----狀態(tài)誤差協(xié)方差

n----系統(tǒng)狀態(tài)量數(shù)

%systemfun---系統(tǒng)方程measurefun--測(cè)量方程

%---------------濾波初始化------------------

alp=1;

%default,tunable

kap=-1;

%default,tunable

beta=2;

%default,第23頁(yè)，共28頁(yè)，2023年，2月20日，星期一三、無(wú)跡卡爾曼濾波算法(UKF)tunable

lamda=alp^2*(n+kap)-n;

%scalingfactor

nc=n+lamda;

%scalingfactor

Wm=[lamda/nc0.5/nc+zeros(1,2*n)];

%weightsformeans

Wc=Wm;

Wc(1)=Wc(1)+(1-alp^2+beta);

%weightsforcovariance

ns=sqrt(nc);

%-------------------------------------------

sxk=0;spk=0;syk=0;pyy=0;pxy=0;p=p0;

%--------------構(gòu)造sigma點(diǎn)-----------------

pk=ns*chol(p);%B=chol(A);meant:A'*A=B;

sigma=xc0;

fork=1:2*n

if(k<=n)

sigma=[sigma,xc0+pk(:,k)];

else

sigma=[sigma,xc0-pk(:,k-n)];

end

end