1、卡爾曼濾波器畢業(yè)論文1. 緒論1.1 概 述在濾波器的發(fā)展過程中，早期的維納濾波器涉及到對不隨時間變化的統(tǒng) 計特性的處理，即靜態(tài)處理。在這種信號處理過程中，有用信號和無用噪聲 的統(tǒng)計特性可與它們的頻率特性聯(lián)系起來，因此與經(jīng)典濾波器在概念上還有 一定的聯(lián)系。由于軍事上的需要，維納濾波器在第二次世界大戰(zhàn)期間得到了廣泛的應(yīng) 用。但是，維納濾波器有如下不足之處：第一，必須利用全部的歷史觀測數(shù) 據(jù)，存儲量和計算量都很大；第二，當(dāng)獲得新的觀測數(shù)據(jù)時，沒有合適的遞 推算法，必須進(jìn)行重新計算；第三，很難用于非平穩(wěn)過程的濾波。為了克服維 納濾 波器的上述不 足之處 ，卡爾 曼等人在維納濾波的 基礎(chǔ)上， 與 60

2、 年 代 初 提 出 了 一 種 遞 推 濾 波 方 法 ， 稱 為 卡 爾 曼 濾 波 。 與 維 納 濾 波 不 同 ， 卡爾曼濾波是對時變統(tǒng)計特性進(jìn)行處理。他不是從頻域，而是從時域的角度 出 發(fā) 來 考 慮 問 題 。 30 多 年 來 。 卡 爾 曼 已 在 各 個 領(lǐng) 域 得 到 了 廣 泛 的 應(yīng) 用 ， 包 括 機(jī)器人導(dǎo)航、控制、傳感器數(shù)據(jù)融合甚至軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤 等。近年來更被應(yīng)用于計算機(jī)圖象處理，例如頭臉識別、圖象分割、圖象邊 緣檢測等等。1.2 濾 波 器 的 發(fā) 展濾波器最初是指某種具有選頻特性的電子網(wǎng)絡(luò)，一般由線圈、電容器和 電阻器等元件組成。濾波器將使它

3、所容許通過的頻率范圍（即通帶）內(nèi)的電 信號產(chǎn)生較小的衰減，而使它所阻止通過的頻率范圍（即阻帶）內(nèi)的電信號 產(chǎn)生較大衰減。劃分通帶和阻帶的頻率，稱為濾波器的截止頻率。按 組 成 電 路 的 元 件 ， 濾 波 器 可 分 為LC、 RLC、 RC、 晶 體 和 陶 瓷 濾 波 器 等 。我們也可以用機(jī)械元件代替電子元件，制成機(jī)械式濾波器，或利用物質(zhì)的鐵 磁共振原理制成可點電調(diào)諧的濾波器。按容器通過的頻率范圍，濾波器可分為低通，高通，帶阻和帶通濾波器 等。具有選頻特性的串聯(lián)或并聯(lián)諧振回路，是一種常用的濾波器。收音機(jī)或 其他差式接收機(jī)中的中頻放大器，也是一中濾波器。也是一種濾波器。各級 中頻放大器中

4、回路靠放大器和變壓器來耦合，形成一定的通帶和阻帶。信號 在通過中放級時，通帶內(nèi)的成分將被放大，而阻帶內(nèi)的成分將大大衰減，而 且對通帶內(nèi)的信號還有放大作用。此外，調(diào)幅波接收機(jī)中的包絡(luò)是一種非線性濾波器。非線性濾波器實例 還有：自動增益控制電路，調(diào)頻接收機(jī)中的鎖相環(huán)以及近年來在組合音響裝 置 中 用 來 提 高 信 噪 比 的 Dobly 系 統(tǒng) 等 。上面所舉的這些濾波器，不論是線性還是非線性的，由于都是用來對模 擬信號進(jìn)行處理，故統(tǒng)稱為模擬濾波器或經(jīng)典濾波器。隨著集成電路技術(shù)的出現(xiàn)，特別是數(shù)字電子計算機(jī)的廣泛應(yīng)用，模擬濾 波 器 開 始 向 數(shù) 字 濾 波 器 方 向 發(fā) 展 。 A/D 或

5、D/A 轉(zhuǎn) 換 器 ， 移 位 寄 存 器 。 只 讀 存 儲器以及微處理機(jī)這樣一些與傳統(tǒng)的模擬濾波電路元件截然不同的電路元件 和模塊被廣泛應(yīng)用于數(shù)字濾波電路中，以適應(yīng)離散數(shù)字信號處理的要求。即 使 是 模 擬 信 號 ， 也 可 通 過 A/D 轉(zhuǎn) 換 先 變 成 離 散 的 數(shù) 字 信 號 ， 經(jīng) 相 應(yīng) 的 處 理 后 再恢復(fù)成模擬信號。與模擬濾波器相比，數(shù)字濾波器不僅可使體積縮小，成本降低，而且還 有如下優(yōu)點：第一，濾波器的參數(shù)可根據(jù)對濾波器性能指標(biāo)的要求來設(shè)定， 從而具有較高的精度；第二，濾波器的參數(shù)很容易重新設(shè)定或使具有自適應(yīng) 性；第三，有些采用微處理機(jī)的數(shù)字濾波器可實現(xiàn)對微處理機(jī)

6、的分時使用， 從而大大提高工作效率。經(jīng) 典 濾 波 器 的 另 一 發(fā) 展 方 向 ，就 是 利 用 統(tǒng) 計 理 論 來 處 理 濾 波 問 題 ，由 此 ， 產(chǎn)生了統(tǒng)計濾波器。從經(jīng)典濾波的觀點來看，有用信號和噪聲信號是分布在不同頻帶之內(nèi)。 因此，我們可用具有一定選頻特性的經(jīng)典濾波網(wǎng)絡(luò)把噪聲盡可能地濾除，而 保留畸變不大的有用信號。但是，我們所遇到的信號和噪聲有時可能是隨機(jī) 的，其特性往往只能從統(tǒng)計的意義上來描述。例如，在導(dǎo)彈控制系統(tǒng)中，由 于目標(biāo)運動的隨機(jī)性，目標(biāo)的位置和速度都是隨機(jī)的。此外，測量裝置也會 有隨機(jī)噪聲。此時，我們就不可能采用一般的經(jīng)典濾波器把有用信號從測量 結(jié)果中分離出來，而

7、只能用統(tǒng)計估算方法給出有用信號的最優(yōu)估計值。從統(tǒng) 計 的 觀 點 來 看 ，一 個 濾 波 器 的 輸 出 越 接 近 實 際 有 用 信 號 ，這 個 濾 波 器 就 越 好 。 也就是說，最優(yōu)濾波器是輸出最接近于實際有用的信號的濾波器。1.3 卡爾曼濾波器的基本思想卡爾曼濾波是線性無偏最小均方誤差遞推濾波器。與維納濾波相比，在 平穩(wěn)條件下，它們所得到的穩(wěn)態(tài)結(jié)果是一致的。然而，它們解決的方法有很 大區(qū)別。維納濾波是根據(jù)全部過去的和當(dāng)前的觀察數(shù)據(jù) 來估計信號的當(dāng)前 值 ， 它 的 解 是 以 均 方 誤 差 最 小 條 件 下 所 得 到 的 系 統(tǒng) 的 傳 遞 函 數(shù) H(z) 或 單 位

8、樣 本 響 應(yīng) h(n) 的 形 式 給 出 的 ， 因 此 稱 這 種 系 統(tǒng) 為 最 佳 線 性 過 濾 器 或 濾 波 器 。 而 卡爾曼過濾是用前一個估計值和最近一個觀察數(shù)據(jù)來估計信號的當(dāng)前值，是 用狀態(tài)方程和遞推的方法進(jìn)行估計的，其解是以估計值形式給出。因此稱這 種系統(tǒng)為線性最優(yōu)估計器或濾波器。卡爾曼過濾中信號和噪聲是狀態(tài)方程和 量測方程表示的，因此設(shè)計卡爾曼濾波器要求已知狀態(tài)方程和量測方程。標(biāo)準(zhǔn)卡爾曼濾波器是在最小均方誤差準(zhǔn)則下的最佳線性過濾器，就是 說，它使系統(tǒng)的狀態(tài)向量和狀態(tài)向量的預(yù)測值之間的均方誤差達(dá)到最小，它 用狀態(tài)方程和遞推方法進(jìn)行估計，它的解是以估計值形式給出的。由于它

9、能 夠?qū)ξ矬w的運動建立某種模型，因此在跟蹤中經(jīng)常被用到。當(dāng)觀測方程不是 線性時，上述標(biāo)準(zhǔn)卡爾曼濾波方程不再適用，但是如果狀態(tài)估計值離真實值 不是很遠(yuǎn)，可以將 觀測方程局 部 線 性化， 得 到擴(kuò) 展 卡爾 曼 濾波器(EKF )。由于 EKF 使 用 泰 勒 展 開 的 一 階 近 似 ，跟 蹤 一 段 時 間 之 后 ，經(jīng) 常 會 引 起 很 大 的 參 數(shù)估計的累計誤差 。為此，UnscentedKalmanFilter(UKF)不再近似 估計觀測方程，它仍然用高斯隨機(jī)變量表示狀態(tài)分布，不過是用特定選擇的樣本點 加以描述。與EKF相比，UKF 的 誤差 僅 僅 出現(xiàn) 在 三 階 以 上的矩

10、中， 而且計算 也 簡 單 ，而 EKF 僅 僅 精 確 到 一 階 矩 ?？?的 來 說 ，卡 爾 曼 濾 波 是 一 個 線 性 的 估計器，能夠有效地跟蹤物體的運動和形狀變化，但它基于兩個假設(shè)：一是 背景相對干凈；二是運動參數(shù)服從高斯分布。因而適用范圍有限，對于復(fù)雜 的多峰情況，還得求助于其它方法。1.4 課 題 解 決 方 案本課題采用卡爾曼濾波器技術(shù)對熱力系統(tǒng)進(jìn)行溫度估計，選取溫室室內(nèi) 溫度作為系統(tǒng)研究對象。具體流程如下：系統(tǒng)建模組織相似狀態(tài)向量狀態(tài)觀測器的設(shè)計形成卡爾曼濾波器的疊代初始值MATLAB 編程運行與調(diào)試2. 卡爾曼濾波器的數(shù)學(xué)模型卡 爾 曼 濾 波 方 法 是 由 R.

11、E.Kalman 建 立 的 ,他在 Wiener 平 穩(wěn) 隨 機(jī) 過 程 的 濾波理論基礎(chǔ)上建立了一種新的遞 推 式濾 波方法,可借助于前一時刻的濾波結(jié)果,遞推出現(xiàn)時刻的狀態(tài)估計量,因 而 大大 提高了下 一 時 刻 的 預(yù) 測 精 度 。 該方法不但需要的歷史資料很短（2 3 月 ）, 而且 大大減少了計算機(jī)的存儲量和計算量 。 利 用 前 一 時 刻 預(yù) 報 誤 差 反 饋 到 原 來 的 預(yù) 報 方 程 ,通 過 修 正 原 預(yù) 報 方 程 系 數(shù) , 來提高一下時刻的預(yù)報精度,這是 卡 爾曼 濾波與多元回歸預(yù)報方法的最大區(qū)別,也是它的最大優(yōu)點。2.1 一 維 時 變 隨 機(jī) 信 號

12、的 數(shù) 學(xué) 模 型對 每 一 確 定 的 取 樣 時 刻 k， x（ k）是 一 個 隨 機(jī) 變 量 。當(dāng) 取 樣 時 刻 的 時 標(biāo) k 變 化 時，我們就得到一個離散的隨機(jī)過程，即隨機(jī)序列 x（k）。假設(shè)待估隨機(jī)信號的數(shù)學(xué)模型是一個由白噪聲序列 w（k）驅(qū) 動 的 一 階自遞歸過程，其動態(tài)方程為：x（k） ax（k 1） （k 1）（2-1）式 中 ： 參 數(shù)a< 1（2-1） 式 中 的 w（k-1 ） 稱 為過 程 噪 聲 或 動 態(tài) 噪 聲 。 當(dāng) 時 標(biāo) k變 化 時 ， 它將 構(gòu)成 一個 白 噪 聲 序列 w（ k） ， 其 統(tǒng) 計 特 性 可 用 以 下 數(shù) 字 特征 來

13、 描 述 ：均 值E （k）0方 差E（k） 22 常 數(shù)2kj自 相 關(guān) 序 列 E （k） （j）0 kj2-1 ）式 所 決 定 的 信 號x（k ），當(dāng) 時 標(biāo) k 變 化 時 ，將 構(gòu) 成 一 個 平 穩(wěn)隨 機(jī) 序 x（k） ， 其 統(tǒng) 計 特 性 可 用 以 下 數(shù) 字 特 性 來 描 述 ：1. 均 值E x(k) 0卡爾曼濾波器畢業(yè)論文2. 方 差E x(k) 22x常數(shù)3. 自 相 關(guān) 序 列 當(dāng) 取 樣 時 刻 的 時 標(biāo) k 變 化 時 ， 取 樣 時 刻 時 標(biāo) 相 差 j 的x( k)的兩樣值見的自相關(guān)序列為Px(j) E x(k)x(k j)2.2 信 號 測 量

14、過 程 的 數(shù) 學(xué) 模 型信號測量過程的數(shù)學(xué)模型，可用如下的測量方程給出y(k) cx(k) v(k)(2-2)式 中 ： x(k)為 k 時刻 的 信 號 值 。 y(k) 為該 時刻 對 x(k) 進(jìn)行 測 量 所 得 到信 號 測 量 樣 值 。v(k) 為此 時 在 測 量 過 程 中 所引 入的 量 測 噪 聲， 可 將 其 視 為獨立的附加白噪聲。當(dāng) k 變化 時 ，x(k) 將組 成 一 個 隨 機(jī) 信 號 序 列 x(k) ， y(k)將組成一個測量樣值序 列y(k) ，v(k) 而將 組 成 一 個 附 加 白 噪 聲 序 列 v(k) 。 C為量測參數(shù)，它是 一個 由測量

15、系統(tǒng)和 測量方法所 確定的 不隨時 間變化 的常 數(shù)。因 為 量 測 噪 聲 序 列 是 一 個 白 噪 聲 序 列 ，故 其 統(tǒng) 計 特 性 可 用 如 下 的 數(shù) 字 特 征來描述：均 值E v(k) 0方 差E v(k) 2v2 常 數(shù)2自相關(guān)序列 E v(k)v(j) v0 kj又 因 量 測 噪 聲 序 列 v(k) 與 隨 機(jī) 信 號 序 列 x(k) 互 不 相 關(guān) ， 故E x(k)v( j) 0卡爾曼濾波器畢業(yè)論文所 以 ，我們 可 以 得 到 一 維 時 變 隨 機(jī) 信 號 及 其 測 量 過 程 的 數(shù) 學(xué) 模 型 ，見 圖2-1 。2-1 一 維 時 變 隨 機(jī) 信 號

16、 及 其 測 量 過 程 的 數(shù) 學(xué) 模 型2.3 多 維 隨 機(jī) 信 號 向 量 及 其 數(shù) 學(xué) 模 型假 設(shè) 我 們 想 同 時 對 q 個 獨 立 的 標(biāo) 量 隨 機(jī) 信 號x1(k) ， x2(k)，, ， xq(k)進(jìn) 行 最 優(yōu) 濾 波 或 預(yù) 測 ， 而 這 q個 獨 立 的 標(biāo) 量 隨 機(jī) 信 號 都 由 它 們 各 自 的 一 階 自 遞歸過程產(chǎn)生，即(2-3)x (k) a x (k 1) w (k 1)式 中 ：=1 ， 2， , ， q 。若上述個一階自遞歸過程中的過程噪聲是彼此獨立的白噪聲序列，則 可 以 定 義 一 個 由 這 q個 獨 立 的 標(biāo) 量 隨 機(jī) 信

17、號 組 成 的 q維 隨 機(jī) 信 號 向 量x1 (k)x(k)x2(k)(2-4)xq (k)和 一 個 由 q個 獨 立 的 白 噪 聲 序 列 組 成 的 q維 過 程 噪 聲 向 量w1(k)w2 (k) w(k)wq(k)并 將 ( 2-3 ) 式 寫 成 一 個 一 階 的 向 量 方 程x(k) Ax(k 1) w(k 1)(2-5)(2-6)式 中 ： 和 均 為 ( q× 1) 維 列 向 量 ， 而 A則 是 一 個 ( q× q) 階 矩 陣a10.00a2.0A.0. . aq我們稱之為系統(tǒng)矩陣。(2-7)在 引 入 q維 過 程 噪 聲 向 量 后

18、 ， 原 標(biāo) 量 過 程 噪 聲 的 方 差2w ,E w (k) 2(1,2,.,q)將 變 成 q 維 過 程 噪 聲 向 量 w(k) 的 協(xié) 方 差 矩 陣Q(k) E w(k)w(k)T(2-8)為 我 們 假 設(shè) 產(chǎn) 生 q個 獨 立 標(biāo) 量 隨 機(jī) 信 號 的 一 階 自 遞 歸 過 程 的 過 程 噪 聲是彼此獨立的白噪聲序列，故2w1,10.002.0Q(k)w2,2.200.wq, q(2-9)顯然，此協(xié)方差矩陣對角線上個元素即為組成該過程噪聲向量的個分量方差。2.4 多 維 測 量 數(shù) 據(jù) 向 量 及 其 數(shù) 學(xué) 模 型假設(shè) 為 了 對 一 個q維隨機(jī) 信號向量進(jìn)行 最

19、優(yōu) 濾 波或預(yù) 測 ， 在 k時 刻 對x(k) 的前 r 個分 量 ， 令r <q， 同時進(jìn)行了一 次 測 量 。得到 r 個 測 量 數(shù) 據(jù) 的 樣值 ， 用y1(k)， y2(k)，,，yr (k)表示，即y1(k)c1x1(k) v1(k)y2 (k) c2x2 (k) v2 (k)( 2-10 ).yr (k) crxr (k) vr (k)式中 ：c1 ， c2，, ，cr為測量系統(tǒng)的量測參 數(shù) ；v1， v2，, ，vr為測量過程中引入的附加量測噪聲。(2-10)式 可 寫成 一個 一階向量方程y(k) Cx(k) v(k)( 2-11 )式中 ：y(k)和v(k)為( r

20、×1)維列向量，x(k)為( q× 1)維 列向 量，而C則 是 一 個 ( r× q) 階 矩 陣c10.0.00c2.0.0C .( 2-12 )0.cr.0稱為量測矩陣回觀測矩陣。顯 然 ，一 階 向 量 方 程( 2-11 )式 就 是 r 維 測 量 數(shù) 據(jù) 向 量 y(k)的 數(shù) 學(xué) 模 型 ， 或 者 說 是 q 維 隨 機(jī) 信 號 向 量 x(k)的 測 量 過 程 的 數(shù) 學(xué) 模 型 。在 引 入 r 維 量 測 噪 聲 向 量 后 ，原 標(biāo) 量 量 測 噪 聲 的 方 差 此 時 也 將 變 成 量 測 噪聲向量的協(xié)方差矩陣v1,102R(k)

21、 E v(k)v(k)T0v2,2.2-13 )2vr ,r003 卡 爾 曼 濾 波 器3.1 標(biāo) 量 卡 爾 曼 濾 波 器 的 基 本 內(nèi) 容一維隨機(jī)信號的遞歸型估計器的一般表達(dá)式：x(k) a(k)x(k 1) b(k)y(k)(3-1)在 信 號 數(shù) 學(xué) 模 型 為 ( 2-1 )式 、測 量 過 程 的 數(shù) 學(xué) 模 型 為( 2-2 )的 條 件 下 ， 以 均 方 估 計 誤 差 最 小 為 準(zhǔn) 則 對 估 計 器 的 加 權(quán) 系 數(shù) a(k)和 b(k)進(jìn) 行 最 優(yōu) 化 ， 并 推導(dǎo)出標(biāo)量卡爾曼濾波器的最優(yōu)估計的遞推算法。( 3-2 ) 式 表 述 的 遞 歸 型 估 計 器

22、 在 k 時 刻 對 信 號 x(k) 的 估 計 誤 差 為e(k) x(k) x(k)(3-2)均方估計差為2P(k) E x(k) x(k)(3-3)若 將 ( 3-1 ) 式 代 入 ( 3-3 ) 式 ， 可 得2P(k) E x(k) a(k)x (k 1) b(k) y(k)(3-4)若 令 P(k)對 a(k)和 b(k)的 偏 導(dǎo) 數(shù) 為 零 ， 即P(k)a(k)2E x(k) a(k) x (k 1) b(k)y(k) x (k 1)(3-5)P(k)b(k)2E x(k) a(k)x (k 1) b(k)y(k) y(k)(3-6)則 由 ( 3-5 ) 式 和 ( 3

23、-6 ) 式 中 解 出 a(k)和 b(k)將 保 證 該 遞 歸 型 估 計 器 的 均方估計誤差為最小。根 據(jù) 統(tǒng) 計 估 計 理 論 中 的 正 交 原 理 ， 我 們 也 可 將 ( 3-5 ) 式 和 ( 3-6 ) 式分別寫成正交方程的形式，即E e(k)x (k 1)0(3-7)E e(k)y(k) 0(3-8)由 ( 3-5 ) 式 ， 我 們 可 得E a(k) x(k 1)x (k1)Ex(k) b(k)y(k)x (k 1)(3-9)再 由 ( 3-9 )式 出 發(fā)， 經(jīng) 過一 系列 的代 換 可 求 出(3-10)a(k) a 1 cb(k)此 式 為 經(jīng) 過 最 優(yōu)

24、 化 得 到 的 a(k)表 達(dá) 式 。 式 中 ：a(k)是最優(yōu) 遞歸 型 估 計 器的一 個時變增益 ， 它將隨時標(biāo)k的改變而變化 。 a是 信 號模型中反映一階自遞 歸 過 程 惰 性 大 小 的 參 數(shù) ， 只 要 信 號模 型 確 定 后 ， 它就 是 一個常 數(shù)。顯 然 ， a(k)和a是兩個意義 完 全不同的量。 我們還可以看 出 ， 由 于 式中還包含另一個未知的 時變增益b(k)，因此它實際 上只是一個a(k)與 b(k)的關(guān)系式。要想最終 確 定 a(k) ， 還 必 須 求 出 b(k) 。最 優(yōu) 遞歸型估 計 器 對信號 x(k)的 均 方 估計 誤差可寫 成P(k)E

25、e(k)x(k)a(k)E e(k)x(k1)b(k)E e(k)y(k)由 正 交公式( 3-7 ) 式和 ( 3-8 ) 式 可 知， 上式等號 右 側(cè) 的 后 兩 項 為零 ，(3-11)P(k) E e(k)x(k)量 測 方 程 ( 2-2 ) 式 ， 我 們 可 得x(k) y(k) v(k) c代 入 ( 3-11 )式 ，因為 信 號 x(k)與 量 測 噪 聲 v(k) 不相 關(guān) ， ( k 1 ) 時 刻信 號 估 計 值 x(k1) 與k 時 刻 的 量 測 噪 聲 也 不 相 關(guān) ， 故12P(k) b(k) v(3-12)c我們還可以把最優(yōu)遞歸型估計器對信號的均方估計

26、誤差寫成2P(k)E ax(k 1) w(k 1) a(k)x (k 1) b(k)cx(k) v(k) (3-13)再 利 用a(k)與 b(k)的 關(guān) 系 式 ( 3-10 ) 式a(k) a 1 cb(k)因 為 e( 1)、 w(k 1) 和 v(k)互 不 相 關(guān) ， 它 們 的 交 叉 乘 積 項 的 均 值 都 為 零 ，故p(k) a2 1 cb(k) 2 P(k 1) 1 cb(k) 22w b2(k) v2(3-14)將 ( 3-12 ) 式 代 入 ( 3-14 ) 式 ， 經(jīng) 整 理 后 求 解 得b(k)(3-15)ca2 P(k 1)2w22222v c2w c2a

27、2P(k 1)此 式 即 經(jīng) 過 最 優(yōu) 化 所 得 到 的 b(k)的 表 達(dá) 式 。3.2 最 優(yōu) 遞 歸 型 估 計 器 的 構(gòu) 成由 ( 3-1 ) 式x(k) a(k) x (k 1) b(k) y(k)所 表 述 的 遞 歸 型 估 計 器 ， 當(dāng) 其 時 變 增 益 a(k)和 b(k)經(jīng) 過 最 優(yōu) 化 ， 即 分 別有 ( 3-10 ) 式 和 ( 3-15 ) 式 給 出 時 ， 就 是 一 個 最 優(yōu) 遞 歸 型 估 計 器 ， 其 均 方 估 計誤差最小。利 用 ( 3-10 ) 式 ， 我 們 可 從 ( 3-1 ) 式 中 消 去 a(k) ， 得 到x(k) ax

28、(k 1) b(k) y(k) acx (k 1)(3-16)( 3-16 ) 式 ， 可 構(gòu) 成 一 個 最 優(yōu) 遞 歸 型 估 計 器 標(biāo) 量 卡 爾 曼 濾 波 器 ，其 框 圖 如 圖 3-1 。x(k)y(k)b(k)y(k) acx (k 1)a x(k 1)3-1 標(biāo) 量 卡 爾 曼 濾 波 器 框 圖對( 3-16) 式 物理意 義的 說明：在尚未獲得k 時刻 的新測量 樣 值y(k) 以前，我們 只 能從 (k1)時 刻 對信號所作出的估計x(k1) 出發(fā)，根據(jù) 由信號 數(shù)學(xué)模型x(k)ax(k1)w(k1) 所確定的規(guī)律來對k 時刻 的信號x(k) 進(jìn)行預(yù)估。由于信號數(shù)學(xué)模

29、 型 中的 動態(tài)噪聲的確 切數(shù)值w(k1)無從 得 知 ，故 對x(k)的預(yù)估值只能取作ax(k1) ?？梢姡?3-16)式等 號 右側(cè)的第一 項x(k1) 就是在未獲得任何新信 息 的情 況下，根據(jù)以 往的測量 數(shù) 據(jù)對k 時刻 的信 號x(k)所做的預(yù)估。在k時刻的新測量 樣 值y(k)尚 未得 到 之前，我們還 可對k 時刻的將要測得的新測量樣值y(k)進(jìn)行 預(yù) 估。 但是，此 時 我們只 能 從對k時刻的信號x(k)的預(yù)估值ax(k1)出 發(fā)，根 據(jù) 量測 方程來 對 k時刻將要測得的y(k)作 出 預(yù)估。由于量測噪聲v(k) 的確切 數(shù) 值無 從得知 ， 故對的預(yù)估值只能取作y(k)

30、 c x (k 1) ac x(k 1)我 們 測 得 k 時 刻 的 新 測 量 樣 值 y(k)后 ， 若 所 測 得 的y(k)值 與 其 預(yù) 估 值y(k) acx(k 1) 之 差 不 為 零 ， 就 說 明 k 時 刻 的 新 測 量 樣 值 y(k) 中 包 含 有 前(k 1)次測 量 中 所沒 有的 新信息。若y(k)與其預(yù)估值y(k) acy(k1)之 差為零 ， 則說明 k時 刻的 新測 量樣值中不包 含 任何新信息。 因 此 ， 我 們把k時刻 的 信 號 實 測 值 y(k)與 其 預(yù) 估 值 y(k) acy(k 1) 之 差 y(k) ac y(k 1) 稱 為

31、第 k 次測量 中的新信息 。顯然，當(dāng)我們 測得 k 時刻 的新測量樣值y(k)之 后 ，可 利用 第k 次測量 中的 新 信 息 y(k) acy(k 1)乘 上 一 個 比 例 系 數(shù) b(k)作 為 修 正 項 ， 對 未 測 得 y(k)前 對 信 號給出的預(yù)估 值a x(k1) 進(jìn)行修正，從而得 到 k 時刻 對信 號的 估計值x(k) ?？梢姡?3-16) 式 等號 右側(cè)的第二項b(k) y(k)acx(k1) 即為對 信號預(yù)估值的修正項。3.3 標(biāo) 量 卡 爾 曼 濾 波 器 的 遞 推 算 法卡 爾 曼 濾 波 的 基 本 算 法 是 預(yù) 估 加 修 正 ， 而 公 式 （ 3

32、-16 ） 式 、 （ 3-15 ） 式和 （ 3-12 ） 式 就 構(gòu) 成 了 標(biāo) 量 卡 爾 曼 濾 波 器 在 信 號 及 其 測 量 過 程 的 數(shù) 學(xué) 模 型 分別 為 x(k) ax(k 1) w(k 1) 和 y(k) cx(k) v(k)時 對 信 號 進(jìn) 行 最 優(yōu) 估 計 的 一套完整的遞推算法。3-6 ） 式 可 用 來 推 算 卡 爾 曼 濾 波 器 在 不 同 取 樣 時 刻 k 對 信 號 x（k） 的 估計 值 x(k)， 即 x(k) ax(k 1) b(k) y(k) acx (k 1)3-15 ）式 可 用 來 推 算 卡 爾 曼 濾 波 器 在 不 同 取

33、 樣 時 刻 k 的 時 變 濾 波 增 益b(k) ， 即b(k)c a2P(k 1)22222v c2 w c2a2P(k 1)3-12 ）式 可 用 來 推 算 卡 爾 曼 濾 波 器 在 不 同 取 樣 時 刻 k 的 均 方 估 計 誤 差P(k)， 即12P(k) vb(k)c為 了 便 于 將 標(biāo) 量 卡 爾 曼 濾 波 器 的 遞 推 算 法 直 接 推 廣 到 向 量 隨 機(jī) 信 號（ 即 多維隨機(jī)信號）的卡爾曼濾波中去，給出如下的一套完整的遞推算法：濾波估計方程x(k) ax(k 1) b(k) y(k) acx (k 1)(3-17)濾波增益方程b(k)c2P1c(Pk1

34、()k) v2(3-18)22式 中 ： P1(k) a2P(k 1) w(3-19)均方濾波誤差方程P(k) P1(k) cb(k)P1(k)(3-20)3.4 標(biāo) 量 卡 爾 曼 預(yù) 測 器假設(shè)待測標(biāo)量隨機(jī)信號的數(shù)學(xué)模型為一階自遞歸過程，即x(k) ax(k 1) w(k 1)信號測量過程的數(shù)學(xué)模型為y(k) cx(k) v(k)線性遞歸型預(yù)測估計器可用如下的遞推方程來表述：x(k 1|k) (k)x(k|k 1)(k)y(k)(3-21)為 使 ( 3-21 ) 式 表 述 的 線 性 預(yù) 估 器 成 為 一 個 最 優(yōu) 線 性 預(yù) 測 器 ， 就 必 須 依據(jù)最小均方誤差 準(zhǔn)則 對 (

35、 3-21) 式 中的 時 變 預(yù) 測 增 益 (k)和 (k)進(jìn) 行 最 優(yōu) 化 。用 上 節(jié) 類 似 的 方 法 ，可 以 求 得 經(jīng) 最 優(yōu) 化 后 的 (k) 和 (k)的 表 達(dá) 式 以 及 相 應(yīng) 的均方預(yù)測誤差P(k 1|k)的 表 達(dá) 式 ，即(k)a c (k)(3-22)(k)2acP(k|k 1) 2(3-23)cPk|k 1 vP(k 1|k) a v2 (k)2w(3-24)c其 中： 根據(jù) ( 3-22) 式， 可由 P(k | k 1)推 算 出 ；再 根 據(jù)( 3-23 )式 又 可 由 (k)推 算 出 P(k 1 | k)。只 要 選 定 了 初 值 ， 這

36、 一 遞 推 過 程 就 可 以 不 斷 的 進(jìn) 行 下 去 。將 ( 3-22 ) 式 代 入 ( 3-21 ) 式 ， 可 得卡爾曼濾波器畢業(yè)論文(3-25)x(k 1 |k) ax(k |k 1)(k) y(k) cx(k |k 1)根 據(jù) （ 3-25 ） 式 ， 可 以 構(gòu) 成 一 個 標(biāo) 量 卡 爾 曼 預(yù) 測 器 ， 其 框 圖 如 圖 3-2 所示。3-2 標(biāo) 量 卡 爾 曼 預(yù) 測 器 框 圖圖 3-2 所 示 的 標(biāo) 量 卡 爾 曼 預(yù) 測 器 的 一 套 完 整 的 遞 推 算 法 有 （3-22 ） 式 、（ 3-23 ） 式 、 （ 3-24 ） 式 組 成 。 但

37、為 了 便 于 將 這 組 公 式 直 接 推 廣 到 向 量 隨 機(jī) 信 號（即多維隨機(jī)信號）的卡爾曼遞推預(yù)測中去，當(dāng)變量隨機(jī)信號及其測量過 程 的 數(shù) 學(xué) 模 型 分 別 為 x(k) ax(k 1) w(k 1)和 y(k) cx(k)v(k)時 ， 將 ( 3-24 ) 式 作 下 改 寫 ， 即 利 用 （ 3-23 ） 式 消 去 （ 3-24 ） 式 中 的經(jīng)過改寫后得到卡爾曼預(yù)測器對此信號進(jìn)行遞推預(yù)測的一套完整的遞推 算法，公式如下：預(yù)測估計方程x(k 1 | k) ax(k | k 1)(k) y(k) cx(k | k 1)(3-26)預(yù)測增益方程(k) acP(k|k 1

38、) 2c2P(k | k 1)(3-27)均方預(yù)測誤差方程卡爾曼濾波器畢業(yè)論文P(k 1 |k) a2P(k|k 1) ac (k)P(k|k 1)2w(3-28)3.5 向量 卡 爾 曼 濾 波 和預(yù) 測與 標(biāo)量情況類似，可用向量x(k) 表示 k時刻對隨機(jī) 信號向量x(k) 的最 優(yōu)線 性 濾 波 估 計 值 ， 用 向 量 x(k 1 | k) 表 示 在 k 時刻對 (k 1) 時 刻 的 隨 機(jī) 信 號 向 量 x(k1) 的最優(yōu)線性預(yù)測估計值。對 向量卡爾曼濾波來說，標(biāo)量 情 況下 的濾波誤差e(k)此時將變 成 一個 濾波誤差向量e1 (k)e(k) x(k) x(k)e2(k)

39、( 3-29 ).eq(k)標(biāo)量情況下的均方濾波誤差，此時將變成一個濾波誤差的協(xié)方差矩陣P(k)e(k)eT (k)P1,1(k)P1,2(k).P1,q(k)P2,1 (k)P2,2(k).P2,q(k)( 3-30 ).Pq,1 (k)Pq,2(k).Pq,q(k)同樣，對向量卡爾曼預(yù)測來說，其預(yù)測誤差向量為e(k 1|k) x(k 1) x(k 1|k)( 3-31 )其預(yù)測協(xié)方差矩陣為P(k 1 |k) Ee(k 1 |k)eT(k 1| k)( 3-32 )所謂向量卡爾曼濾波器或預(yù)測器，實際上就是一種能對向量隨機(jī)信號進(jìn)行 最 優(yōu) 線 性 濾 波 或 預(yù) 測 的 遞 歸 型 濾 波 器

40、 。 “ 最 優(yōu) ”的 含 義 ，是 指 能 使 每 個 信 號 分量的均方估計誤差同時為最小?？紤]到向量卡爾曼濾波器或預(yù)測與標(biāo)量卡爾曼濾波器或預(yù)測無論條件上 （即信號 及其測量過程的數(shù) 學(xué)模型）還是在要 求上（即“最優(yōu)”的含義）都 完全相似，因而，可以直接把標(biāo)量卡爾曼濾波或預(yù)測的遞推公式推廣到向量 情況。在直接引用標(biāo)量卡爾曼濾波或預(yù)測的遞推公式時，除了應(yīng)把標(biāo)量形式的 信號、信號估計值和測量樣值表述成向量形式，把 系 統(tǒng) 參 數(shù) a和量 測 參 數(shù) c表述成系統(tǒng)矩陣（或稱系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣）A和 量 測 矩 陣 C，把 均 方 估 計誤 差 和 有 關(guān) 噪 聲 和 方 差 表 述 成 相 應(yīng)

41、的 協(xié) 方 差 矩 陣 外 ， 還 應(yīng) 根 據(jù) 表 3-1 將 遞 推 公式中的標(biāo)量運算變換成相應(yīng)的矩陣運算。表 3-1 從 標(biāo) 量 運 算 到 矩 陣 運 算 的 轉(zhuǎn) 換 表標(biāo)量運算矩陣運算abABabABa2bABAT111(A B) 1ab注 意 ： 在 使 用 表 3-1 時 必 須 使 變 換 后 的 沒 一 步 矩 陣 運 算 都 能 進(jìn) 行 下 去 ，并使等號兩側(cè)最終得到的矩陣在階數(shù)上保持一致。我 們 可 以 從 標(biāo) 量 卡 爾 曼 濾 波 的 遞 推 算 法 （ 3-17 ） （3-20 ） 式 直 接 得 到 向量卡爾曼濾波的遞推算法：濾波估計方程：x(k) Ax(k 1)

42、K(k) y(k) CAx(k 1)(3-33 )濾波增益方程：K(k) P1(k)CT CP1 (k)CT R(k) 13-34 )式 中 ： P1(k) AP(k 1)AT Q(k 1)3-35 )濾波協(xié)方差方程：3-36 )P(k) P1(k) K(k)CP1(k)和標(biāo)量卡爾曼濾波器一樣，向量卡爾曼濾波器也是以預(yù)測加修正作為其遞推 濾波的基本算法的?？柭鼮V波器的這一特性，使得很容易用計算機(jī)來實現(xiàn) 對信號的實時濾波，為此，可采用軟件芳案來實現(xiàn)卡爾曼濾波。在 向 量 卡 爾 曼 濾 波 的 一 整 套 遞 推 算 法 中 ， ( 3-33 )式 可 構(gòu) 成 向 量 卡 爾 曼 濾波 的 主

43、 程 序 算 法 ， 圖 3-3 給 出 了 主 程 序 算 法 的 框 圖 。3-3 向 量 卡 爾 曼 濾 波 的 主 程 序 算 法 框 圖從 圖 3-3 中 可 以 看 出 ， 向 量 卡 爾 曼 濾 波 的 主 程 序 算 法 主 要 分 三 步 來 進(jìn) 行 。第 一 步 ： 在 已 知 ( k-1 ) 時 刻 對 信 號 向 量 x(k 1) 的 估 計 值 x(k 1)的 條 件下 ，用 系 統(tǒng) 矩 陣 A乘 以 x(k 1) ，得 到 在( k-1 )時 刻 對 k 時 刻 信 號 向 量 x(k) 的預(yù) 測 值 x(k | k 1) Ax(k 1)。第二步：用量 測 矩 陣

44、C 乘以 x(k | k 1) ，得到 在( k-1 )時刻對k 時刻的測量數(shù)據(jù)向 量 y(k)的 預(yù) 測 值 y(k | k 1) CAx(k 1)；再 用 y(k) 的 實 測 值 減 去 預(yù) 測 值 ， 得 到 殘 差 ( 新 信 息 ) e (k) y(k) y(k |k 1) y(k) CAx(k 1) ；最 后 用 濾 波 增 益 矩陣 K(k)乘 以e'(k)，得到 修 正 量 K(k)e'(k)。'第三步：把對信號的預(yù)測值x(k | k1) 加上修 正 量K(k)e(k)，得 到 信號 的濾波 估 計 值x(k)。值 得 注 意 的是：在上述運算 過 程

45、中 ，所用 的 濾波 增 益 矩 陣K(k)并不 是在主程序中計算出來的，而是從向量卡爾曼濾波的子程序算法中計算出來的。 上述運算過程中所得到的x(k) 值 應(yīng)存儲起 來，以供下一次遞推時使用。只要確定了信號估計值的初值x(0) ，例如設(shè)x(0)0，則隨著時間的推移，可在測得 y(1) 后 算 出 x(1) ，在 測 得 y(2) 后 算 出 x(2)，依 次 類 推 。 在 從 ( k-1 ) 時 刻 到 k 時刻這段時間內(nèi)，只需把x(k 1) 存儲起來 。隨著遞推的不斷進(jìn)行，再對它不斷更新。 當(dāng) 然， 如 果 系 統(tǒng) 矩 陣 和 量 測 矩 陣 是時變 的， 就 需把A(k,k1)和C(k

46、)也存儲起來。向量卡 爾曼濾波 的 子 程序 算 法 是 由 (3-33)(3-36 )式 構(gòu)成的，其算法框圖由圖3-4所示。從圖3-4中可以 看 出 ，向 量 卡 爾 曼 濾波 的子程 序 算法也 分三 步 來進(jìn) 行。第一步： 在以知 P(k 1) 、 Q(k 1)和 A(k,k 1) 的條件下，利用 ( 3-35) 式求出P1(k) 。第 二 步 ： 將P1(k)、 C(k)和 R(k) 代 入 ( 3-34 ) 式 ， 求 出 K (k)， 供 主 程 序 調(diào)用。第 三 步 ：將P1(k)、K (k)和 C(k) 代 入( 3-36 )式 ，求 出 P(k) 并 存 儲 起 來 ，供下一

47、次遞推用。P(k)K(k)圖3-4向 量 卡爾曼濾波的子程序算法框圖由 于 濾 波 增 益 矩 陣 K (k)只 與 矩 陣 A(k, k 1) 、 C(k) 、 Q(k 1) 和 R(k) 以 及 濾 波 協(xié)方 差 矩 陣的初 值P(0)無關(guān)，而與信號的估計值和測量值 無 關(guān) ， 所 以 對K (k)的計 算 可 在對信 號進(jìn) 行 估計之前就由子程序來獨立完成。需 要 指 出 的 是 ： 即 使 A、 C、 Q、 R 都 是 與 時 間 無 關(guān) 的 常 數(shù) 矩 陣 ， 濾 波 協(xié)方差矩陣P(k) 和 濾 波 增益矩陣K(k)仍將 與 時 間 有 關(guān) 。只 要 選 定了濾波 協(xié)方 差矩陣的初值

48、 P(0)， 子 程序中的遞 推 運算 即 可 反 復(fù) 進(jìn) 行 下 去 ， 從而對P(k)和 主程序所需調(diào) 用 的 K(k)不斷進(jìn)行更 新 。此 外 ， 由 于 在用 (3-34) 式 求K (k)的過 程中 需 對 ( r× r)階 矩 陣CP1 (k)CT R(k) 求逆 ，所 以量 測 數(shù)據(jù) 向 量y(k)的 維 數(shù) r 一 般 不宜 取 得 過 大 。最后把向量卡爾曼濾波的主程序算法和子程序合在一起，看看子程序中 影 響 濾 波 增 益 K(k)的 諸 因 數(shù) 與 K(k)在 主 程 序 中 所 起 的 作 用 。在卡爾曼主程序中：首先根據(jù)信號模型對信號進(jìn)行外推預(yù)測，得到信號

49、 的外推 預(yù) 測值x(k |k 1)；然后，根據(jù)信號的 量 測方 程 和信號的實測值，求出殘差e'(k)，經(jīng)調(diào) 用 K(k)加 權(quán)后，作為對 信號 預(yù) 測值 的 修正量；最后，將預(yù)測值和修 正 值相加 ， 給 出 信號的濾波估計值x(k) 。K(k)在主程序中是修正量的加 權(quán) 因 子 。 K(k) 大 ，意 味 著 實 際 測 量 值 在 濾 波 估 計 中 所 起 的 作 用 大 ； K(k)小 ， 意味著外推預(yù)測值在濾波估計中所起的作用大。K(k)的大小是在卡爾曼 濾 波的子 程 序中計算出來的。從 ( 3-34 ) 式 可 知 ， K(k)的 大 小 與 R(k) 有 關(guān) 。 若

50、 R(k) 變 小 ， 則 K(k)變 大。這 一 結(jié)果的物理意義很 明 顯。R(k)變小意味著測量過程中 引 入的量 測噪聲變小 ， 因此信號實測值y(k)的準(zhǔn)確 度 較高。此時，當(dāng)然把K(k)取大一 些，使信號的濾波估計值依賴實測值的比重加大。從( 3-34 )式 還 可 知 ， K (k)的 大 小 還 與P1 (k)有 關(guān) ，即 與 預(yù) 測 誤 差 的 協(xié) 方差P(k | k 1)有關(guān) 。 根 據(jù) ( 3-35) 式 ， 若 P(k 1) 變 小 ， 或 Q(k1)變小，或兩者 都 變 小 ，則P1(k)變 小 。此 時 ， K(k)也 變 小 。這 一 結(jié) 果 的 物 理 意 義 也

51、 不 難 理解。 P(k 1)變小 意 味 著 原 有 濾 波 估 計 值 x(k 1) 較為 準(zhǔn) 確 ， Q(k1) 變小意味著動態(tài)過程噪聲在信號模型中起的作用較小，這都會使信號的外推預(yù)測值 x(k |k 1)的 可 靠 性 提 高 。這 一 點 ，也 可 以 從 預(yù) 測 協(xié) 方 差 P1(k) 變 小 看 出 。此 時 ， 當(dāng) 然 應(yīng) 把 K(k)值 取 得 小 一 些 ， 使 信 號 的 濾 波 估 計 值 依 賴 外 推 預(yù) 測 值 的 比 重 加 大。3.6 卡 爾 曼 的 線 性 化卡爾曼濾波在測量領(lǐng)域的應(yīng)用一般都存在線性化的問題。卡爾曼濾波的 線 性 化 目 前 主 要 有 按 系 統(tǒng) 標(biāo) 稱 狀 態(tài) 線 性 化 和 按 系 統(tǒng) 狀 態(tài) 最 優(yōu) 估 計 線 性 化 (也 稱 廣 義 卡 爾 曼 濾 波 )兩 種 方 式 。 在 靜 態(tài) 定 位 領(lǐng) 域 ， 由 于 觀 測 目 標(biāo) 狀 態(tài) 不 變 ， 因 而標(biāo)稱狀態(tài)的誤差不會隨濾波過程增大，這兩種線性化方法都易于使用。但 在 動 態(tài) 定 位 領(lǐng) 域 ，由 于 狀 態(tài)