《MATLAB輔助現(xiàn)代工程數(shù)字信號處理》課件第9章

第9章線性預測與自適應濾波9.1維納濾波器

9.2卡爾曼濾波

9.3卡爾曼濾波在信號處理中的應用

9.4自適應濾波器

9.5自適應濾波在信號處理中的應用

9.6小結

9.1維納濾波器

若線性系統(tǒng)的單位樣本響應為h(n)，當輸入一個隨機信號x(n)

x(n)=s(n)+υ(n)

(9.1)

式中，s(n)表示信號，υ(n)表示噪聲。則輸出y(n)為

(9.2)通常稱y(n)為s(n)的估計值，用表示，即(9.3)該線性系統(tǒng)h(·)稱為對于s(n)的一種估計器。通常稱用觀察值{x(n)}估計當前的信號值為濾波;稱用過去的觀察值估計將來的信號值為預測或外推;稱用過去的觀察值來估計過去的信號值為平滑或內(nèi)插。維納濾波與卡爾曼濾波都是以最小均方誤差準則來解決最佳線性濾波和預測問題的。維納濾波是根據(jù)全部過去的和當前的觀察數(shù)據(jù)來估計信號的當前值，它的解是以均方誤差最小條件下所得到的系統(tǒng)傳遞函數(shù)H(z)或單位樣本響應h(n)的形式給出的。因此，稱維納濾波系統(tǒng)為最佳線性濾波器。維納濾波器只適用于平穩(wěn)隨機過程。維納濾波最初是對連續(xù)信號以模擬濾波器的形式出現(xiàn)的，之后才有了離散形式。本章僅討論維納濾波的離散形式。9.1.1維納濾波器的時域分析

設計維納濾波器的過程就是在最小均方誤差下，尋求濾波器的單位樣本響應h(n)或傳遞函數(shù)H(z)的表達式的過程，其實質是求解維納-霍夫(Wiener-Hopf)方程?？蓪⒐烙?9.4)看成現(xiàn)在和過去各輸入的加權之和，簡記為(9.5)存在均方誤差(9.6)假定，分別為x的自相關函數(shù)和x與s的互相關函數(shù)，若h(n)是一個物理可實現(xiàn)的因果序列，則維納-霍夫方程為

(9.7)式(9.7)的解h就是在最小均方誤差下的最佳hopt。對于非因果序列，維納-霍夫方程為(9.8)維納-霍夫方程的矩陣形式為

(9.9)其中，h1，h2，…，hN為h(n)序列在n=0，1，…，N－1時的值，表示為(9.10)[φxx]為x的自相關矩陣，表示為(9.11)[φxs]為x與s的互相關矩陣，表示為(9.12)

由此可解出(9.13)MATLAB程序如下:

%MATLABPROGRAM9-1

clcl;

clf;

%設置各變量初始值

L=500;N=10;

a=0.95;

row1=L-99:L;row2=1:N;

b=sqrt((1-a^2)*3);

c=sqrt(3);

%產(chǎn)生信號S、噪聲V和隨機信號X

W=unifrnd(－b，b，1，L);

S=zeros(1，L);

S(1，1)=W(1，1);

fori=2:L

S(1，i)=a*S(1，i－1)+W(1，i);

end

cleari;

V=unifrnd(-c，c，1，L);

X=zeros(1，L);

fori=1:L

X(1，i)=S(1，i)+V(1，i);

end

cleari;

fori=1:100

S1(1，i)=S(1，L－100+i);

end

cleari;

fori=1:100

X1(1，i)=X(1，L－100+i);

end

cleari;

%估計X自相關矩陣

corXX1=zeros(1，N);

fori=0:N－1

forj=1:L－i

corXX1(1，i+1)=X(1，j)*X(1，j+i)+corXX1(1，i+1);

end

corXX1(1，i+1)=corXX1(1，i+1)/(L－i);

end

cleari;

clearj;

corXX=zeros(N，N);

fori=1:N

forj=1:N

corXX(i，j)=corXX1(1，abs(i－j)+1);

end

end

cleari;

clearj;

%估計XS相關向量

corXS=zeros(1，N);

fori=0:N－1

forj=1:L－i

corXS(1，i+1)=X(1，j)*S(1，j+i)+corXS(1，i+1);

end

corXS(1，i+1)=corXS(1，i+1)/(L－i);

end

cleari;

clearj;

corXS=corXS′;

%計算兩類h(n)并比較差異

h1=inv(corXX)*corXS;

h2=zeros(1，N);

h2(1，1)=0.238;

fori=2:N

h2(1，i)=h2(1，i－1)*0.724;

end

cleari;

Eh=0;

Eh2=0;

fori=1:N

Eh=h1(i，1)－h(huán)2(1，i)+Eh;

Eh2=(h1(i，1)－h(huán)2(1，i))^2+Eh2;

end

Eh

Eh2

%計算X理想濾波情況

Sl=zeros(1，L);

Sl(1，1)=X(1，1);

fori=2:L

Sl(1，i)=0.724*Sl(1，i－1)+0.238*X(1，i);

end

%計算X的FIR濾波情況

Sr=zeros(1，L);

h1=h1′;

fori=1:L

Srr=0;

forj=1:N

if(i－j<=0)

break;

else

Srr=h1(1，j)*X(1，i－j)+Srr;

end

Sr(1，i)=Srr;

end

end

cleari;

clearj;

%計算Ex，El，Er

Ex=0;

El=0;

Er=0;

fori=1:L

Ex=(X(1，i)－S(1，i))^2+Ex;

El=(Sl(1，i)－S(1，i))^2+El;

Er=(Sr(1，i)－S(1，i))^2+Er;

end

Ex=Ex/L

El=El/L

Er=Er/L

figure(1);

plot(row1，S1，′b′，row1，X1，′r－－′);

legend(′隨機信號S′，′加噪后X′，1);

figure(2);

plot(row2，h1，′*b′，row2，h2，′or′);

legend(′估算的h(n)′，′理想的h(n)′，1);

figure(3);

fori=1:100

Sl1(1，i)=Sl(1，L-100+i);

end

cleari;

plot(row1，Sl1，′b－－′，row1，S1，′r′);

legend(′理想h(n)濾波后S′，′b－－′，′原信號S′，1);

figure(4);

fori=1:100

Sr1(1，i)=Sr(1，L－i+1);

end

cleari;

plot(row1，Sr1，′b－－′，row1，S1，′r′);

legend(′估算h(n)濾波后S′，′原信號S′，1);

程序運行結果如下:

Eh=－0.0659

Eh2=0.0017

Ex=1.0063

El=0.2780

Er=0.3554

維納濾波前后信號估計曲線與理想曲線如圖9.1所示。圖9.1利用維納濾波器估算h(n)

【例9.2】

利用MATLAB編寫程序，完成維納濾波器對含噪聲的隨機信號的處理。其中，信號由MATLAB函數(shù)提供，信號的樣本數(shù)和FIR濾波器的階數(shù)可變。

MATLAB程序如下:

%MATLABPROGRAM9-2

L=input(′請輸入信號樣本個數(shù)L:′);

N=input(′請輸入FIR濾波器階數(shù)N:′);

Ex=0;%x(n)對s(i)的均方誤差

Ei=0;

%si(n)對s(i)的均方誤差

Er=0;

%sr(n)對s(i)的均方誤差

b1=sqrt(3);

%產(chǎn)生v(n)的參數(shù)

b2=0.5408;

%產(chǎn)生w(n)的參數(shù)

a=0.95;

%設定a值

v=2*b1*rand(1，L)－b1;%產(chǎn)生v(n)，方差為1的隨機數(shù)

u=zeros(1，L);

fori=1:L，%產(chǎn)生u(n)

u(i)=1;

end

w=2*b2*rand(1，L)－b2;%產(chǎn)生w(n)，方差為1－a^2

s=zeros(1，L);

s(1)=0;

%初始化s(1)=0

fori=2:L，%得到信號s(n)

s(i)=a*s(i－1)+w(i);

end

x=zeros(1，L);

x=s+v;%得到含有噪聲的信號x(n)

n=L－100:L;

figure(1);

plot(n，x(n)，′k:′，n，s(n)，′b′);

%在同一坐標系中畫出最后100個s(n)和x(n)

legend(′x(n)′，′s(n)′);

xlabel(′n′);ylabel(′x(n)－－s(n)′);title(′信號s(n)和噪聲信號混合x(n)′);

Rxx=zeros(N，N);%Rxx為x(n)的自相關

fori=1:N，%i為行數(shù)

forj=1:N，%j為列數(shù)

teR=0;

te=abs(i－j);

%括號中的值是行數(shù)減去列數(shù)

fork=1:L－te，

teR=teR+x(k)*x(k+te);

end

teR=teR/(L－te);

Rxx(i，j)=teR;

%賦值給Rxx

end

end

rxs=zeros(N，1);

%rxs為x(n)和s(n)的相關數(shù)

form=0:N－1，

ter=0;

fori=1:L－m，

ter=ter+x(i)*s(i+m);

end

ter=ter/(L－m);

rxs(m+1)=ter;

end

h_e=Rxx^(－1)*rxs;

h=zeros(N，1);%維納濾波器理想脈沖響應

fori=1:N，

h(i)=0.238*0.724^i*u(i);

%求出理想h

end

n=1:N;

figure(2);

plot(n，h_e(n)，′:*′，n，h(n)，′－o′);

%在同一坐標系中繪出h和它的估計值

legend(′h_e(n)′，′h(n)′);

xlabel(′n′);ylabel(′h_e(n)－－h(huán)(n)′);title(′估計h_e(n)和理想h(n)′);

v=2*b1*rand(1，L)－b1;%產(chǎn)生v(n)，方差為1的隨機數(shù)

w=2*b2*rand(1，L)－b2;%產(chǎn)生w(n)，方差為1－a^2

s=zeros(1，L);

s(1)=0;

%初始化s(1)=0

fori=2:L，%得到信號s(n)

s(i)=a*s(i－1)+w(i);

end

x=zeros(1，L);

x=s+v;

%得到含有噪聲的信號x(n)

si_e=zeros(1，L);

si_e(1)=0;

fori=2:L，%得到si_e(n)

si_e(i)=0.724*si_e(i－1)+0.238*x(i);

end

n=L－100:L;

figure(3);

plot(n，si_e(n)，′:′，n，s(n));%將s與理想維納濾波的sl值繪于同一坐標系中

legend(′si_e(n)′，′s(n)′);

xlabel(′n′);ylabel(′si_e(n)－－s(n)′);title(′si_e(n)和s(n)′);

sr_e=zeros(1，L);

fori=N:L，

form=0:N－1，

sr_e(i)=sr_e(i)+h_e(m+1)*x(i－m);

end

end%計算出est_sr(n)

n=L－100:L;

figure(4);

plot(n，sr_e(n)，′:′，n，s(n));%將s與S_R值繪于同一坐標系中

legend(′sr_e(n)′，′s(n)′);

xlabel(′n′);ylabel(′sr_e(n)－－s(n)′);title(′sr_e(n)和s(n)′);

fori=1:L，

Ex=Ex+(x(i)－s(i))^2;

end

Ex=Ex/L，

fori=1:L，

Ei=Ei+(si_e(i)－s(i))^2;

end

Ei=Ei/L，

fori=1:L，

Er=Er+(sr_e(i)－s(i))^2;

end

Er=Er/L，

figure(5);

plot(1，Ex，′p′，1，Ei，′*′，1，Er，′o′);%將EX^2，EL^2，ER^2繪于同一坐標系中

legend(′Ex′，′Ei′，′Er′);title(′Ex，Ei，Er′);

程序運行結果如下:

請輸入信號樣本個數(shù)L:1024

請輸入FIR濾波器階數(shù)N:30

Ex=0.9843

Ei=0.2485

Er=0.2842

原始信號曲線與維納濾波后的曲線對比圖如圖9.2所示。圖9.2維納濾波器對信號濾波的曲線圖9.1.2維納濾波器的頻域分析

當要求維納濾波器單位樣本響應h(n)是一個物理可實現(xiàn)的因果序列時，所得到的維納-霍夫方程式將附有k≥0的約束條件。這使得在要求滿足物理可實現(xiàn)條件下，求解維納-霍夫方程成為一個十分困難的問題。若把x(n)加以白化，則求解維納-霍夫方程的z域解就變得簡單。

任何具有有理分式型的功率譜密度的隨機信號都可以看成是由白噪聲ω(n)激勵一個物理網(wǎng)絡產(chǎn)生的。一般信號s(n)的功率譜密度Φss(z)是z的有理分式，其中A(z)表示信號s(n)的形成網(wǎng)絡的傳遞函數(shù)。白噪聲的自相關函數(shù)及功率譜密度分別為

Φωω(n)=σ2ωδ(n)(9.14)

Φωω(z)=σ2ω

(9.15)

因此，s(n)的功率譜密度表示為

Φss(z)=σ2ωA(z)A(z－1)

(9.16)

若x(n)的功率譜密度也為z的有理分式，則

Φxx(z)=σω2B(z)B(z－1)

(9.17)

式中，B(z)是x(n)的形成網(wǎng)絡的傳遞函數(shù)。因此(9.18)由于B(z)是一個最小相移網(wǎng)絡函數(shù)，故1/B(z)也是一個物理可實現(xiàn)的最小相移網(wǎng)絡，因此可以利用式(9.18)的關系白化x(n)。設計維納濾波器是求在最小條件下的最佳H(z)的問題，如圖9.3(a)所示。為了便于求得這個Hopt(z)，將濾波器分解成如圖9.3(b)所示的兩個串聯(lián)的濾波器，即1/B(z)與G(z)。圖9.3白化法求解維納-霍夫方程于是有(9.19)式中，B(z)由Φxx(z)在單位圓內(nèi)的零極點組成。對于一個物理可實現(xiàn)的因果系統(tǒng)來說，若已知信號的Φxx(z)，則可求得B(z)。因此，求在最小均方誤差下的最佳Hopt(z)的問題就歸結為求最佳G(z)的問題，而G(z)的激勵源是將x(n)白化后得到的白噪聲。非因果維納濾波器的頻率特性為(9.20)式(9.20)說明，Hopt(ejω)決定于信號與噪聲的功率譜密度。

當沒有噪聲時，Pυυ(ω)=0，Hopt(ejω)=1;隨著Pυυ(ω)的增加,Hopt(ejω)將減小;當Pss(ω)=0而Pυυ(0)≠0時,Hopt(ejω)=0。

物理可實現(xiàn)的因果維納濾波器的傳遞函數(shù)為(9.21)兩種情況下的維納濾波器的最小均方誤差E[e2(n)]min均為(9.22)

【例9.3】

利用MATLAB編程求解基于維納-霍夫方程的維納濾波器。信號由MATLAB編程給出，觀測點數(shù)N取100，并計算濾波誤差。

MATLAB程序如下:

%MATLABPROGRAM9-3

clc;

clearall;

maxlag=100;

N=100;%觀測點數(shù)取100

x=zeros(N，1);

y=zeros(N，1);

var=1;

%列出狀態(tài)方程

x(1)=randn(1，1);%令x(－1)=x(－2)=x(－3)=x(－4)=0

x(2)=randn(1，1)+1.352*x(1);

x(3)=randn(1，1)+1.352*x(2)－1.338*x(1);

x(4)=randn(1，1)+1.352*x(3)－1.338*x(2)+0.602*x(1);

forn=5:N

x(n)=1.352*x(n－1)－1.338*x(n－2)+0.602*x(n－3)－0.24*x(n－4)+randn(1，1);%x為真實值

end;

v=randn(N，1);

y=x+v;

%z_x為觀測樣本值=真值+噪聲

%濾波

x=x′;

y=y′;

xk_s(1)=y(1);%賦初值

xk_s(2)=y(2);

xk_s(3)=y(3);

xk_s(4)=y(4);

xk=[y(1);y(2);y(3);y(4)];

%維納濾波器的生成

[rx，lags]=xcorr(y,maxlag,′biased′);%觀測信號的自相關函數(shù)

rx1=toeplitz(rx(101:end));%對稱化自相關函數(shù)矩陣使之成為方陣，濾波器的階數(shù)為101階

rx2=xcorr(x，y，maxlag，′biased′);%觀測信號與期望信號的互相關函數(shù)

rx2=rx2(101:end);

h=inv(rx1)*rx2′;%維納-霍夫方程

xk_s=filter(h，1，y);%加噪信號通過濾波器后的輸出

e_x=0;

eq_x=0;

e_x1=N:1;

%計算濾波的均值，計算濾波誤差的均值

fori=1:N

e_x(i)=x(i)－xk_s(i);%誤差=真實值-濾波估計值

end

t=[1:N];

figure(1);

plot(t，x，′r-′，t，y，′g:′，t，xk_s，′b.′);

xlabel(′采樣點′);ylabel(′輸出′);

legend(′真實軌跡′，′觀測樣本′，′估計軌跡′);

figure(2);

plot(e_x);xlabel(′采樣點′);ylabel(′e_x′);legend(′平均誤差′);程序運行結果如圖9.4所示。圖9.4維納濾波估計及其誤差分析9.1.3維納預測器

若輸入x(n)中不含噪聲υ(n)，即x(n)=s(n)，則稱對s(n+N)的估計為純預測問題。隨機信號在任一時刻時x(n)或s(n)的取值雖具有偶然性，但利用x(n)與s(n)的某些統(tǒng)計特性，可以預測和確定當前和今后最可能的取值。一個預測器的輸入輸出信號如圖9.5表示，其中yd(n)是希望得到的輸出，即s(n+N)，而實際得到的輸出y(n)為s(n+N)的估計值。圖9.5維納預測器維納濾波器所希望的輸出為yd=s(n)，維納預測器所希望的輸出為yd=s(n+N)。前者的實際輸出為

，后者的實際輸出為(9.23)設計維納預測器的問題就是求在最小條件下的h(n)或H(z)的問題。非因果維納預測器的H(z)為(9.24)物理可實現(xiàn)的因果維納預測器的傳遞函數(shù)為(9.25)

兩種情況下的最小均方誤差均為(9.26)

【例9.4】

隨機信號AR模型由白噪聲產(chǎn)生，高斯噪聲為干擾信號，利用MATLAB設計一維納預測器進行濾波，估計并繪制信號波形。MATLAB程序如下:

%MATLABPROGRAM9-4

clear;clc;

%根據(jù)AR模型由白噪聲產(chǎn)生隨機信號

n=1:100;

W=randn(1，100);

plot(n，W);

B=[1];

A=[1，－1.352，1.338，－0.662，0.240];

X=filter(B，A，W);

%產(chǎn)生方差為4的高斯噪聲

V=randn(1，100);

V=V/std(V);

V=V－mean(V);

a=0;

b=sqrt(4);

V=a+b*V;

Y=X+V;%產(chǎn)生觀測信號

%維納濾波器的生成

maxlag=100;

[rx，lags]=xcorr(Y，maxlag，′biased′);

%觀測信號的自相關函數(shù)

rx1=toeplitz(rx(101:end));%對稱化自相關函數(shù)矩陣使之成為方

陣，濾波器的階數(shù)為101階

rx2=xcorr(X，Y，maxlag，′biased′);

%觀測信號與期望信號的互相關函數(shù)

rx2=rx2(101:end);

xlabel(′采樣點′);ylabel(′輸出′);

title(′信號波形(v(n)=4)′);

legend(′期望輸出信號′，′Wiener濾波估計′);

程序運行結果如圖9.6所示。圖9.6期望信號波形與維納濾波估計波形

9.2卡爾曼濾波

9.2.1離散狀態(tài)方程

卡爾曼濾波由狀態(tài)方程和量測方程兩部分組成。

離散狀態(tài)方程為(9.27)x(k+1)=Ax(k)+Be(k)式中，x(k)代表一組狀態(tài)變量組成的多維狀態(tài)矢量;A和B都是矩陣，由系統(tǒng)拓撲結構、元件性質和數(shù)值所確定;e(k)為激勵信號。狀態(tài)方程是多維一階的差分方程。已知初始狀態(tài)x(0)時，可用遞推方法得到解x(k)

(9.28)式中，第一項Akx(0)只與系統(tǒng)本身的特性A和初始狀態(tài)x(0)有關，與激勵e(·)無關，稱為零輸入響應;第二項只與激勵和系統(tǒng)本身特性有關，而與初始狀態(tài)無關，稱為零狀態(tài)響應。假定φ(k)=Ak，當e(k)=0時，x(k)=Akx(0)=φ(k)x(0)。由此可見，通過φ(k)可將k=0時的狀態(tài)過渡到任何k>0時的狀態(tài)，故稱φ(k)為過渡矩陣或狀態(tài)轉移矩陣。將Ak=φ(k)代入式(9.28)，得(9.29)式(9.29)就是式(9.27)的解。當已知初始狀態(tài)x(0)、激勵e(j)及A與B矩陣時，即可求得x(k)。如果用k0表示起始點的k值，則式(9.29)中的k0=0，表明從初始狀態(tài)x(0)開始遞推。如果k0≠0，則從x(k0)開始遞推，有

(9.30)式中，φk，k0代表從k0狀態(tài)到k狀態(tài)的過渡矩陣。如果k0=k－1，得到一步遞推公式為(9.31)φ(θ)為單位矩陣，即φ(0)=A0=I，代入式(9.31)有(9.32)式中，φk，k－1=A(k－k+1)=A，說明k時刻的狀態(tài)x(k)可以由前一個時刻的狀態(tài)x(k－1)求得。

若激勵源為白噪聲，即Be(k－1)=ω(k－1)，系統(tǒng)是時變的，φk，k－1=A(k)，則式(9.32)可改寫成

x(k)=A(k)x(k－1)+ω(k－1)

(9.33)

為書寫方便，將變量k用下標表示，則式(9.33)可寫為

xk=Akxk－1+ωk－1

(9.34)9.2.2量測方程

卡爾曼濾波是根據(jù)系統(tǒng)的量測數(shù)據(jù)，對系統(tǒng)的運動進行估計的。因此，除了狀態(tài)方程以外，還需要量測方程。量測系統(tǒng)可以是時不變系統(tǒng)，也可以是時變系統(tǒng)。假定量測數(shù)據(jù)和系統(tǒng)的各狀態(tài)變量間存在線性關系。若用yk表示量測或觀察到的信號矢量序列，則它與狀態(tài)變量xk的關系可以寫成

yk=Ckxk+υk

(9.35)

式中，υk是觀察或量測時引入的噪聲，代表測量誤差的隨機向量，一般可以假定為均值為零的正態(tài)白噪聲。yk的維數(shù)不一定與xk的維數(shù)相等，因為不一定能量測到所有需要的狀態(tài)參數(shù)。Ck稱為量測矩陣，它是一個m×n的矩陣(m為yk的維數(shù)，n為xk的維數(shù))。υk的維數(shù)應和yk的維數(shù)一致。用sk=Ckxk表示量測數(shù)據(jù)真值，則式(9.35)可寫成

yk=sk+υk

(9.36)

由量測方程與狀態(tài)方程可以得到卡爾曼濾波在多維時的信號模型，如圖9.7(a)所示;圖9.7(b)表示一維時的信號模型。圖9.7(a)中的虛線框內(nèi)部即為傳遞函數(shù)A(z)。圖9.7卡爾曼濾波的信號模型9.2.3卡爾曼濾波的基本遞推算法

卡爾曼濾波是要尋找在最小均方誤差下xk的估計值。一般采用遞推方法計算。假定已知動態(tài)系統(tǒng)的狀態(tài)方程和量測方程分別為

xk=Akxk－1+ωk－1

(9.37)

yk=Ckxk+υk

(9.38)

式中，Ak與Ck已知;yk是測量到的數(shù)據(jù)。

將估計輸出值與yk的實際觀察值作比較，其差用表示，有(9.39)的產(chǎn)生是由于忽略了ωk－1與υk所引起的。由此可知，隱含了ωk－1與υk的信息，或者說隱含了當前的觀察值yk的信息，故稱為新息(innovation)。若將乘以Hk來修正原先的值，會得到更好的估計:

(9.40)式中，與真值xk的均方誤差是一個誤差方陣。如果能求得這個誤差陣最小條件下的Hk

，然后將此Hk代入式(9.40)，則所得到的就是對xk的線性最優(yōu)估計。假定Pk表示均方誤差陣，且為(9.41)令(9.42)

由于式(9.41)中，，由此可得均方誤差陣最小條件下的的一組卡爾曼遞推公式:(9.43)(9.44)(9.45)(9.46)其中，I為單位矩陣。由式(9.43)可見，若已知Hk，利用前一個xk的估計值與當前的量測值yk，就可以求得。若Hk是按式(9.44)計算的，即為滿足最小均方誤差陣的Hk，則將此Hk代入式(9.40)，就得到所求的在最小均方誤差陣條件下的。根據(jù)已知矩陣Ak、Ck、Qk、

Rk以及觀測值yk，就能用遞推算法得到所有的、、…、以及P1、P2、…、Pk。

【例9.5】

假定目標沿水平方向運動，起始位置為(-2000，500)m，運動速度為10m/s，掃描周期T=5s，μa=0，σa=100。利用卡爾曼遞推濾波算法，對信號進行濾波仿真，繪制濾波曲線，并分析濾波誤差。

MATLAB程序如下:

%MATLABPROGRAM9-5

clc;

T=2;

num=100;

%真實軌跡

N=800/T;

x=zeros(N，1);y=zeros(N，1);

vx=zeros(N，1);vy=zeros(N，1);

x(1)=－2000;

y(1)=500;

forj=1:N

vx(j)=10;

end

var=100;

fori=1:N－1;

x(i+1)=x(i)+vx(i)*T;%+0.5*T^2*ax;

y(i+1)=y(i)+vy(i)*T;%+0.5*T^2*ay;

end

nx=zeros(N，1);ny=zeros(N，1);

nx=100*randn(N，1);

ny=100*randn(N，1);

zx=x+nx;zy=y+ny;

%濾波

form=1:num

z=2:1;

xks(1)=zx(1);

yks(1)=zy(1);

xks(2)=zx(2);

yks(2)=zy(2);

o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4;

o=[1，T，0，0;0，1，0，0;0，0，1，T;0，0，0，1];

h=[1000;0010];

g=[T/2，0;T/2，0;0，T/2;0，T/2;0，T/2];

q=[100000;010000];

perr=[var^2，var^2/T，0，0;var*var/T，2*var^2/(T^2)，0，0;0，0，var^2，var^2/T;0，0，var^2/T，2*var^2/(T^2)];

vx=(zx(2)－zx(1))/2;

vy=(zy(2)－zy(1))/2;

xk=[zx(1);vx;zy(1);vy];

%卡爾曼濾波開始

forr=3:N;

z=[zx(r);zy(r)];

xk1=o*xk;

perr1=o*perr*o′;

k=perr1*h′*inv(h*perr1*h′+q);

xk=xk1+k*(z-h*xk1);

perr=(eye(4)－k*h)*perr1;

xks(r)=xk(1，1);

yks(r)=xk(3，1);

vkxs(r)=xk(2，1);

vkys(r)=xk(4，1);

xk1s(r)=xk1(1，1);

yk1s(r)=xk1(3，1);

perr11(r)=perr(1，1);

perr12(r)=perr(1，2);

perr22(r)=perr(2，2);

rex(m，r)=xks(r);

rey(m，r)=yks(r);

end

end

ex=0;ey=0;

eqx=0;eqy=0;

ey1=0;

ex1=N:1;ey1=N:1;

fori=1:N

forj=1:num

ex=ex+x(i)-rex(j，i);

ey=ey+y(i)-rey(j，i);

end

ex1(i)=ex/num;

ey1(i)=ey/num;

ex=0;eqx=0;ey=0;eqy=0;

end

figure(1);

plot(x，y，′k-′，zx，zy，′b:′，xks，yks，′r－.′);

legend(′真實軌跡′，′觀測樣本′，′估計軌跡′);

figure(2);

plot(ey1);legend(′x方向上的誤差′);程序運行結果如圖9.8所示。由圖可知，在濾波開始時濾波誤差較大，但隨著時間的推移，濾波誤差迅速降低，估計值逐步逼近真實軌跡。圖9.8卡爾曼濾波算法對信號進行濾波9.3卡爾曼濾波在信號處理中的應用

9.3.1目標跟蹤的卡爾曼濾波

在應用卡爾曼濾波理論時，要先定義估計問題的數(shù)學模型來描述某個時刻狀態(tài)變量與以前時刻的關系。狀態(tài)變量應與系統(tǒng)的能量相聯(lián)系，如目標的運動模型，狀態(tài)變量可選用目標的位置(與目標的引力能相聯(lián)系)和速度(與目標的動能相聯(lián)系)。一般來說，狀態(tài)變量的增加會使估計的計算量相應增加，因此在滿足模型的精度和跟蹤性能的條件下，常采用簡單的數(shù)學模型。跟蹤濾波的目的是根據(jù)已獲得的目標觀測數(shù)據(jù)對目標的狀態(tài)進行精確估計。在跟蹤問題中遇到的運動載體如飛機、船只等，一般都按照恒速直線運動的軌跡運動，運動載體的轉彎、機動所引起的加速度則看做恒速直線航跡的攝動。在直角坐標系中，該目標運動的數(shù)學模型可用差分方程描述為(9.47)(9.48)上式中，X(k)=[x(k)，y(k)]T和分別表示雷達第k次掃描時目標在坐標方向上的位置和速度。假定目標的加速度aX(k)是平穩(wěn)隨機序列，服從零均值、方差為σa2的正態(tài)分布，且在某一時刻aX(k)的加速度與另一時刻aX(l)的加速度不相關，即I為2×2階的單位矩陣。對于恒速模型(非機動模型)來說，若目標以恒定的速度在運動，則可得其狀態(tài)方程為

X(k+1)=ΦX(k)+GW(k)

(9.49)

式中，

，，，W(k)是零均值、方差陣為Q的高斯隨機序列。在兩個坐標方向上的加速度相互獨立并具有相同的方差σa2，故Q=σa2I，即E[W(k)WT(j)]=Qδkj。(9.50)式中，，V為零均值、協(xié)方差陣為R的白噪聲，且與W不相關。

【例9.6】利用卡爾曼濾波對飛行物進行目標跟蹤。飛行物初始時刻距雷達站距離為30km，初始速度為300m/s，加速度為20m/s2，采樣周期T=0.5s，試編程給出仿真分析結果。MATLAB程序如下:

%MATLABPROGRAM9-6

clc;

N=100;

x(:，1)=[30;300;20];

T=0.5;

F=[1，T，T^2/2;0，1，T;0，0，1];

Gama=[T^3/6，T^2/2，T]′;

H=[100];

A=x(:，1);

Q=1;

R=100;

fork=2:N

w=normrnd(0，1);

v=normrnd(0，10);

x(:，k)=F*x(:，k-1)+Gama*w;

A=[A，x(:，k－1)];

z(:，k)=H*x(:，k)+v;

end

%初始化

p0=zeros(3);

x0=[1;1;1];

x_hat(:，1)=x0;

B=x0;

C=z(:，1);

P=p0;

RR=zeros(1，N);

VV=zeros(1，N);

form=2:N

xx_hat(:，m－1)=F*x_hat(:，m－1);

B=[B，xx_hat(:，m－1)];

z1(:，m)=H*xx_hat(:，m－1);

C=[C，z1(:，m)];

PP=F*P*F′+Gama*Q*Gama′;

S=H*PP*H′+R;

KK=PP*H′*inv(S);

z1_tutor(:，m)=z(:，m)－C(:，m);

x_hat(:，m)=xx_hat(:，m－1)+KK*z1_tutor(:，m);

P=PP－KK*S*KK′;

RR(1，m)=P(1，1);

VV(1，m)=P(2，2);

end

k=1:N;

figure(1);

plot(k，B(1，k)，′b*′，k，A(1，k)，′r.′);title(′運動軌跡的比較′);

legend(′卡爾曼濾波估計軌跡′，′實際軌跡′);

figure(2);

plot(k，B(2，k)，′b*′，k，A(2，k)，′r.′);title(′運動速度的比較′);

legend(′卡爾曼濾波估計速度′，′實際速度′);

figure(3);

plot(k，RR，′b.′，k，VV，′r*′);title(′估計方差′);

legend(′位置估計方差′，′速度估計方差′);

程序運行結果如圖9.9所示。圖9.9目標跟蹤的卡爾曼濾波9.3.2機動模型的濾波跟蹤

對于機動模型(恒加速模型)來說。若目標以恒定的加速度在運動，則其狀態(tài)方程可表示為

(9.51)式中，

觀測模型與非機動模型在形式上相同，只是H矩陣變?yōu)镠m，即(9.52)由于目標動態(tài)模型是線性的，過程噪聲W(k)和測量噪聲V(k)服從高斯分布且相互獨立，故可利用卡爾曼濾波對目標的位置和速度進行估計，估計的均方誤差是最小的。在目標的跟蹤濾波中，一般先采用非機動模型對目標進行跟蹤，再對目標采用更為有效的濾波方法進行跟蹤濾波。濾波器開始工作于正常模式，其輸出的信息序列為v(k)，令

μ(k)=αμ(k－1)+vT(k)S－1(k)v(k)

(9.53)

式中，S(k)是協(xié)方差矩陣，取Δ=(1－α)－1作為檢測機動的有效窗口長度，若μ(k)≥Th，則認為目標在開始時有一恒定的加速度加入，這時目標模型由非機動模型轉向機動模型。由機動模型退回到低階非機動模型的檢測方法是，檢驗加速度估計值是否有統(tǒng)計顯著性意義。

令(9.54)式中，是加速度分量的估計值，Pma(k/k)是協(xié)方差矩陣的對應塊，若μa(k)<Ta，則加速度估計無顯著性意義，濾波器退出機動模型。當在第k次檢測到機動時，則在開始時有一恒定的加速度，在窗內(nèi)對狀態(tài)估計進行相應的修正。

【例9.7】

雷達需對平面上運動的一個目標進行觀測。目標起始點為(2000，10000)m，目標在t=0~400s時沿y軸作恒速直線運動，運動速度為－20m/s;在t=400~600s時向x軸方向作90°的慢轉彎，加速度為μx=μy=0.075m/s2，完成慢轉彎后加速度將降為零;從t=610s時開始作90°的快轉彎，加速度為0.35m/s2，在t=660s時結束轉彎，加速度降至零。雷達掃描周期T=2s，x和y獨立地進行觀測，觀測噪聲的標準差均為100m。試建立雷達對目標的跟蹤算法，并進行仿真分析，給出仿真分析結果。仿真中各算法的運用和參數(shù)的選擇:假定非機動模型的系統(tǒng)擾動噪聲方差為0，機動模型的系統(tǒng)擾動噪聲標準差為加速度估計的5%，加權衰減因子α=0.8，機動檢測門限Th=35，退出機動的檢測門限Tα=13。在跟蹤的開始，首先采用非機動模型，之后激活機動檢測器。

MATLAB程序如下:

%MATLABPROGRAM9-7

clear;

T=2;

%雷達掃描周期

num=50;

%濾波次數(shù)

%產(chǎn)生真實軌跡

N1=400/T;N2=600/T;N3=610/T;N4=660/T;N5=900/T;

x=zeros(N5，1);

y=zeros(N5，1);

vx=zeros(N5，1);

vy=zeros(N5，1);

x(1)=2000;

y(1)=10000;

vx(1)=0;

vy(1)=－15;

ax=0;ay=0;var=100;

fori=1:N5－1

if(i>N1－1&i<=N2－1)

ax=0.075;

ay=0.075;

vx(i+1)=vx(i)+0.075*T;

vy(i+1)=vy(i)+0.075*T;

elseif(i>N2－1&i<=N3－1)

ax=0;

ay=0;

vx(i+1)=vx(i);

vy(i+1)=vy(i);

elseif(i>N3－1&i<=N4－1)

ax=－0.3;

ay=－0.3;

vx(i+1)=vx(i)－0.3*T;

vy(i+1)=vy(i)－0.3*T;

else

ax=0;

ay=0;

vx(i+1)=vx(i);

vy(i+1)=vy(i);

end

x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2;%真實軌跡

y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2;

end

rex=num:N5;

rey=num:N5;

form=1:num

%噪聲

nx=randn(N5，1)*100;

ny=randn(N5，1)*100;

zx=x+nx;

zy=y+ny;

%卡爾曼濾波初始化

rex(m，1)=2000;

rey(m，1)=10000;

ki=0;

low=1;high=0;

u=0;ua=0;

e=0.8;

z=2:1;

xks(1)=zx(1);

yks(1)=zy(1);

xks(2)=zx(2);

yks(2)=zy(2);

o=4:4;g=4:2;c=2:4;q=2:2;xk=4:1;perr=4:4;

o=[1，T，0，0;0，1，0，0;0，0，1，T;0，0，0，1];

g=[(T^2)/2，0;T，0;0，(T^2)/2;0，T];

h=[1，0，0，0;0，0，1，0];

q=[100000;010000];

perr=[var^2，var^2/T，0，0;var^2/T，2*var^2/(T^2)，0，0;0，0，var^2，var^2/T;0，0，

var^2/T，2*var^2/(T^2)];

vx=(zx(2)－zx(1))/2;

vy=(zy(2)－zy(1))/2;

xk=[zx(1);vx;zy(1);vy];

%卡爾曼濾波開始

forn=3:N5;

if(u<=40)%非機動模型、賦初值

if(low==0)

[o，h，g，q，perr，xk]=lmode_initial(T，n，zx，zy，vkxs，vkys，perr2);

z=2:2;

high=0;

low=1;

ua=0;

end

z=[zx(n);zy(n)];

xkl=o*xk;

perr1=o*perr*o′;

k=perr1*h′*inv(h*perr1*h′+q);

xk=xkl+k*(z－h(huán)*xkl);

perr=(eye(4)－k*h)*perr1;

xks(n)=xk(1，1);

yks(n)=xk(3，1);

vkxs(n)=xk(2，1);

vkys(n)=xk(4，1);

xkls(n)=xkl(1，1);

ykls(n)=xkl(3，1);

perr11(n)=perr(1，1);

perr12(n)=perr(1，2);

perr22(n)=perr(2，2);

if(n>=20)

v=z-h*xkl;

w=h*perr*h′+q;

p=v′*inv(w)*v;

u=e*u+p;

s(n-19)=u;

end

elseif(u>40)%啟動機動檢測

if(high==0)

[o，g，h，q，perr，xk]=hmode_initial(T，n，e，zx，zy，xkls，ykls，vkxs，vkys，perr11，

perr12，perr22);

high=1;

low=0;

fori=n－5:n－1

z=[zx(i);zy(i)];

xkl=o*xk;

perr1=o*perr*o′;

k=perr1*h′*inv(h*perr1*h′+q);

xk=xkl+k*(z－h(huán)*xkl);

perr=(eye(6)－k*h)*perr1;

xks(n)=xk(1，1);

yks(n)=xk(3，1);

vkxs(n)=xk(2，1);

vkys(n)=xk(4，1);

xkls(n)=xkl(1，1);

ykls(n)=xkl(3，1);

end

end

z=[zx(n);zy(n)];

xkl=o*xk;

perr1=o*perr*o′;

k=perr1*h′*inv(h*perr1*h′+q);

xk=xkl+k*(z－h(huán)*xkl);

perr=(eye(6)－k*h)*perr1;

xks(n)=xk(1，1);

yks(n)=xk(3，1);

vkxs(n)=xk(2，1);

vkys(n)=xk(4，1);

xkls(n)=xkl(1，1);

ykls(n)=xkl(3，1);

ag=[xk(5，1);xk(6，1)];

perr2=perr;

ki=ki+1;

pm=[perr(5，5)perr(5，6);perr(6，5)perr(6，6)];

pa=ag′*inv(pm)*ag;

sa(n)=pa;

if(ki>5)%退出機動判斷

ul=sa(n－4)+sa(n－3)+sa(n－2)+sa(n－1)+sa(n);

sb(n)=ul;

if(ul<20)

u=0;

end

end

end

rex(m，n)=xks(n);%將濾波值存入數(shù)組

rey(m，n)=yks(n);

end

end

%計算濾波誤差的均值以及標準差

ex=0;ey=0;

eqx=0;eqy=0;

ey1=0;

ex1=N5:1;ey1=N5:1;

qx=N5:1;qy=N5:1;

fori=1:N5

forj=1:num

ex=ex+x(i)－rex(j，i);

ey=ey+y(i)－rey(j，i);

eqx=eqx+(x(i)－rex(j，i))^2;