卡爾曼濾波器（Kalman

Filter）濾波是什么？所謂濾波，就是從混合在一起的諸多信號中提取出所需要的信號。信號的分類（數(shù)學(xué)關(guān)系）？（1）確定性信號：可以表示為確定的時間函數(shù)，可確定其在任何時刻的量值。（具有確定的頻譜）（2）隨機(jī)信號：不能用確定的數(shù)學(xué)關(guān)系式來描述的，不能預(yù)測其未來任何瞬時值，其值的變化服從統(tǒng)計規(guī)律。（頻譜不確定，功率譜確定）確定性信號的濾波可采用低通、高通、帶通、帶阻等模擬濾波器或者計算機(jī)通過算法實現(xiàn)——常規(guī)濾波隨機(jī)信號的濾波根據(jù)有用信號和干擾信號的功率譜設(shè)計濾波器——維納濾波（Wiener

Filtering）或卡爾曼濾波（Kalman

Filter）隨機(jī)信號的濾波也可以看做是估計問題?？柭鼮V波的由來卡爾曼，全名RudolfEmil

Kalman，匈牙利數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953，1954年于麻省理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。我們在現(xiàn)代控制理論中要學(xué)習(xí)的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文《ANewApproachtoLinearFiltering

andPredictionProblems》（線性濾波與預(yù)測問題的新方法）?？柭鼮V波的由來卡爾曼濾波理論作為最優(yōu)估計的一種，它的創(chuàng)立是科學(xué)技術(shù)和社會需要發(fā)展到一定程度的必然結(jié)果。在1795年，高斯為測定行星運動軌道而提出最小二乘估計法。為了解決火力控制系統(tǒng)精度跟蹤問題，維納于1942年提出了維納濾波理論，利用有用信號和干擾信號的功率譜確定線性濾波器的頻率特性，首次將數(shù)理統(tǒng)計理論與線性理論有機(jī)的聯(lián)系在一起，形成了對隨機(jī)信號做平滑、估計或者預(yù)測的最優(yōu)估計新理論。但是采用頻域設(shè)計法是造成維納濾波器設(shè)計困難的根本原因。于是，人們逐漸轉(zhuǎn)向?qū)で笤跁r域內(nèi)直接設(shè)計最優(yōu)濾波器的方法，而卡爾曼研究的卡爾曼濾波理論很好的解決了這個問題卡爾曼濾波器是什么？簡單的說，卡爾曼濾波器是一個“optimal

recursive

dataprocessing

algorithm(最優(yōu)化自回歸數(shù)據(jù)處理算法)”。從形式上，卡爾曼濾波器是5條公式。對于解決很大部分的問題，他是最優(yōu)，效率最高甚至

是最有用的。他的廣泛應(yīng)用已經(jīng)超過了30年，包括機(jī)器人

導(dǎo)航、控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)

以及導(dǎo)彈追蹤等等。而近年來更被應(yīng)用于計算機(jī)圖像處理，例如頭臉識別、圖像分割、圖像邊緣檢測等等?？柭鼮V波的特點卡爾曼濾波處理的對象是隨機(jī)信號；被處理的信號無有用和干擾之分，濾波的目的是要估計出所有被處理的信號(區(qū)別于維納濾波)；系統(tǒng)的白噪聲激勵和測量噪聲并不是需要濾除的對象，它們的統(tǒng)計特性是估計過程中需要利用的信息；（區(qū)別最小二乘）算法是遞推的，且使用狀態(tài)空間法在時域內(nèi)設(shè)計濾波器，適用于對多維隨機(jī)過程的估計；被估計量既可以是平穩(wěn)的，也可以是非平穩(wěn)的；估計過程中，只需要考慮過程噪聲和測量噪聲及當(dāng)前時刻系統(tǒng)狀態(tài)的統(tǒng)計特性。（計算機(jī)計算時，所占空間?。┧悸稰art

1線性系統(tǒng)的卡爾曼濾波方程線性離散系統(tǒng)線性連續(xù)系統(tǒng)Part

2非線性系統(tǒng)的卡爾曼濾波方程擴(kuò)展卡爾曼濾波器EKF無跡卡爾曼濾波器UKF卡爾曼濾波的基本思想在海圖作業(yè)中，航海長通常以前一時刻的船位為基準(zhǔn)，根據(jù)航向、船速和海流等一系列因素推算下一個船位，但是他并不輕易認(rèn)為

船位就一定在推算船位上，還要選擇適當(dāng)?shù)姆椒ǎㄟ^儀器得到另一個推算船位。觀測和推算這兩個船位一般不重合，航海長需要通過分析和判斷選擇一個可靠的船位，作為船艦當(dāng)前的位置。觀測變量

z

，再在預(yù)測與觀測之間進(jìn)行分析，或者k最優(yōu)狀態(tài)估計

x

。以K-1時刻的最優(yōu)估計xk

-1為準(zhǔn)，預(yù)測K時刻的狀態(tài)變量x?k

/k

-1

，同時又對該狀態(tài)進(jìn)行觀測，得到k說是以觀測量對預(yù)測量進(jìn)行修正，從而得到K

時刻的卡爾曼濾波思想例子假設(shè)我們要研究一個房間的溫度，以一分鐘為時間單位。根

據(jù)我們的經(jīng)驗判斷，這個房間的溫度是恒定的，但是對我們

的經(jīng)驗不是完全相信，可能存在上下幾度的偏差，我們把該

偏差看做是高斯白噪聲。另外，我們在房間里放一個溫度計，溫度計也不準(zhǔn)確，測量值會與實際值存在偏差，我們也把這

偏差看做是高斯白噪聲?，F(xiàn)在，我們要根據(jù)我們的經(jīng)驗溫度

和溫度計的測量值及它們各自的噪聲來估算出房間的實際溫

度。例子假如我們要估算k時刻的實際溫度值。首先你要根據(jù)k-1時刻的溫度值，來預(yù)測k時刻的溫度（K時刻的經(jīng)驗溫度）。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預(yù)測值是跟

k-1時刻一樣的，假設(shè)是23度（*公式一），同時該值（預(yù)測值）的高斯噪聲的偏差是5度（5是這樣得到的：如果k-1時刻估算出的最優(yōu)溫度值的偏差是3，你對自己預(yù)測的不確定度是4

度，他們平方相加再開方，就是5（*公式二））。然后，你從溫度計那里得到了k時刻的溫度值，假設(shè)是25度，同時該值的偏差是4度。例子現(xiàn)在，我們用于估算K時刻房間的實際溫度有兩個溫度值：估計值

23度和測量值25度。究竟實際溫度是多少呢？是相信自己還是相信溫度計？究竟相信誰多一點？我們需要用他們的均方誤差來判斷。因為，（*公式三），所以我們可以估算出K時刻的最優(yōu)溫度值為：得到了K時刻的最優(yōu)溫度，下一步就是對K+1時刻的溫度值進(jìn)行最優(yōu)估算，需要得到K時刻的最優(yōu)溫度（24.56）的偏差，算法如下：（*公式五）=爾2曼.35濾波器就不斷的把均方誤差遞歸，從而估算出最優(yōu)的溫度值，運行速度快，且只保留上一時刻的協(xié)方差。252H

=

H

=

0.7852

+

4223度+（0.7*8公*(式25四-2）3)。=24.56就(1這-樣H

)，*5卡2無控制離散型卡爾曼濾波器的基本公式系統(tǒng)的狀態(tài)方程：x

k系統(tǒng)的測量方程：=

fk

,k

-1

*x k

-1

+Gk

-1w k

-1Z

k

=

Ck

*x

k

+

v

kw k

-1系統(tǒng)測量方程的輸出量Z

(k如果

w k

-1

v

k

滿足為過程噪聲；v

k

為測量噪聲;

G

為噪聲驅(qū)動陣E

wk

=

0,Cov

wk

,

wj

=

Qkdkj

;E

[Vk

]=

0,Cov

Vk

,Vj

=

Rkdkj

;Cov

Wk

,Vj

=

0Qk

為過程噪聲的協(xié)方差，其為非負(fù)定陣；Rk

為測量噪聲的協(xié)方差，其為正定陣。是可以實際測量的量。無控制離散型卡爾曼濾波的基本方程P?k

/

k

-1

k

,k

-1

k

-1

k

,k

-1k

-1

k

-1

k

-1=

f

*fT*P

+

G

QGT（1）狀態(tài)的一步預(yù)測方程：x?k

/

k

-1

=

fk

,k

-1

*xk

-1（2）均方誤差的一步預(yù)測：?TkHk

=

P?

*Ck

/k

-1T-1k k

/

k

-1

kC

*P*C

+

Rk

=

x?k

/

k

-1

+

Hk

Zk

-

Ck

*x?k

/

k

-1xkk

/

k

-1Pk

=

I

-

Hk

*Ck

*P?（3）濾波增益方程（權(quán)重）：（4）濾波估計方程（K時刻的最優(yōu)值）：（5）均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：帶有控制的離散型卡爾曼濾波基本方程系統(tǒng)的狀態(tài)方程：

xk

=

fk

,k

-1xk

-1

+Gk

-1wk

-1

+

Bk

-1uk

-1系統(tǒng)的測量方程：Z

k

=

Ckx

k

+

v

k如果

w k

-1

v

k

滿足E

Wk

=

0,Cov

Wk

,Wj

=

Qkdkj

;E

[Vk

]=

0,Cov

Vk

,Vj

=

Rkdkj

;Cov

Wk

,Vj

=

0Qk

為過程噪聲的協(xié)方差，其為非負(fù)定陣；Rk

為測量噪聲的協(xié)方差，其為正定陣。P?k

/

k

-1

k

,k

-1

k

-1

k

,k

-1k

-1

k

-1

k

-1=

f

*fT*P

+

G

QGT（1）狀態(tài)的一步預(yù)測方程：x?k

/

k

-1

=

fk

,k

-1

*xk

-1

+

Bk

-1uk

-1（2）均方誤差的一步預(yù)測：?TkHk

=

P?

*Ck

/k

-1T-1k k

/

k

-1

kC

*P*C

+

Rk

=

x?k

/

k

-1

+

Hk

Zk

-

Ck

*x?k

/

k

-1

-

Ck

Bk

-1uk

-1xkk

/

k

-1Pk

=

I

-

Hk

*Ck

*P?（3）濾波增益方程（權(quán)重）：（4）濾波估計方程（K時刻的最優(yōu)值）：（5）濾波均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：帶有控制的離散型卡爾曼濾波基本方程線性離散型卡爾曼濾波方程的一般形式系統(tǒng)方程和測量方程的一般形式：xk

=

fk

,k

-1xk

-1

+

Bk

-1uk

-1

+Gk

-1wk

-1Zk

=

Ck

xk

+

vk如果

wk

-1vk滿足E

wk

=

0,Cov

wk

,

wj

=

Qkdkj

;E

[vk

]=

0,Cov

vk

,

v

j

=

Rkdkj

;Cov

wk

,

v

j

=

SkdkjQk

為過程噪聲的協(xié)方差，其為非負(fù)定陣；Rk

為測量噪聲的協(xié)方差，其為正定陣。k k

,k

-1

k

-1x

=

f*

x+

Bk

-1uk

-1+

J

Z

+

w*

;k

-1

k

-1

k

-1f*k

,k

-1k

,k

-1k

-1

k

-1k

-1

k

-1

k

-1k

-1

k

-1=

f

-

J

C

;

w*

=

Gw

-

J

vZk

=

Ck

xk

+

vk其中：，對狀態(tài)方程進(jìn)行等效變換：k引入矩陣Jk

k

k-1=G

S

R[

]***=

0kk

j

k

kj

k

jE

wE

v=

0,Cov w

,

w=

Q

d

;k

=

0,Cov

v

,

vk j

k

kj=

R

d

;Cov w

,

v

Qk

為過程噪聲的協(xié)方差，其為非負(fù)定陣；Rk

為測量噪聲的協(xié)方差，其為正定陣。一般形式的卡爾曼濾波方程*xk

-1

+

Bk

-1uk

-1k

-1

k

-1x?k

/

k

-1

k

,k

-1=

f*+

J

ZP?k

/

k

-1

k

,k

-1

k

-1

k

,k

-1k

-1

k

-1

k

-1=

f*

*(f**P

)T

+

G

QGT-

JST

GTk

-1

k

-1

k

-1?TkHk

=

P?

*Ck

/

k

-1T-1k k

/k

-1

kC

*P*C

+

Rk

=

x?k

/

k

-1

+

Bk

-1uk

-1

+

Jk

-1Zk

-1

+

Hk

Zk

-

Ck

*x?k

/

k

-1

-

Ck

Bk

-1uk

-1xkk

/

k

-1Pk

=

I

-

Hk

*Ck

*P?（1）狀態(tài)的一步預(yù)測方程：（2）均方誤差的一步預(yù)測：（3）濾波增益方程（權(quán)重）：（4）濾波估計方程（K時刻的最優(yōu)值）：（5）濾波均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：離散型卡爾曼濾波基本方程使用要點（1）濾波初值的選取卡爾曼濾波是一種遞推算法，啟動時必須先給初值x?0

,P0，卡爾曼濾波器是無偏的，即濾波穩(wěn)定，但是實際上這樣的初值很難得到；情況二：如果系統(tǒng)是一致完全隨機(jī)可控和一致完全隨機(jī)可觀測

的，則卡爾曼濾波器一定是一致漸近穩(wěn)定的，此時盲目的選取

濾波初值不影響最終估計值（大多數(shù)情況下）。情況一：一般情況下，取

x?0

=

E

x0,

P0

=

Cov

x0離散型卡爾曼濾波基本方程使用要點（2）估計均方誤差的等價形式及選用Pk

=(I

-

HkCk(

)(

);

(1)P?k/k-1(2)?TTk

kk

k

kPk

=(I

-HkCk

)P?k/k-1I

-H

C

+H

R

H

;----k-1-1T

-1k/k-1

k

k

kP

=

P+C

R

C

(3)公式（1）形式簡單，計算量小，但是積累誤差容易使協(xié)方差矩陣失去非負(fù)定性甚至對稱性，所以實際中常使用公式（2）；如果在濾波初值對被估計量的統(tǒng)計特性缺乏了解，選取濾波初值盲目，則宜采用公式（3）。離散型卡爾曼濾波基本方程使用要點（3）連續(xù)系統(tǒng)離散化卡爾曼濾波的基本方程只適用于系統(tǒng)方程和測量方程均為離散的情況，但實際的物理系統(tǒng)一般都是連續(xù)的，動力學(xué)特性用連續(xù)微分方程來描述，所以在使用基本方程之前，需要對系統(tǒng)方程和測量方程進(jìn)行離散化處理。連續(xù)系統(tǒng)的離散化處理包括對過程白噪聲的等效離散化處理。連續(xù)系統(tǒng)的卡爾曼濾波基本方程通過對實際的物理系統(tǒng)進(jìn)行分析后得到的系統(tǒng)模型一般為連續(xù)型的。連續(xù)型卡爾曼濾波方程可在離散型卡爾曼濾波器基本方程的基礎(chǔ)上推導(dǎo)出來?；舅悸罚簩⑦B續(xù)系統(tǒng)離散化，應(yīng)用離散型卡爾曼濾波器的基本方程和導(dǎo)數(shù)概念推導(dǎo)出連續(xù)型濾波方程。采用遞推算法是離散型卡爾曼濾波的最大優(yōu)點，算法可由計算機(jī)執(zhí)行，不必存儲時間過程中得大量測量信息。連續(xù)型卡爾曼濾波則根據(jù)連續(xù)時間過程中的測量值，采用求解矩陣微分方程的方法估計系統(tǒng)狀態(tài)變量的時間連續(xù)值，因此算法失去了遞推性。連續(xù)系統(tǒng)的狀態(tài)空間表達(dá)式為：x

t

=

A

t

x

t

+

B

t

u

t

+

G

t

w

tz

(t

)=

C

(t

)x

(t

)+

v

(t

)其中：E

w(t

=

0,Cov

w(t

,

w(t

=

Q

(t

d

(t

-t

;E

v

(t

)

=

0,Cov

v

(t

),

v

(t)

=

R

(t

)d

(t

-t);Cov

w(t

),

v

(t)

=

S

(t)d

(t

-t)Q

t

為非負(fù)定矩陣；R

t

為正定陣連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程與連續(xù)系統(tǒng)模型等效的離散系統(tǒng)的數(shù)學(xué)模型：x

(t

+

Dt

=

F

(t

+

Dt,t x

(t

+Y

(t

+

Dt,t u

(t

+G

(t

+

Dt,t W

(tz

(t

+

Dt

)=

C

(t

+

Dt

)x

(t

+

Dt

)+V

(t

+

Dt

)其中：

F

(t

+

Dt,t

=

I

+

A(t

Dt

+o

(Dt連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程Y

(t

+

Dt,t

)=

B

(t

)Dt

+o

(Dt

)G

(t

+

Dt,t

)=

G

(t

)Dt

+o

(Dt

)是零均值分段常值白噪聲過程，其協(xié)方差為：W

t

,V

tCov

W

(t

),V

(t)

=

S

(t

)kjkjCov

W

(t

),W

(t)

=

Q

t

d

;Cov

V

(t

),V

(t)

=

R

t

d

;

Dt

Dt

dkjDt;t=

t0

+

kDt,t

=

t0

+

jDt;

k,

j

=

0,1,

2連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程引入矩陣J來去除過程噪聲與測量噪聲的相關(guān)性J

(t

=

G

(t

+

Dt

t

S

(t R-1

(t（1）狀態(tài)的一步預(yù)測方程：x?(t

+

Dt

t

=

F

(t

+

Dt,t x

(t

+Y

(t

+

Dt,t u

(t

+

J

(t z

(t

-

C

(t x

(t（2）均方誤差的一步預(yù)測：P?

(t

+

D

t t

)=

F

(t

+

D

t

,

t

)-

J

(t

)C

(t

)

P

(t

)

F

(t

+

D

t

,

t

)-

J

(t

)C

(t

)

T+G

(t

+

D

t

,

t)(

)TTQ

(t

)R

t

J(

)

(t

)G

(t

+

D

t

,

t

)-

J

tD

t

D

tR

(t

)-1H

(t

+

Dt

)=

P?

(t

+

Dt t

)CT

(t

+

Dt

)C

(t

+

Dt

)P?

(t

+

Dt t

)CT

(t

+

Dt

)+Dt

（3）濾波增益方程（權(quán)重）：連續(xù)系統(tǒng)模型的卡爾曼濾波基本方程（4）濾波估計方程（K時刻的最優(yōu)值）：x

t

+

Dt

=

x

t

+

Dt

t

+

H t

+

Dt

z

t

+

Dt

t=

x

(t

)+{A(t

)x

(t

)+

B

(t

)u

(t

)+

G

(t

)S

(t

)R-1

(t

)

z

(t

)-

C

(t

)x

(t

)}Dt+H

(t

+

Dt

){z

(t

+

Dt

)-

C

(t

+

Dt

)

x

(t

)+[

A(t

)x

(t

)+

B

(t

)u

(t

)+

G

(t

)S

(t

)R-1

(t

)(z

(t

)-

C

(t

)x

(t

))

Dt]}將其變形求極限x

(t

=

A(t x

(t

+

B

(t u

(t

+

Hs

(t

z

(t

-

H

(t x

(t

Hs

(t

)=

H

(t

)+

G

(t

)S

(t

)R

(t

)-1（5）濾波均方誤差更新矩陣（K時刻的最優(yōu)均方誤差）：P

(t

+

Dt

=

I

-

H

(t

+

Dt C

(t

+

Dt

P

(t

+

Dt

t(

(

(

(

(

(

(

((

(sTst

HtT

T

-1=

A

t

P

t+

P

t

A

t

+

G

t

Q

t

G

t

-

H

t

RP

(t普通卡爾曼濾波是在線性高斯情況下利用最小均方誤差準(zhǔn)則獲得目標(biāo)的動態(tài)估計，適應(yīng)于過程和測量都屬于線性系統(tǒng)，且誤差符合高斯分布的系統(tǒng)。但是實際上很多系統(tǒng)都存在一定的非線性，表現(xiàn)在過程方程（狀態(tài)方程）是非線性的，或者觀測與狀態(tài)之間的關(guān)系（測量方程）是非線性的。這種情況下就不能使用一般的卡爾曼濾波了。解決的方法是將非線性關(guān)系進(jìn)行線性近似，將其轉(zhuǎn)化成線性問題。對于非線性問題線性化常用的兩大途徑：將非線性環(huán)節(jié)線性化，對高階項采用忽略或逼近措施；（EKF）用采樣方法近似非線性分布. (

UKF)擴(kuò)展卡爾曼濾波器（EKF）非線性系統(tǒng)模型：x

(t

=

f x

(t

,t

+

g x

(t

,t

w(tz

(t

)=

C

(x

(t

),t

)+

v

(t

)其中：E

w(t

=

0,Cov

w(t

,

w(t

=

Q

(t

d

(t

-t

;E

v

(t

)

=

0,Cov

v

(t

),

v

(t)

=

R

(t

)d

(t

-t);Cov

w(t

),

v

(t)

=

0假設(shè)在t

時刻已獲得系統(tǒng)狀態(tài)x的濾波估計

x?

t

，將f和C x

(t

,t

在

x?

t附近線性化，即非線性系統(tǒng)將隨時在新估計的結(jié)果附近進(jìn)行線性化。x

(t

,t擴(kuò)展卡爾曼濾波器（EKF）將

f x

(t

,t

和

C x

(t

,t

在

x

t附近展開成泰勒級數(shù)，忽略二階以上的高階項，則得線性化方程為：=

x?

t?x?(t

)x

(t

)=

f

(x?(t

),t

)+

?f

x?(t

,t

[x

(t

)-

x?(t

)]

+

g

(x?(t

),t

)w(t

)z

(t

)=

h

(x?(t

),t

)+

?C

(x?(t

),t

)[x

(t

)-

x?(t

)]

+

v

(t

)?x?(t

)?f

x?

t

,t

?C

x?

t

,tF

(t

)=

;

H

(t

)=

;G

(t

)=

g

(x?(t

),t

)?x?(t

)

?x?(t

)u

(t

)=

f

(x?(t

),t

)-

?f

(x?(t

),t

)x?(t

);?x?(t

)y

(t

)=

h

(x?(t

),t

)-

?C

(x?(t

),t

)x?(t

)?x?(t

)非線性系統(tǒng)線性化后的系統(tǒng)狀態(tài)空間表達(dá)式為：x

(t

=

F

(t x

(t

+

u

(t

+

G

(t

w(t

z

(t

)=

H

(t

)x

(t

)+

y(t)

+

v(t)將其變形，取EKF基本方程系統(tǒng)模型測量模型x

(t

=

f x

(t

,t

+

g x

(t

,t

w(t

,

w(t

~

N

0,Q

(t

z

(t

)=

C

(x

(t

),t

)+

v

(t

),

v

(t

)~

N

0,

R

(t

)初始條件其他規(guī)定x

(t

~

N

x?0

,

P0Cov

w(t

),

v

(t)

=

0狀態(tài)估計方程x?(t

=

F

(x?(t

,t

+

K

(t

z

(t

-

C

(x?(t

,t

誤差協(xié)方差P

(t

=

F

x?(t

,t P

(t

+

P

(