卡爾曼濾波器

1 簡(jiǎn)介(Brief Introduction)

在學(xué)習(xí)卡爾曼濾波器之前，首先看看為什么叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級(jí)數(shù)等等）一樣，卡爾曼也是一個(gè)人的名字，而跟他們不同的是，他是個(gè)現(xiàn)代人！

卡爾曼全名Rudolf Emil Kalman，匈牙利數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953，1954年于麻省理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。我們現(xiàn)在要學(xué)習(xí)的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文《A New Approach to Linear Filtering and Prediction Problems》（線性濾波與預(yù)測(cè)問(wèn)題的新方法）。如果對(duì)這編論文有興趣，可以到這里的地址下載閱讀：論文

卡爾曼濾波器到底是干嘛的？我們來(lái)看下wiki上的解釋：

卡爾曼濾波的一個(gè)典型實(shí)例是從一組有限的，包含噪聲的，對(duì)物體位置的觀察序列（可能有偏差）預(yù)測(cè)出物體的位置的坐標(biāo)及速度。在很多工程應(yīng)用(如雷達(dá)、計(jì)算機(jī)視覺(jué))中都可以找到它的身影。同時(shí)，卡爾曼濾波也是控制理論以及控制系統(tǒng)工程中的一個(gè)重要課題。例如,對(duì)于雷達(dá)來(lái)說(shuō)，人們感興趣的是其能夠跟蹤目標(biāo)。但目標(biāo)的位置、速度、加速度的測(cè)量值往往在任何時(shí)候都有噪聲?？柭鼮V波利用目標(biāo)的動(dòng)態(tài)信息，設(shè)法去掉噪聲的影響，得到一個(gè)關(guān)于目標(biāo)位置的好的估計(jì)。這個(gè)估計(jì)可以是對(duì)當(dāng)前目標(biāo)位置的估計(jì)(濾波)，也可以是對(duì)于將來(lái)位置的估計(jì)(預(yù)測(cè))，也可以是對(duì)過(guò)去位置的估計(jì)(插值或平滑)。

斯坦利.施密特(Stanley Schmidt)首次實(shí)現(xiàn)了卡爾曼濾波器?？柭贜ASA埃姆斯研究中心訪問(wèn)時(shí)，發(fā)現(xiàn)他的方法對(duì)于解決阿波羅計(jì)劃的軌道預(yù)測(cè)很有用，后來(lái)阿波羅飛船的導(dǎo)航電腦便使用了這種濾波器。 關(guān)于這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發(fā)表。

目前,卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn).卡爾曼最初提出的形式現(xiàn)在一般稱為簡(jiǎn)單卡爾曼濾波器。除此以外，還有施密特?cái)U(kuò)展濾波器、信息濾波器以及很多Bierman, Thornton 開(kāi)發(fā)的平方根濾波器的變種。也許最常見(jiàn)的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛存在。

簡(jiǎn)單來(lái)說(shuō)，卡爾曼濾波器是一個(gè)“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對(duì)于解決很大部分的問(wèn)題，他是最優(yōu)，效率最高甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過(guò)30年，包括機(jī)器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等。近年來(lái)更被應(yīng)用于計(jì)算機(jī)圖像處理，例如頭臉識(shí)別，圖像分割，圖像邊緣檢測(cè)等等。

說(shuō)起Kalman濾波器的歷史

最早要追溯到17世紀(jì)，Roger Cotes開(kāi)始研究最小均方問(wèn)題。但由于缺少實(shí)際案例的支撐（那個(gè)時(shí)候哪來(lái)那么多雷達(dá)啊啥的這些信號(hào)啊），Cotes的研究讓人看著顯得很模糊，因此在估計(jì)理論的發(fā)展中影響很小。17世紀(jì)中葉，最小均方估計(jì)（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用于月球運(yùn)動(dòng)的估計(jì)，Leonard Euler在1749年、Pierre Laplace在1787分別用于木星和土星的運(yùn)動(dòng)估計(jì)。Roger Boscovich在1755用最小均方估計(jì)地球的大小。1777年，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發(fā)明了最大似然估計(jì)算法。遞歸的最小均方估計(jì)理論是由Karl Gauss建立在1809年（好吧，他聲稱在1795年就完成了），當(dāng)時(shí)還有Adrien Legendre在1805年完成了這項(xiàng)工作，Robert Adrain在1808年完成的，至于到底誰(shuí)是Boss，矮子們就別管了吧！

在1880年，丹麥的天文學(xué)家Thorvald Nicolai Thiele在之前最小均方估計(jì)的基礎(chǔ)上開(kāi)發(fā)了一個(gè)遞歸算法，與Kalman濾波非常相似。在某些標(biāo)量的情況下，Thiele的濾波器與Kalman濾波器時(shí)等價(jià)的，Thiele提出了估計(jì)過(guò)程噪聲和測(cè)量噪聲中方差的方法（過(guò)程噪聲和測(cè)量噪聲是Kalman濾波器中關(guān)鍵的概念）。

上面提到的這么多研究估計(jì)理論的先驅(qū)，大多是天文學(xué)家而非數(shù)學(xué)家。現(xiàn)在，大部分的理論貢獻(xiàn)都源自于實(shí)際的工程。“There is nothing so practical as a good theory”，應(yīng)該就是“實(shí)踐是檢驗(yàn)真理的唯一標(biāo)準(zhǔn)”之類吧。

現(xiàn)在，我們的控制論大Wiener終于出場(chǎng)了，還有那個(gè)叫Kolmogorov（柯?tīng)柲曷宸?#xff09;的神人。在19世紀(jì)40年代，Wiener設(shè)計(jì)了Wiener濾波器，然而，Wiener濾波器不是在狀態(tài)空間進(jìn)行的（這個(gè)學(xué)過(guò)Wiener濾波的就知道，它是直接從觀測(cè)空間z(n)=s(n)+w(n)進(jìn)行的濾波），Wiener是穩(wěn)態(tài)過(guò)程，它假設(shè)測(cè)量是通過(guò)過(guò)去無(wú)限多個(gè)值估計(jì)得到的。Wiener濾波器比Kalman濾波器具有更高的自然統(tǒng)計(jì)特性。這些也限制其只是更接近理想的模型，要直接用于實(shí)際工程中需要足夠的先驗(yàn)知識(shí)（要預(yù)知協(xié)方差矩陣），美國(guó)NASA曾花費(fèi)多年的時(shí)間研究維納理論，但依然沒(méi)有在空間導(dǎo)航中看到維納理論的實(shí)際應(yīng)用。

在1950末期，大部分工作開(kāi)始對(duì)維納濾波器中協(xié)方差的先驗(yàn)知識(shí)通過(guò)狀態(tài)空間模型進(jìn)行描述。通過(guò)狀態(tài)空間表述后的算法就和今天看到的Kalman濾波已經(jīng)極其相似了。Johns Hopkins大學(xué)首先將這個(gè)算法用在了導(dǎo)彈跟蹤中，那時(shí)在RAND公司工作的Peter Swerling將它用在了衛(wèi)星軌道估計(jì)，Swerling實(shí)際上已經(jīng)推導(dǎo)出了（1959年發(fā)表的）無(wú)噪聲系統(tǒng)動(dòng)力學(xué)的Kalman濾波器，在他的應(yīng)用中，他還考慮了使用非線性系統(tǒng)動(dòng)力學(xué)和和測(cè)量方程?？梢赃@樣說(shuō)，Swerling和發(fā)明Kalman濾波器是失之交臂，一線之隔。在kalman濾波器聞名于世之后，他還寫(xiě)信到AIAA Journal聲討要獲得Kalman濾波器發(fā)明的榮譽(yù)（然而這時(shí)已經(jīng)給濾波器命名Kalman了）。總結(jié)其失之交臂的原因，主要是Swerling沒(méi)有直接在論文中提出Kalman濾波器的理論，而只是在實(shí)踐中應(yīng)用。

Rudolph Kalman在1960年發(fā)現(xiàn)了離散時(shí)間系統(tǒng)的Kalman濾波器，這就是我們?cè)诮裉旄鞣N教材上都能看到的，1961年Kalman和Bucy又推導(dǎo)了連續(xù)時(shí)間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計(jì)的角度推導(dǎo)出了Kalman濾波器方程。

目前，卡爾曼濾波已經(jīng)有很多不同的實(shí)現(xiàn)?？柭畛跆岢龅男问浆F(xiàn)在一般稱為簡(jiǎn)單卡爾曼濾波器。除此以外，還有施密特?cái)U(kuò)展濾波器、信息濾波器以及很多Bierman, Thornton開(kāi)發(fā)的平方根濾波器的變種。也許最常見(jiàn)的卡爾曼濾波器是鎖相環(huán)，它在收音機(jī)、計(jì)算機(jī)和幾乎任何視頻或通訊設(shè)備中廣泛存在。

2 卡爾曼濾波器的介紹 (Introduction to the Kalman Filter)

為了可以更加容易的理解卡爾曼濾波器，這里會(huì)應(yīng)用形象的描述方法來(lái)講解，而不是像大多數(shù)參考書(shū)那樣羅列一大堆的數(shù)學(xué)公式和數(shù)學(xué)符號(hào)。
但是，他的5條公式是其核心內(nèi)容。結(jié)合現(xiàn)代的計(jì)算機(jī)，其實(shí)卡爾曼的程序相當(dāng)?shù)暮?jiǎn)單，只要你理解了他的那5條公式。

在介紹他的5條公式之前，先讓我們來(lái)根據(jù)下面的例子一步一步的探索。

假設(shè)我們要研究的對(duì)象是一個(gè)房間的溫度。根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度(假設(shè)我們用一分鐘來(lái)做時(shí)間單位)。假設(shè)你對(duì)你的經(jīng)驗(yàn)不是100%的相信，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲(White Gaussian Noise)，也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合高斯分配(Gaussian Distribution)。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。

好了，現(xiàn)在對(duì)于某一分鐘我們有兩個(gè)有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗(yàn)的預(yù)測(cè)值(系統(tǒng)的預(yù)測(cè)值)和溫度計(jì)的值(測(cè)量值)。下面我們要用這兩個(gè)值結(jié)合他們各自的噪聲來(lái)估算出房間的實(shí)際溫度值。

假如我們要估算k時(shí)刻的是實(shí)際溫度值。首先你要根據(jù)k-1時(shí)刻的溫度值，來(lái)預(yù)測(cè)k時(shí)刻的溫度。因?yàn)槟阆嘈艤囟仁呛愣ǖ?#xff0c;所以你會(huì)得到k時(shí)刻的溫度預(yù)測(cè)值是跟k-1時(shí)刻一樣的，假設(shè)是23度，同時(shí)該值的高斯噪聲的偏差是5度(5是這樣得到的：如果k-1時(shí)刻估算出的最優(yōu)溫度值的偏差是3，你對(duì)自己預(yù)測(cè)的不確定度是4度，他們平方相加再開(kāi)方，就是5)。然后，你從溫度計(jì)那里得到了k時(shí)刻的溫度值，假設(shè)是25度，同時(shí)該值的偏差是4度。

由于我們用于估算k時(shí)刻的實(shí)際溫度有兩個(gè)溫度值，分別是23度和25度。究竟實(shí)際溫度是多少呢？相信自己還是相信溫度計(jì)呢？究竟相信誰(shuí)多一點(diǎn)，我們可以用他們的covariance(協(xié)方差)來(lái)判斷。因?yàn)镵g^2=5^2/(5^2+4^2)，所以Kg=0.78，我們可以估算出k時(shí)刻的實(shí)際溫度值是：23+0.78*(25-23)=24.56度。可以看出，因?yàn)闇囟扔?jì)的covariance比較小(比較相信溫度計(jì))，所以估算出的最優(yōu)溫度值偏向溫度計(jì)的值。

現(xiàn)在我們已經(jīng)得到k時(shí)刻的最優(yōu)溫度值了，下一步就是要進(jìn)入k+1時(shí)刻，進(jìn)行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒(méi)看到什么自回歸的東西出現(xiàn)。對(duì)了，在進(jìn)入k+1時(shí)刻之前，我們還要算出k時(shí)刻那個(gè)最優(yōu)值(24.56度)的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。這里的5就是上面的k時(shí)刻你預(yù)測(cè)的那個(gè)23度溫度值的偏差，得出的2.35就是進(jìn)入k+1時(shí)刻以后k時(shí)刻估算出的最優(yōu)溫度值的偏差(對(duì)應(yīng)于上面的3)。

就是這樣，卡爾曼濾波器就不斷的把covariance遞歸，從而估算出最優(yōu)的溫度值。他運(yùn)行的很快，而且它只保留了上一時(shí)刻的covariance。上面的Kg，就是卡爾曼增益(Kalman Gain)。他可以隨不同的時(shí)刻而改變他自己的值，是不是很神奇！

---

到這里，應(yīng)該對(duì)Kalman濾波有個(gè)總體的概念了，有幾個(gè)觀點(diǎn)很重要，是建立Kalman濾波器的基礎(chǔ)：

• 一個(gè)是n-1時(shí)刻對(duì)n時(shí)刻的估計(jì)值，一個(gè)是n時(shí)刻的測(cè)量值，估計(jì)值和測(cè)量值都存在誤差，且誤差都假設(shè)滿足獨(dú)立的高斯分布。
• Kalman濾波器就是充分結(jié)合了估計(jì)值和測(cè)量值得到n時(shí)刻更接近真值的估計(jì)結(jié)果
• Kalman濾波器引入狀態(tài)空間的目的是避免了“像Wiener濾波器一樣需要對(duì)過(guò)去所有[0,n-1]時(shí)刻協(xié)方差先驗(yàn)知識(shí)都已知”，而直接可以通過(guò)上一時(shí)刻即n-1時(shí)刻的狀態(tài)信息和均方誤差信息就可遞推得到n時(shí)刻的估計(jì)。盡管遞推使得實(shí)際應(yīng)用中方便了，但n-1時(shí)刻對(duì)n時(shí)刻的估計(jì)實(shí)際上使用到了所有前[0,n-1]時(shí)刻的信息，只不過(guò)信息一直通過(guò)最小均方誤差進(jìn)行傳遞到n-1時(shí)刻。基于此，Kalman濾波也需要先驗(yàn)知識(shí)，即-1時(shí)刻的初始值。

在上小節(jié)中只看到Kalman的結(jié)論，那么Kalman濾波器是如何將估計(jì)值和測(cè)量值結(jié)合起來(lái)，如何將信息傳遞下去的呢？這其中，“獨(dú)立高斯分布”的假設(shè)條件功勞不可謂不大！測(cè)量值z(mì)(n)~N(uz,σz^2)，估計(jì)值x(n)~N(ux,σx^2)。

Kalman濾波器巧妙的用“獨(dú)立高斯分布的乘積”將這兩個(gè)測(cè)量值和估計(jì)值進(jìn)行融合！

下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。

3 卡爾曼濾波器算法 (The Kalman Filter Algorithm)

在這一部分，我們就來(lái)描述源于Dr. Kalman 的卡爾曼濾波器。

下面的描述，會(huì)涉及一些基本的概念知識(shí)，包括概率(Probability)，隨即變量(Random Variable)，高斯或正態(tài)分配(Gaussian Distribution)還有State-space Model等等。
但對(duì)于卡爾曼濾波器的詳細(xì)證明，這里不能一一描述。

首先，我們先要引入一個(gè)離散控制過(guò)程的系統(tǒng)。
該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程(Linear Stochastic Difference equation)來(lái)描述：X(k)=A X(k-1)+B U(k)+W(k), 再加上系統(tǒng)的測(cè)量值：Z(k)=H X(k)+V(k).

在上兩式子中，X(k)是k時(shí)刻的系統(tǒng)狀態(tài)，U(k)是k時(shí)刻對(duì)系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃嚒?
Z(k)是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過(guò)程和測(cè)量的噪聲。
他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R(這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化)。

對(duì)于滿足上面的條件(線性隨機(jī)微分系統(tǒng)，過(guò)程和測(cè)量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。

下面我們來(lái)用他們結(jié)合他們的covariances 來(lái)估算系統(tǒng)的最優(yōu)化輸出(類似上一節(jié)那個(gè)溫度的例子)。

首先我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在狀態(tài)：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒(méi)有控制量，它可以為0。

到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的covariance還沒(méi)更新。我們用P表示covariance：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的covariance，A’表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程的covariance。

式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。

現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在 狀態(tài)(k) 的最優(yōu)化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg為 卡爾曼增益(Kalman Gain) ：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要讓卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下的X(k|k)的covariance：

P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)

其中I 為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。

卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序。

下面，我會(huì)用程序舉一個(gè)實(shí)際運(yùn)行的例子。。。

4 簡(jiǎn)單例子 (A Simple Example)

這里我們結(jié)合第二第三節(jié)，舉一個(gè)非常簡(jiǎn)單的例子來(lái)說(shuō)明卡爾曼濾波器的工作過(guò)程。所舉的例子是進(jìn)一步描述第二節(jié)的例子，而且還會(huì)配以程序模擬結(jié)果。

根據(jù)第二節(jié)的描述，把房間看成一個(gè)系統(tǒng)，然后對(duì)這個(gè)系統(tǒng)建模。當(dāng)然，我們見(jiàn)的模型不需要非常地精確。我們所知道的這個(gè)房間的溫度是跟前一時(shí)刻的溫度相同的，所以A=1。沒(méi)有控制量，所以U(k)=0。因此得出
：
X(k|k-1)=X(k-1|k-1) ……….. (6)

式子(2)可以改成：

P(k|k-1)=P(k-1|k-1) +Q ……… (7)

因?yàn)闇y(cè)量的值是溫度計(jì)的，跟溫度直接對(duì)應(yīng)，所以H=1。
式子3，4，5可以改成以下：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)

Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)

P(k|k)=(1-Kg(k))P(k|k-1) ……… (10)

現(xiàn)在我們模擬一組測(cè)量值作為輸入。假設(shè)房間的真實(shí)溫度為25度，我模擬了200個(gè)測(cè)量值，這些測(cè)量值的平均值為25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白噪聲(在圖中為藍(lán)線)。

為了令卡爾曼濾波器開(kāi)始工作，我們需要告訴卡爾曼兩個(gè)零時(shí)刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個(gè)就可以了，因?yàn)殡S著卡爾曼的工作，X會(huì)逐漸的收斂。但是對(duì)于P，一般不要取0，因?yàn)檫@樣可能會(huì)令卡爾曼完全相信你給定的X(0|0)是系統(tǒng)最優(yōu)的，從而使算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。

該系統(tǒng)的真實(shí)溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優(yōu)化結(jié)果(該結(jié)果在算法中設(shè)置了Q=1e-6，R=1e-1)。

一段Matlab代碼是從網(wǎng)上找到的，程序簡(jiǎn)單直接，但作為學(xué)習(xí)分析用很棒:

% KALMANF - updates a system state vector estimate based upon an % observation, using a discrete Kalman filter. % % Version 1.0, June 30, 2004 % % This tutorial function was written by Michael C. Kleder % % INTRODUCTION % % Many people have heard of Kalman filtering, but regard the topic % as mysterious. While it's true that deriving the Kalman filter and % proving mathematically that it is "optimal" under a variety of % circumstances can be rather intense, applying the filter to % a basic linear system is actually very easy. This Matlab file is % intended to demonstrate that. % % An excellent paper on Kalman filtering at the introductory level, % without detailing the mathematical underpinnings, is: % "An Introduction to the Kalman Filter" % Greg Welch and Gary Bishop, University of North Carolina % http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html % % PURPOSE: % % The purpose of each iteration of a Kalman filter is to update % the estimate of the state vector of a system (and the covariance % of that vector) based upon the information in a new observation. % The version of the Kalman filter in this function assumes that % observations occur at fixed discrete time intervals. Also, this % function assumes a linear system, meaning that the time evolution % of the state vector can be calculated by means of a state transition % matrix. % % USAGE: % % s = kalmanf(s) % % "s" is a "system" struct containing various fields used as input % and output. The state estimate "x" and its covariance "P" are % updated by the function. The other fields describe the mechanics % of the system and are left unchanged. A calling routine may change % these other fields as needed if state dynamics are time-dependent; % otherwise, they should be left alone after initial values are set. % The exceptions are the observation vectro "z" and the input control % (or forcing function) "u." If there is an input function, then % "u" should be set to some nonzero value by the calling routine. % % SYSTEM DYNAMICS: % % The system evolves according to the following difference equations, % where quantities are further defined below: % % x = Ax + Bu + w meaning the state vector x evolves during one time % step by premultiplying by the "state transition % matrix" A. There is optionally (if nonzero) an input % vector u which affects the state linearly, and this % linear effect on the state is represented by % premultiplying by the "input matrix" B. There is also % gaussian process noise w. % z = Hx + v meaning the observation vector z is a linear function % of the state vector, and this linear relationship is % represented by premultiplication by "observation % matrix" H. There is also gaussian measurement % noise v. % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q % v ~ N(0,R) meaning v is gaussian noise with covariance R % % VECTOR VARIABLES: % % s.x = state vector estimate. In the input struct, this is the % "a priori" state estimate (prior to the addition of the % information from the new observation). In the output struct, % this is the "a posteriori" state estimate (after the new % measurement information is included). % s.z = observation vector % s.u = input control vector, optional (defaults to zero). % % MATRIX VARIABLES: % % s.A = state transition matrix (defaults to identity). % s.P = covariance of the state vector estimate. In the input struct, % this is "a priori," and in the output it is "a posteriori." % (required unless autoinitializing as described below). % s.B = input matrix, optional (defaults to zero). % s.Q = process noise covariance (defaults to zero). % s.R = measurement noise covariance (required). % s.H = observation matrix (defaults to identity). % % NORMAL OPERATION: % % (1) define all state definition fields: A,B,H,Q,R % (2) define intial state estimate: x,P % (3) obtain observation and control vectors: z,u % (4) call the filter to obtain updated state estimate: x,P % (5) return to step (3) and repeat % % INITIALIZATION: % % If an initial state estimate is unavailable, it can be obtained % from the first observation as follows, provided that there are the % same number of observable variables as state variables. This "auto- % intitialization" is done automatically if s.x is absent or NaN. % % x = inv(H)*z % P = inv(H)*R*inv(H') % % This is mathematically equivalent to setting the initial state estimate % covariance to infinity.function s = kalmanf(s)% set defaults for absent fields: if ~isfield(s,'x'); s.x=nan*z; end if ~isfield(s,'P'); s.P=nan; end if ~isfield(s,'z'); error('Observation vector missing'); end if ~isfield(s,'u'); s.u=0; end if ~isfield(s,'A'); s.A=eye(length(x)); end if ~isfield(s,'B'); s.B=0; end if ~isfield(s,'Q'); s.Q=zeros(length(x)); end if ~isfield(s,'R'); error('Observation covariance missing'); end if ~isfield(s,'H'); s.H=eye(length(x)); endif isnan(s.x)% initialize state estimate from first observationif diff(size(s.H))error('Observation matrix must be square and invertible for state autointialization.');ends.x = inv(s.H)*s.z;s.P = inv(s.H)*s.R*inv(s.H'); else% This is the code which implements the discrete Kalman filter:% Prediction for state vector and covariance:s.x = s.A*s.x + s.B*s.u;s.P = s.A * s.P * s.A' + s.Q;% Compute Kalman gain factor:K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);% Correction based on observation:s.x = s.x + K*(s.z-s.H*s.x);s.P = s.P - K*s.H*s.P;% Note that the desired result, which is an improved estimate% of the sytem state vector x and its covariance P, was obtained% in only five lines of code, once the system was defined. (That's% how simple the discrete Kalman filter is to use.) Later,% we'll discuss how to deal with nonlinear systems.endreturn

下面是一段測(cè)試代碼：

% Define the system as a constant of 12 volts: clear all s.x = 12; s.A = 1; % Define a process noise (stdev) of 2 volts as the car operates: s.Q = 2^2; % variance, hence stdev^2 % Define the voltimeter to measure the voltage itself: s.H = 1; % Define a measurement error (stdev) of 2 volts: s.R = 2^2; % variance, hence stdev^2 % Do not define any system input (control) functions: s.B = 0; s.u = 0; % Do not specify an initial state: s.x = nan; s.P = nan; % Generate random voltages and watch the filter operate. tru=[]; % truth voltage for t=1:20tru(end+1) = randn*2+12;s(end).z = tru(end) + randn*2; % create a measurements(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration end figure hold on grid on % plot measurement data: hz=plot([s(1:end-1).z],'r.'); % plot a-posteriori state estimates: hk=plot([s(2:end).x],'b-'); ht=plot(tru,'g-'); legend([hz hk ht],'observations','Kalman output','true voltage',0) title('Automobile Voltimeter Example') hold off

Kalman濾波C程序

我就在上面公式的基礎(chǔ)上實(shí)現(xiàn)了基本的Kalman濾波器，包括1維和2維狀態(tài)的情況。先在頭文件中聲明1維和2維Kalman濾波器結(jié)構(gòu)：

/** FileName : kalman_filter.h* Author : xiahouzuoxin @163.com* Version : v1.0* Date : 2014/9/24 20:37:01* Brief : * * Copyright (C) MICL,USTB*/ #ifndef _KALMAN_FILTER_H #define _KALMAN_FILTER_H/* * NOTES: n Dimension means the state is n dimension, * measurement always 1 dimension *//* 1 Dimension */ typedef struct {float x; /* state */float A; /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */float H; /* z(n)=H*x(n)+w(n),w(n)~N(0,r) */float q; /* process(predict) noise convariance */float r; /* measure noise convariance */float p; /* estimated error convariance */float gain; } kalman1_state;/* 2 Dimension */ typedef struct {float x[2]; /* state: [0]-angle [1]-diffrence of angle, 2x1 */float A[2][2]; /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */float H[2]; /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2 */float q[2]; /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */float r; /* measure noise convariance */float p[2][2]; /* estimated error convariance,2x2 [p0 p1; p2 p3] */float gain[2]; /* 2x1 */ } kalman2_state; extern void kalman1_init(kalman1_state *state, float init_x, float init_p); extern float kalman1_filter(kalman1_state *state, float z_measure); extern void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]); extern float kalman2_filter(kalman2_state *state, float z_measure);#endif /*_KALMAN_FILTER_H*/

源碼給了有詳細(xì)的注釋，kalman1_state是狀態(tài)空間為1維/測(cè)量空間1維的Kalman濾波器，kalman2_state是狀態(tài)空間為2維/測(cè)量空間1維的Kalman濾波器。

兩個(gè)結(jié)構(gòu)體都需要通過(guò)初始化函數(shù)初始化相關(guān)參數(shù)、狀態(tài)值和均方差值。

/** FileName : kalman_filter.c* Author : xiahouzuoxin @163.com* Version : v1.0* Date : 2014/9/24 20:36:51* Brief : * * Copyright (C) MICL,USTB*/#include "kalman_filter.h"/** @brief * Init fields of structure @kalman1_state.* I make some defaults in this init function:* A = 1;* H = 1; * and @q,@r are valued after prior tests.** NOTES: Please change A,H,q,r according to your application.** @inputs * state - Klaman filter structure* init_x - initial x state value * init_p - initial estimated error convariance* @outputs * @retval */ void kalman1_init(kalman1_state *state, float init_x, float init_p) {state->x = init_x;state->p = init_p;state->A = 1;state->H = 1;state->q = 2e2;//10e-6; /* predict noise convariance */state->r = 5e2;//10e-5; /* measure error convariance */ }/** @brief * 1 Dimension Kalman filter* @inputs * state - Klaman filter structure* z_measure - Measure value* @outputs * @retval * Estimated result*/ float kalman1_filter(kalman1_state *state, float z_measure) {/* Predict */state->x = state->A * state->x;state->p = state->A * state->A * state->p + state->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q *//* Measurement */state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);state->x = state->x + state->gain * (z_measure - state->H * state->x);state->p = (1 - state->gain * state->H) * state->p;return state->x; }/** @brief * Init fields of structure @kalman1_state.* I make some defaults in this init function:* A = {{1, 0.1}, {0, 1}};* H = {1,0}; * and @q,@r are valued after prior tests. ** NOTES: Please change A,H,q,r according to your application.** @inputs * @outputs * @retval */ void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]) {state->x[0] = init_x[0];state->x[1] = init_x[1];state->p[0][0] = init_p[0][0];state->p[0][1] = init_p[0][1];state->p[1][0] = init_p[1][0];state->p[1][1] = init_p[1][1];//state->A = {{1, 0.1}, {0, 1}};state->A[0][0] = 1;state->A[0][1] = 0.1;state->A[1][0] = 0;state->A[1][1] = 1;//state->H = {1,0};state->H[0] = 1;state->H[1] = 0;//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */state->q[0] = 10e-7;state->q[1] = 10e-7;state->r = 10e-7; /* estimated error convariance */ }/** @brief * 2 Dimension kalman filter* @inputs * state - Klaman filter structure* z_measure - Measure value* @outputs * state->x[0] - Updated state value, Such as angle,velocity* state->x[1] - Updated state value, Such as diffrence angle, acceleration* state->p - Updated estimated error convatiance matrix* @retval * Return value is equals to state->x[0], so maybe angle or velocity.*/ float kalman2_filter(kalman2_state *state, float z_measure) {float temp0 = 0.0f;float temp1 = 0.0f;float temp = 0.0f;/* Step1: Predict */state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];/* p(n|n-1)=A^2*p(n-1|n-1)+q */state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];/* Step2: Measurement *//* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];temp = state->r + state->H[0] * temp0 + state->H[1] * temp1;state->gain[0] = temp0 / temp;state->gain[1] = temp1 / temp;/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp); state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];return state->x[0]; }

其實(shí)，Kalman濾波器由于其遞推特性，實(shí)現(xiàn)起來(lái)很簡(jiǎn)單。但調(diào)參有很多可研究的地方，主要需要設(shè)定的參數(shù)如下：

init_x：待測(cè)量的初始值，如有中值一般設(shè)成中值（如陀螺儀）
init_p：后驗(yàn)狀態(tài)估計(jì)值誤差的方差的初始值
q：預(yù)測(cè)（過(guò)程）噪聲方差
r：測(cè)量（觀測(cè)）噪聲方差。以陀螺儀為例，測(cè)試方法是：保持陀螺儀不動(dòng)，統(tǒng)計(jì)一段時(shí)間內(nèi)的陀螺儀輸出數(shù)據(jù)。數(shù)據(jù)會(huì)近似正態(tài)分布，按3σ原則，取正態(tài)分布的(3σ)^2作為r的初始化值。
其中q和r參數(shù)尤為重要，一般得通過(guò)實(shí)驗(yàn)測(cè)試得到。

找兩組聲陣列測(cè)向的角度數(shù)據(jù)，對(duì)上面的C程序進(jìn)行測(cè)試。一維Kalman（一維也是標(biāo)量的情況，就我所知，現(xiàn)在網(wǎng)上看到的代碼大都是使用標(biāo)量的情況）和二維Kalman（一個(gè)狀態(tài)是角度值，另一個(gè)狀態(tài)是向量角度差，也就是角速度）的結(jié)果都在圖中顯示。這里再稍微提醒一下：狀態(tài)量不要取那些能突變的量，如加速度.

上面所有C程序的源代碼及測(cè)試程序都公布在原作者的Github上。

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎(jiǎng)勵(lì)來(lái)咯，堅(jiān)持創(chuàng)作打卡瓜分現(xiàn)金大獎(jiǎng)

總結(jié)

以上是生活随笔為你收集整理的卡尔曼滤波器(Kalman Filter) 理解的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。