動態(tài)線性系統(tǒng)的卡爾曼濾波解析課件

第九講：動態(tài)線性系統(tǒng)的卡爾曼濾波

第九講：工程實踐中的估計問題有兩類：1、系統(tǒng)的參數(shù)部分或全部未知-----有待確定；2、實施最優(yōu)控制時，需要了解系統(tǒng)的狀態(tài)，而系統(tǒng)中部分或全部狀態(tài)變量不能直接測得。這樣就：包含了兩類估計問題：參數(shù)估計

狀態(tài)估計。不同估計準則會導(dǎo)致不同的估計方法；同樣不同的觀測信息，也會導(dǎo)致不同的估計方法。估計問題都是由三個部分構(gòu)成：1、函數(shù)模型（估計約束條件）2、隨機模型（估計驗前信息）3、估計準則。工程實踐中的估計問題有兩類：不同估計準則會導(dǎo)致不同的估計方法前面討論的各種最小二乘分解法，都是針對靜態(tài)函數(shù)模型的。即通過觀測向量L所估計的參數(shù)向量是不依賴于時間t的。但是，在許多實際的問題中，如：應(yīng)用計算技術(shù)進行適時控制的需要；大壩的變形監(jiān)測、GPS導(dǎo)航等等，以上被估計參數(shù)都是隨著時間的變化而不斷變化的，必需在觀測過程中不斷的對系統(tǒng)的狀態(tài)進行估計，并且隨著新觀測值的獲得不斷修正這種估計。

卡爾曼濾波理論便是適應(yīng)適時控制的需要，對系統(tǒng)的狀態(tài)參數(shù)進行線性估計的一種遞推算法。前面討論的各種最小二乘分解法，都是針對靜態(tài)函數(shù)模型的。即通過Kalman濾波是卡爾曼（kalman）于1960年提出的一種濾波方法。特點：是對狀態(tài)空間進行估計。狀態(tài)空間估計一般是動態(tài)估計。估計過程利用：系統(tǒng)狀態(tài)方程、觀測方程、系統(tǒng)過程噪聲以及觀測噪聲的統(tǒng)計特性構(gòu)成濾波算法。Kalman采用遞推算法。Kalman濾波是卡爾曼（kalman）于1960年提出的一何謂kalman濾波？如：要研究的對象是一個房間的溫度，根據(jù)經(jīng)驗，房間溫度恒定的。1）根據(jù)上一時刻的溫度值來推算下一時刻的溫度

（稱：預(yù)測值或估計值，有偏差）；2）溫度計測量（稱：觀測值，有偏差）。根據(jù)經(jīng)驗預(yù)測估計值和當前狀態(tài)的觀測值，并結(jié)合其各自噪聲來估算出房間的實際溫度值。何謂kalman濾波？基本概念：1、狀態(tài)向量隨時間不斷變化的隨機變量稱為動態(tài)系統(tǒng)在ｔ時刻的“狀態(tài)”向量。如：GPS導(dǎo)航、變形監(jiān)測中是以點的位置、運動速度為狀態(tài)向量的。狀態(tài)向量：點的位置、運動速度、加速度?；靖拍睿?、狀態(tài)向量2、狀態(tài)方程（描述隨機過程的狀態(tài)變化）看一簡單例子：勻加速直線運動K點K+1點K點：位置xkK+1點：位置xk+1

速度vk

速度vk+1

加速度ａk

加速度ａk+1?t狀態(tài)向量2、狀態(tài)方程（描述隨機過程的狀態(tài)變化）K點K+1點K點：位置則K點與K+1點3個量之間關(guān)系為：

勻加速，故

xk+1=xk+vk?t+1/2ａ?t2vk+1=vk+ａ?t

ａk=ａk+1=ａ狀態(tài)用矩陣表示（狀態(tài)方程）

則K點與K+1點3個量之間關(guān)系為：其中：為系統(tǒng)矩陣，表示位置變化的轉(zhuǎn)移；

為狀態(tài)（過程）噪聲?？紤]系統(tǒng)噪聲，狀態(tài)方程為：其中：為系統(tǒng)矩陣，表示位置變化的轉(zhuǎn)移；3、觀測方程（描述隨機過程與觀測量的關(guān)系）

在K+1處對點的位置（狀態(tài)的一項）進行了觀測，得觀測向量Ｌk+1，則觀測方程：其中：△為觀測噪聲。3、觀測方程（描述隨機過程與觀測量的關(guān)系）其中：△為觀測噪聲線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。觀測方程：觀測向量與狀態(tài)向量之間存在的某種函數(shù)關(guān)系，也稱為輸出方程。白噪聲：協(xié)方差為零的噪聲。有色噪聲：不同時刻的動態(tài)噪聲或觀測噪聲是相關(guān)的。線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。下面將針對以下問題進行學習：1、隨機線性系統(tǒng)的數(shù)學模型（離散線性系統(tǒng)）；2、隨機線性離散系統(tǒng)的kalman濾波方程；3、kalman濾波的應(yīng)用。下面將針對以下問題進行學習：

一、

離散線性系統(tǒng)的數(shù)學模型離散時間系統(tǒng)：如僅在確定的瞬間來研究系統(tǒng)的性能，則把這樣的系統(tǒng)叫做離散時間系統(tǒng)。包括兩種：

1）系統(tǒng)本身就是一個離散系統(tǒng)；

2）本身是連續(xù)系統(tǒng)，為研究方便，僅在離散時間內(nèi)研究其性能。一、離散線性系統(tǒng)的數(shù)學模型離散時間系統(tǒng)：1、函數(shù)模型：狀態(tài)方程和觀測方程稱為動態(tài)系統(tǒng)的函數(shù)模型。離散系統(tǒng)的狀態(tài)方程和觀測方程為：是狀態(tài)外加的控制（輸入）向量，是常量（非隨機）即：非隨機控制項。一般忽略不計。稱系數(shù)矩陣。1、函數(shù)模型：是狀態(tài)外加的控制（輸入）向量，是常量（非隨機）對于連續(xù)線性系統(tǒng)的狀態(tài)方程和觀測方程：設(shè)位置、速度為狀態(tài)向量，加速度為系統(tǒng)噪聲；按照運動狀態(tài)知它們之間關(guān)系（是一個微分方程）：觀測方程：對于連續(xù)線性系統(tǒng)的狀態(tài)方程和觀測方程：2、離散線性系統(tǒng)的隨機模型：Dirac函數(shù)2、離散線性系統(tǒng)的隨機模型：Dirac函數(shù)二、

離散線性系統(tǒng)的卡爾曼濾波1）狀態(tài)方程、觀測方程離散線性系統(tǒng)的估計

即利用觀測向量L1，L2，…LK，根據(jù)其數(shù)學模型求定第ｔj時刻狀態(tài)向量Xｊ的最佳估值。二、離散線性系統(tǒng)的卡爾曼濾波1）狀態(tài)方程、觀測方程估計量分為三種情況:ｊ=ｋ，稱為濾波；(用依據(jù)過去直至現(xiàn)在的觀測值來估計現(xiàn)在的狀態(tài)，多用于隨機系統(tǒng)的實時控制。）ｊ＞ｋ，稱為預(yù)測或外推；（依據(jù)過去直至現(xiàn)在的觀測值來預(yù)測未來的狀態(tài)，用于對系統(tǒng)未來狀態(tài)的預(yù)測和實時控制）ｊ＜ｋ，平滑或內(nèi)插。（依據(jù)過去直至現(xiàn)在的觀測值來估計過去歷史狀態(tài)，用于通過分析實驗或?qū)嶒灁?shù)據(jù)對系統(tǒng)進行評估）預(yù)測是濾波的基礎(chǔ)，濾波又是平滑的基礎(chǔ)。估計量分為三種情況:2）隨機模型：上述模型的卡爾曼慮波稱為“完全不相關(guān)的白噪聲作用下的卡爾曼慮波”。注意：隨機模型參數(shù)僅僅給出初始狀態(tài)的統(tǒng)計特性。即其他參數(shù)均為非隨機的。2）隨機模型：狀態(tài)估計的方法：1）最小方差估計或極大驗后估計原理；2）廣義最小二乘原理。狀態(tài)估計的方法：以下將按照廣義最小二乘原理并采用逐次平差方法導(dǎo)出卡爾曼濾波方程。設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：以下將按照廣義最小二乘原理并采用逐次平差方法導(dǎo)出卡爾曼濾波方為方便起見（類似觀測方程），將狀態(tài)方程改寫為：并有：相應(yīng)觀測方程：的方差陣：以上實質(zhì)是：將動態(tài)噪聲項看成是相應(yīng)于某種觀測向量的觀測噪聲。為方便起見（類似觀測方程），將狀態(tài)方程改寫為：并有：相應(yīng)觀測設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：因只有初始狀態(tài)是隨機參數(shù)，根據(jù)廣義最小二乘原理，應(yīng)將隨機參數(shù)X0的先驗期望看成是虛擬觀測值。則可以寫出誤差方程：因只有初始狀態(tài)是隨機參數(shù)，根據(jù)廣義最小二乘原理，應(yīng)將隨機參數(shù)這樣，就將上述動態(tài)系統(tǒng)的估計問題變換為一個最小二乘問題。這時，式中均看作是非隨機參數(shù)；而看作是方差陣為的相應(yīng)“觀測值”，它們之間的協(xié)方差陣均為零。這樣，就將上述動態(tài)系統(tǒng)的估計問題變換為一個最小二乘問題。進行“逐次平差”：(1)取第一、第二式進行最小二乘間接平差法方程形如：其中:進行“逐次平差”：其中:整理法方程，可求解利用協(xié)方差傳播律可得解的誤差方差陣、協(xié)方差陣整理法方程，可求解(2)以上解作為虛擬觀測值，與L1

的誤差方程一起作第二次平差:誤差方程組、解法方程并整理，得：(2)以上解作為虛擬觀測值，與L1的誤差方程一起作第二次平(3)再以上解作為虛擬觀測值，與LS（1）一起作第三次平差：誤差方程解為：(3)再以上解作為虛擬觀測值，與LS（1）一起作第三次平差：（4）依此類推，就可得到遞推計算公式（5個），即“卡爾曼濾波方程”。（可分3步進行）一步預(yù)測值：濾波值：增益矩陣：（4）依此類推，就可得到遞推計算公式（5個），即“卡爾曼濾波

時間更新（預(yù)測值）（1）計算先驗狀態(tài)估計值（2）計算先驗誤差估計值測量更新（修正估計值）（1）計算修正矩陣（2）更新觀測值（3）更新誤差協(xié)方差時間更新（預(yù)測值）（1）計算先驗狀態(tài)估計值測量更新（修動態(tài)線性系統(tǒng)的卡爾曼濾波解析課件關(guān)于濾波公式:（1）項稱為“預(yù)報殘差”（新息）；（2）JK濾波增益矩陣；公式的物理意義是:濾波值等于預(yù)報值加一修正項,該修正項由預(yù)報殘差乘增益矩陣構(gòu)成。關(guān)于濾波公式:卡爾曼慮波的實質(zhì)利用卡爾曼濾波器進行濾波時,需要知道系統(tǒng)的狀態(tài)方程和量測方程。計算包括：

1）從一狀態(tài)推（K）求另一狀態(tài)（K+1）的預(yù)測值；

2）從此狀態(tài)本身有的觀測LK+1推求該狀態(tài)估值；即：歷史信息與觀測信息的綜合?？柭鼞]波的實質(zhì)三、卡爾曼濾波的應(yīng)用以下通過幾個實例來進一步理解卡爾曼濾波過程。三、卡爾曼濾波的應(yīng)用以下通過幾個實例來進一步理解卡爾曼濾波過[例1]設(shè)狀態(tài)方程和觀測方程為：隨機模型是：又已知兩次觀測數(shù)據(jù)L1=4，L2=2，求[例1]設(shè)狀態(tài)方程和觀測方程為：解：（1）計算一步預(yù)測值（2）求增益距陣J1，（3）計算（4）以（1）-（3）的公式計算（5）計算解：[例2]將一物體以初速度51米/秒從地面垂直上拋，在時刻tK觀測物體離地面的距離SK，得觀測值LK如下：設(shè)各觀測值的觀測是白噪聲，其方差D?i=1；初始狀態(tài)（初始距離和速度）為其先驗方差為設(shè)不考慮動態(tài)噪聲，求在各時刻tK物體離地面的距離和瞬時速度的估值和它們的方差。tK（秒）0.01.02.03.04.05.06.07.08.09.0LK（米）0.045.380.1105.8121.7123.9109.585.552.3[例2]將一物體以初速度51米/秒從地面垂直上拋，在時刻t（1）根據(jù)運動學，建立k-1時刻與k時刻物體離地面距離、瞬時速度的關(guān)系，即動態(tài)方程?；?qū)憺椋海ǚ请S機項）（時間間隔1秒）（1）根據(jù)運動學，建立k-1時刻與k時刻物體離地面距離、瞬時（2）觀測方程為或?qū)憺椋?）觀測方程為或?qū)憺?）計算預(yù)報值2）計算預(yù)報值的方差3）計算增益距陣J1）計算預(yù)報值4）計算估值5）計算估值方差6）按以上1）-5）重復(fù)計算求得各時刻估值及方差4）計算估值α—β模型（CV)α—β—γ模型(CA)將加速度的變化率視為動態(tài)噪聲加速度視為動態(tài)噪聲[例3]在動態(tài)水準監(jiān)測中常用的動態(tài)方程：α—β模型（CV)α—β—γ模型(CA)將加速度的變化率視為思考：

在GPS變形監(jiān)測中，將變形體視為一個動態(tài)系統(tǒng)，將一組觀測值作為系統(tǒng)輸出，則卡爾曼濾波就可以用來描述這個變形體的運動情況。試寫出GPS網(wǎng)的狀態(tài)方程、觀測方程。思考：1）設(shè)GPS監(jiān)測網(wǎng)有n個監(jiān)測點組成，以GPS點三維位置和三維速率為狀態(tài)向量；2）設(shè)i點在時刻t的：位置向量為------------------

瞬時速率為------------------

將瞬時加速度看成是隨機干擾—3）i點的狀態(tài)向量為：1）設(shè)GPS監(jiān)測網(wǎng)有n個監(jiān)測點組成，以GPS點三維位置和三維4）全網(wǎng)的狀態(tài)向量為：5)離散卡爾曼濾波的狀態(tài)方程純量形式為：4）全網(wǎng)的狀態(tài)向量為：卡爾曼濾波的優(yōu)點：1)濾波方程是一組遞推計算公式,計算過程是一個不斷預(yù)測、修正的過程；2)不需保留用過的觀測值序列,并且當?shù)玫叫碌挠^測數(shù)據(jù)時,可隨時計算新的濾波值,便于實時處理觀測成果,把參數(shù)估計和預(yù)報有機地結(jié)合起來.3)卡爾曼濾波特別適合變形監(jiān)測、組合導(dǎo)航等動態(tài)數(shù)據(jù)處理。4）卡爾曼濾波值是最小方差無偏估值?？柭鼮V波的優(yōu)點：卡爾曼濾波應(yīng)用于動態(tài)觀測系統(tǒng)中，可實時獲得系統(tǒng)當前狀態(tài)；且濾波精度較高；卡爾曼濾波除了可掌握系統(tǒng)的當前狀態(tài)外，還可預(yù)測系統(tǒng)未來。（可以模擬動態(tài)目標系統(tǒng)的變化規(guī)律等）（預(yù)測和平滑）卡爾曼濾波應(yīng)用于動態(tài)觀測系統(tǒng)中，可實時獲得系統(tǒng)當前狀態(tài)；且濾我們已介紹的近代平差方法：1）從函數(shù)模型的類型：靜態(tài)、動態(tài)；2）從觀測方程系數(shù)陣：滿秩、列秩虧；3）從估計參數(shù)：非隨機變量、隨機變量，或同時出現(xiàn)；4）從觀測誤差：偶然誤差、系統(tǒng)誤差、粗差；5）從估計模型：函數(shù)模型估計、隨機模型估計。我們已介紹的近代平差方法：另外還有：6)觀測值中含有粗差時，用“穩(wěn)健估計”（抗差估計）來處理平差問題；7)附加系統(tǒng)參數(shù)方法處理系統(tǒng)誤差時，因參數(shù)過度化引入，導(dǎo)致法方程系數(shù)陣病態(tài)而提出的“有偏估計”。

另外還有：第九講：動態(tài)線性系統(tǒng)的卡爾曼濾波

第九講：工程實踐中的估計問題有兩類：1、系統(tǒng)的參數(shù)部分或全部未知-----有待確定；2、實施最優(yōu)控制時，需要了解系統(tǒng)的狀態(tài)，而系統(tǒng)中部分或全部狀態(tài)變量不能直接測得。這樣就：包含了兩類估計問題：參數(shù)估計

狀態(tài)估計。不同估計準則會導(dǎo)致不同的估計方法；同樣不同的觀測信息，也會導(dǎo)致不同的估計方法。估計問題都是由三個部分構(gòu)成：1、函數(shù)模型（估計約束條件）2、隨機模型（估計驗前信息）3、估計準則。工程實踐中的估計問題有兩類：不同估計準則會導(dǎo)致不同的估計方法前面討論的各種最小二乘分解法，都是針對靜態(tài)函數(shù)模型的。即通過觀測向量L所估計的參數(shù)向量是不依賴于時間t的。但是，在許多實際的問題中，如：應(yīng)用計算技術(shù)進行適時控制的需要；大壩的變形監(jiān)測、GPS導(dǎo)航等等，以上被估計參數(shù)都是隨著時間的變化而不斷變化的，必需在觀測過程中不斷的對系統(tǒng)的狀態(tài)進行估計，并且隨著新觀測值的獲得不斷修正這種估計。

卡爾曼濾波理論便是適應(yīng)適時控制的需要，對系統(tǒng)的狀態(tài)參數(shù)進行線性估計的一種遞推算法。前面討論的各種最小二乘分解法，都是針對靜態(tài)函數(shù)模型的。即通過Kalman濾波是卡爾曼（kalman）于1960年提出的一種濾波方法。特點：是對狀態(tài)空間進行估計。狀態(tài)空間估計一般是動態(tài)估計。估計過程利用：系統(tǒng)狀態(tài)方程、觀測方程、系統(tǒng)過程噪聲以及觀測噪聲的統(tǒng)計特性構(gòu)成濾波算法。Kalman采用遞推算法。Kalman濾波是卡爾曼（kalman）于1960年提出的一何謂kalman濾波？如：要研究的對象是一個房間的溫度，根據(jù)經(jīng)驗，房間溫度恒定的。1）根據(jù)上一時刻的溫度值來推算下一時刻的溫度

（稱：預(yù)測值或估計值，有偏差）；2）溫度計測量（稱：觀測值，有偏差）。根據(jù)經(jīng)驗預(yù)測估計值和當前狀態(tài)的觀測值，并結(jié)合其各自噪聲來估算出房間的實際溫度值。何謂kalman濾波？基本概念：1、狀態(tài)向量隨時間不斷變化的隨機變量稱為動態(tài)系統(tǒng)在ｔ時刻的“狀態(tài)”向量。如：GPS導(dǎo)航、變形監(jiān)測中是以點的位置、運動速度為狀態(tài)向量的。狀態(tài)向量：點的位置、運動速度、加速度?；靖拍睿?、狀態(tài)向量2、狀態(tài)方程（描述隨機過程的狀態(tài)變化）看一簡單例子：勻加速直線運動K點K+1點K點：位置xkK+1點：位置xk+1

速度vk

速度vk+1

加速度ａk

加速度ａk+1?t狀態(tài)向量2、狀態(tài)方程（描述隨機過程的狀態(tài)變化）K點K+1點K點：位置則K點與K+1點3個量之間關(guān)系為：

勻加速，故

xk+1=xk+vk?t+1/2ａ?t2vk+1=vk+ａ?t

ａk=ａk+1=ａ狀態(tài)用矩陣表示（狀態(tài)方程）

則K點與K+1點3個量之間關(guān)系為：其中：為系統(tǒng)矩陣，表示位置變化的轉(zhuǎn)移；

為狀態(tài)（過程）噪聲?？紤]系統(tǒng)噪聲，狀態(tài)方程為：其中：為系統(tǒng)矩陣，表示位置變化的轉(zhuǎn)移；3、觀測方程（描述隨機過程與觀測量的關(guān)系）

在K+1處對點的位置（狀態(tài)的一項）進行了觀測，得觀測向量Ｌk+1，則觀測方程：其中：△為觀測噪聲。3、觀測方程（描述隨機過程與觀測量的關(guān)系）其中：△為觀測噪聲線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。觀測方程：觀測向量與狀態(tài)向量之間存在的某種函數(shù)關(guān)系，也稱為輸出方程。白噪聲：協(xié)方差為零的噪聲。有色噪聲：不同時刻的動態(tài)噪聲或觀測噪聲是相關(guān)的。線性系統(tǒng)：狀態(tài)方程為線性的，稱線性系統(tǒng)。下面將針對以下問題進行學習：1、隨機線性系統(tǒng)的數(shù)學模型（離散線性系統(tǒng)）；2、隨機線性離散系統(tǒng)的kalman濾波方程；3、kalman濾波的應(yīng)用。下面將針對以下問題進行學習：

一、

離散線性系統(tǒng)的數(shù)學模型離散時間系統(tǒng)：如僅在確定的瞬間來研究系統(tǒng)的性能，則把這樣的系統(tǒng)叫做離散時間系統(tǒng)。包括兩種：

1）系統(tǒng)本身就是一個離散系統(tǒng)；

2）本身是連續(xù)系統(tǒng)，為研究方便，僅在離散時間內(nèi)研究其性能。一、離散線性系統(tǒng)的數(shù)學模型離散時間系統(tǒng)：1、函數(shù)模型：狀態(tài)方程和觀測方程稱為動態(tài)系統(tǒng)的函數(shù)模型。離散系統(tǒng)的狀態(tài)方程和觀測方程為：是狀態(tài)外加的控制（輸入）向量，是常量（非隨機）即：非隨機控制項。一般忽略不計。稱系數(shù)矩陣。1、函數(shù)模型：是狀態(tài)外加的控制（輸入）向量，是常量（非隨機）對于連續(xù)線性系統(tǒng)的狀態(tài)方程和觀測方程：設(shè)位置、速度為狀態(tài)向量，加速度為系統(tǒng)噪聲；按照運動狀態(tài)知它們之間關(guān)系（是一個微分方程）：觀測方程：對于連續(xù)線性系統(tǒng)的狀態(tài)方程和觀測方程：2、離散線性系統(tǒng)的隨機模型：Dirac函數(shù)2、離散線性系統(tǒng)的隨機模型：Dirac函數(shù)二、

離散線性系統(tǒng)的卡爾曼濾波1）狀態(tài)方程、觀測方程離散線性系統(tǒng)的估計

即利用觀測向量L1，L2，…LK，根據(jù)其數(shù)學模型求定第ｔj時刻狀態(tài)向量Xｊ的最佳估值。二、離散線性系統(tǒng)的卡爾曼濾波1）狀態(tài)方程、觀測方程估計量分為三種情況:ｊ=ｋ，稱為濾波；(用依據(jù)過去直至現(xiàn)在的觀測值來估計現(xiàn)在的狀態(tài)，多用于隨機系統(tǒng)的實時控制。）ｊ＞ｋ，稱為預(yù)測或外推；（依據(jù)過去直至現(xiàn)在的觀測值來預(yù)測未來的狀態(tài)，用于對系統(tǒng)未來狀態(tài)的預(yù)測和實時控制）ｊ＜ｋ，平滑或內(nèi)插。（依據(jù)過去直至現(xiàn)在的觀測值來估計過去歷史狀態(tài)，用于通過分析實驗或?qū)嶒灁?shù)據(jù)對系統(tǒng)進行評估）預(yù)測是濾波的基礎(chǔ)，濾波又是平滑的基礎(chǔ)。估計量分為三種情況:2）隨機模型：上述模型的卡爾曼慮波稱為“完全不相關(guān)的白噪聲作用下的卡爾曼慮波”。注意：隨機模型參數(shù)僅僅給出初始狀態(tài)的統(tǒng)計特性。即其他參數(shù)均為非隨機的。2）隨機模型：狀態(tài)估計的方法：1）最小方差估計或極大驗后估計原理；2）廣義最小二乘原理。狀態(tài)估計的方法：以下將按照廣義最小二乘原理并采用逐次平差方法導(dǎo)出卡爾曼濾波方程。設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：以下將按照廣義最小二乘原理并采用逐次平差方法導(dǎo)出卡爾曼濾波方為方便起見（類似觀測方程），將狀態(tài)方程改寫為：并有：相應(yīng)觀測方程：的方差陣：以上實質(zhì)是：將動態(tài)噪聲項看成是相應(yīng)于某種觀測向量的觀測噪聲。為方便起見（類似觀測方程），將狀態(tài)方程改寫為：并有：相應(yīng)觀測設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：設(shè)有K個觀測向量，則狀態(tài)方程和觀測方程為：因只有初始狀態(tài)是隨機參數(shù)，根據(jù)廣義最小二乘原理，應(yīng)將隨機參數(shù)X0的先驗期望看成是虛擬觀測值。則可以寫出誤差方程：因只有初始狀態(tài)是隨機參數(shù)，根據(jù)廣義最小二乘原理，應(yīng)將隨機參數(shù)這樣，就將上述動態(tài)系統(tǒng)的估計問題變換為一個最小二乘問題。這時，式中均看作是非隨機參數(shù)；而看作是方差陣為的相應(yīng)“觀測值”，它們之間的協(xié)方差陣均為零。這樣，就將上述動態(tài)系統(tǒng)的估計問題變換為一個最小二乘問題。進行“逐次平差”：(1)取第一、第二式進行最小二乘間接平差法方程形如：其中:進行“逐次平差”：其中:整理法方程，可求解利用協(xié)方差傳播律可得解的誤差方差陣、協(xié)方差陣整理法方程，可求解(2)以上解作為虛擬觀測值，與L1

的誤差方程一起作第二次平差:誤差方程組、解法方程并整理，得：(2)以上解作為虛擬觀測值，與L1的誤差方程一起作第二次平(3)再以上解作為虛擬觀測值，與LS（1）一起作第三次平差：誤差方程解為：(3)再以上解作為虛擬觀測值，與LS（1）一起作第三次平差：（4）依此類推，就可得到遞推計算公式（5個），即“卡爾曼濾波方程”。（可分3步進行）一步預(yù)測值：濾波值：增益矩陣：（4）依此類推，就可得到遞推計算公式（5個），即“卡爾曼濾波

時間更新（預(yù)測值）（1）計算先驗狀態(tài)估計值（2）計算先驗誤差估計值測量更新（修正估計值）（1）計算修正矩陣（2）更新觀測值（3）更新誤差協(xié)方差時間更新（預(yù)測值）（1）計算先驗狀態(tài)估計值測量更新（修動態(tài)線性系統(tǒng)的卡爾曼濾波解析課件關(guān)于濾波公式:（1）項稱為“預(yù)報殘差”（新息）；（2）JK濾波增益矩陣；公式的物理意義是:濾波值等于預(yù)報值加一修正項,該修正項由預(yù)報殘差乘增益矩陣構(gòu)成。關(guān)于濾波公式:卡爾曼慮波的實質(zhì)利用卡爾曼濾波器進行濾波時,需要知道系統(tǒng)的狀態(tài)方程和量測方程。計算包括：

1）從一狀態(tài)推（K）求另一狀態(tài)（K+1）的預(yù)測值；

2）從此狀態(tài)本身有的觀測LK+1推求該狀態(tài)估值；即：歷史信息與觀測信息的綜合。卡爾曼慮波的實質(zhì)三、卡爾曼濾波的應(yīng)用以下通過幾個實例來進一步理解卡爾曼濾波過程。三、卡爾曼濾波的應(yīng)用以下通過幾個實例來進一步理解卡爾曼濾波過[例1]設(shè)狀態(tài)方程和觀測方程為：隨機模型是：又已知兩次觀測數(shù)據(jù)L1=4，L2=2，求[例1]設(shè)狀態(tài)方程和觀測方程為：解：（1）計算一步預(yù)測值（2）求增益距陣J1，（3）計算（4）以（1）-（3）的公式計算（5）計算解：[例2]將一物體以初速度51米/秒從地面垂直上拋，在時刻tK觀測物體離地面的距離SK，得觀測值LK如下：設(shè)各觀測值的觀測是白噪聲，其方差D?i=1；初始狀態(tài)（初始距離和速度）為其先驗方差為設(shè)不考慮動態(tài)噪聲，求在各時刻tK物體離地面的距離和瞬時速度的估值和它們的方差。tK（秒）0.01.02.03.04.05.06.07.08.09.0LK（米）0.045.380.1105.8121.7123.9109.585.552.3[例2]將一物體以初速度51米/秒從地面垂直上拋，在時刻t（1）根據(jù)運動學，建立k-1時刻與k時刻物體離地面距離、瞬時速度的關(guān)系，即動態(tài)方