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

第9章線性預(yù)測(cè)與自適應(yīng)濾波9.1維納濾波器

9.2卡爾曼濾波

9.3卡爾曼濾波在信號(hào)處理中的應(yīng)用

9.4自適應(yīng)濾波器

9.5自適應(yīng)濾波在信號(hào)處理中的應(yīng)用

9.6小結(jié)

9.1維納濾波器

若線性系統(tǒng)的單位樣本響應(yīng)為h(n)，當(dāng)輸入一個(gè)隨機(jī)信號(hào)x(n)

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

(9.1)

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

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

設(shè)計(jì)維納濾波器的過(guò)程就是在最小均方誤差下，尋求濾波器的單位樣本響應(yīng)h(n)或傳遞函數(shù)H(z)的表達(dá)式的過(guò)程，其實(shí)質(zhì)是求解維納-霍夫(Wiener-Hopf)方程。可將估計(jì)(9.4)看成現(xiàn)在和過(guò)去各輸入的加權(quán)之和，簡(jiǎn)記為(9.5)存在均方誤差(9.6)假定，分別為x的自相關(guān)函數(shù)和x與s的互相關(guān)函數(shù)，若h(n)是一個(gè)物理可實(shí)現(xiàn)的因果序列，則維納-霍夫方程為

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

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

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

%MATLABPROGRAM9-1

clcl;

clf;

%設(shè)置各變量初始值

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)生信號(hào)S、噪聲V和隨機(jī)信號(hào)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;

%估計(jì)X自相關(guān)矩陣

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;

%估計(jì)XS相關(guān)向量

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′;

%計(jì)算兩類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

%計(jì)算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

%計(jì)算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;

%計(jì)算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(′隨機(jī)信號(hào)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－－′，′原信號(hào)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′，′原信號(hào)S′，1);

程序運(yùn)行結(jié)果如下:

Eh=－0.0659

Eh2=0.0017

Ex=1.0063

El=0.2780

Er=0.3554

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

【例9.2】

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

MATLAB程序如下:

%MATLABPROGRAM9-2

L=input(′請(qǐng)輸入信號(hào)樣本個(gè)數(shù)L:′);

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

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

Ei=0;

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

Er=0;

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

b1=sqrt(3);

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

b2=0.5408;

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

a=0.95;

%設(shè)定a值

v=2*b1*rand(1，L)－b1;%產(chǎn)生v(n)，方差為1的隨機(jī)數(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，%得到信號(hào)s(n)

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

end

x=zeros(1，L);

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

n=L－100:L;

figure(1);

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

%在同一坐標(biāo)系中畫(huà)出最后100個(gè)s(n)和x(n)

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

xlabel(′n′);ylabel(′x(n)－－s(n)′);title(′信號(hào)s(n)和噪聲信號(hào)混合x(chóng)(n)′);

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

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

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

teR=0;

te=abs(i－j);

%括號(hào)中的值是行數(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)的相關(guā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);%維納濾波器理想脈沖響應(yīng)

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′);

%在同一坐標(biāo)系中繪出h和它的估計(jì)值

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

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

v=2*b1*rand(1，L)－b1;%產(chǎn)生v(n)，方差為1的隨機(jī)數(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，%得到信號(hào)s(n)

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

end

x=zeros(1，L);

x=s+v;

%得到含有噪聲的信號(hào)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值繪于同一坐標(biāo)系中

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%計(jì)算出est_sr(n)

n=L－100:L;

figure(4);

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

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繪于同一坐標(biāo)系中

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

程序運(yùn)行結(jié)果如下:

請(qǐng)輸入信號(hào)樣本個(gè)數(shù)L:1024

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

Ex=0.9843

Ei=0.2485

Er=0.2842

原始信號(hào)曲線與維納濾波后的曲線對(duì)比圖如圖9.2所示。圖9.2維納濾波器對(duì)信號(hào)濾波的曲線圖9.1.2維納濾波器的頻域分析

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

任何具有有理分式型的功率譜密度的隨機(jī)信號(hào)都可以看成是由白噪聲ω(n)激勵(lì)一個(gè)物理網(wǎng)絡(luò)產(chǎn)生的。一般信號(hào)s(n)的功率譜密度Φss(z)是z的有理分式，其中A(z)表示信號(hào)s(n)的形成網(wǎng)絡(luò)的傳遞函數(shù)。白噪聲的自相關(guān)函數(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)絡(luò)的傳遞函數(shù)。因此(9.18)由于B(z)是一個(gè)最小相移網(wǎng)絡(luò)函數(shù)，故1/B(z)也是一個(gè)物理可實(shí)現(xiàn)的最小相移網(wǎng)絡(luò)，因此可以利用式(9.18)的關(guān)系白化x(n)。設(shè)計(jì)維納濾波器是求在最小條件下的最佳H(z)的問(wèn)題，如圖9.3(a)所示。為了便于求得這個(gè)Hopt(z)，將濾波器分解成如圖9.3(b)所示的兩個(gè)串聯(lián)的濾波器，即1/B(z)與G(z)。圖9.3白化法求解維納-霍夫方程于是有(9.19)式中，B(z)由Φxx(z)在單位圓內(nèi)的零極點(diǎn)組成。對(duì)于一個(gè)物理可實(shí)現(xiàn)的因果系統(tǒng)來(lái)說(shuō)，若已知信號(hào)的Φxx(z)，則可求得B(z)。因此，求在最小均方誤差下的最佳Hopt(z)的問(wèn)題就歸結(jié)為求最佳G(z)的問(wèn)題，而G(z)的激勵(lì)源是將x(n)白化后得到的白噪聲。非因果維納濾波器的頻率特性為(9.20)式(9.20)說(shuō)明，Hopt(ejω)決定于信號(hào)與噪聲的功率譜密度。

當(dāng)沒(méi)有噪聲時(shí)，Pυυ(ω)=0，Hopt(ejω)=1;隨著Pυυ(ω)的增加,Hopt(ejω)將減小;當(dāng)Pss(ω)=0而Pυυ(0)≠0時(shí),Hopt(ejω)=0。

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

【例9.3】

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

MATLAB程序如下:

%MATLABPROGRAM9-3

clc;

clearall;

maxlag=100;

N=100;%觀測(cè)點(diǎn)數(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為真實(shí)值

end;

v=randn(N，1);

y=x+v;

%z_x為觀測(cè)樣本值=真值+噪聲

%濾波

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′);%觀測(cè)信號(hào)的自相關(guān)函數(shù)

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

rx2=xcorr(x，y，maxlag，′biased′);%觀測(cè)信號(hào)與期望信號(hào)的互相關(guān)函數(shù)

rx2=rx2(101:end);

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

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

e_x=0;

eq_x=0;

e_x1=N:1;

%計(jì)算濾波的均值，計(jì)算濾波誤差的均值

fori=1:N

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

end

t=[1:N];

figure(1);

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

xlabel(′采樣點(diǎn)′);ylabel(′輸出′);

legend(′真實(shí)軌跡′，′觀測(cè)樣本′，′估計(jì)軌跡′);

figure(2);

plot(e_x);xlabel(′采樣點(diǎn)′);ylabel(′e_x′);legend(′平均誤差′);程序運(yùn)行結(jié)果如圖9.4所示。圖9.4維納濾波估計(jì)及其誤差分析9.1.3維納預(yù)測(cè)器

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

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

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

【例9.4】

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

%MATLABPROGRAM9-4

clear;clc;

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

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)生觀測(cè)信號(hào)

%維納濾波器的生成

maxlag=100;

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

%觀測(cè)信號(hào)的自相關(guān)函數(shù)

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

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

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

%觀測(cè)信號(hào)與期望信號(hào)的互相關(guān)函數(shù)

rx2=rx2(101:end);

xlabel(′采樣點(diǎn)′);ylabel(′輸出′);

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

legend(′期望輸出信號(hào)′，′Wiener濾波估計(jì)′);

程序運(yùn)行結(jié)果如圖9.6所示。圖9.6期望信號(hào)波形與維納濾波估計(jì)波形

9.2卡爾曼濾波

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

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

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

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

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

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

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

(9.33)

為書(shū)寫(xiě)方便，將變量k用下標(biāo)表示，則式(9.33)可寫(xiě)為

xk=Akxk－1+ωk－1

(9.34)9.2.2量測(cè)方程

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

yk=Ckxk+υk

(9.35)

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

yk=sk+υk

(9.36)

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

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

xk=Akxk－1+ωk－1

(9.37)

yk=Ckxk+υk

(9.38)

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

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

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

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

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

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

【例9.5】

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

MATLAB程序如下:

%MATLABPROGRAM9-5

clc;

T=2;

num=100;

%真實(shí)軌跡

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];

%卡爾曼濾波開(kāi)始

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(′真實(shí)軌跡′，′觀測(cè)樣本′，′估計(jì)軌跡′);

figure(2);

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

9.3.1目標(biāo)跟蹤的卡爾曼濾波

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

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

(9.49)

式中，

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

【例9.6】利用卡爾曼濾波對(duì)飛行物進(jìn)行目標(biāo)跟蹤。飛行物初始時(shí)刻距雷達(dá)站距離為30km，初始速度為300m/s，加速度為20m/s2，采樣周期T=0.5s，試編程給出仿真分析結(jié)果。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(′運(yùn)動(dòng)軌跡的比較′);

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

figure(2);

plot(k，B(2，k)，′b*′，k，A(2，k)，′r.′);title(′運(yùn)動(dòng)速度的比較′);

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

figure(3);

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

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

程序運(yùn)行結(jié)果如圖9.9所示。圖9.9目標(biāo)跟蹤的卡爾曼濾波9.3.2機(jī)動(dòng)模型的濾波跟蹤

對(duì)于機(jī)動(dòng)模型(恒加速模型)來(lái)說(shuō)。若目標(biāo)以恒定的加速度在運(yùn)動(dòng)，則其狀態(tài)方程可表示為

(9.51)式中，

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

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

(9.53)

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

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

【例9.7】

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

MATLAB程序如下:

%MATLABPROGRAM9-7

clear;

T=2;

%雷達(dá)掃描周期

num=50;

%濾波次數(shù)

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

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;%真實(shí)軌跡

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];

%卡爾曼濾波開(kāi)始

forn=3:N5;

if(u<=40)%非機(jī)動(dòng)模型、賦初值

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)%啟動(dòng)機(jī)動(dòng)檢測(cè)

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)%退出機(jī)動(dòng)判斷

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

%計(jì)算濾波誤差的均值以及標(biāo)準(zhǔn)差

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;