????????本文主要介紹卡爾曼濾波的推導過程及建模步驟，是網(wǎng)站的學習筆記。本文主要是通過例子來引出卡爾曼濾波的建模思想及算法步驟。
參考網(wǎng)站：（這個網(wǎng)站講得真的很詳細很清楚，層層遞進，邏輯清晰）Kalman Filter Tutorialhttps://www.kalmanfilter.net/
目錄
一、引言
? ? ? ? 1）引例1——運動狀態(tài)建模
二、g-h(-k)濾波器
? ? ? ? 1）引例2——黃金稱重
? ? ? ? 2）引例3——一維模式下的等速飛機追蹤
? ? ? ? 3）引例4——勻加速飛機軌跡預(yù)測
三、一維卡爾曼濾波器
? ? ? ? 1）引入觀測噪聲（Measurement Error）
? ? ? ? 2）引入過程噪聲（Process Noise）
? ? ? ? 3）引例5
四、多維卡爾曼濾波器
? ? ? ? 1）多維情況下的狀態(tài)外推方程
? ? ? ? 2）多維情況下的協(xié)方差外推方程
? ? ? ? 3）多維情況下的狀態(tài)更新方程
? ? ? ? 4）協(xié)方差更新方程
? ? ? ? 5）卡爾曼增益方程
? ? ? ? 6）引例6
? ? ? ? 7）引例7
一、引言
? ? ? ? 1）引例1——運動狀態(tài)建模
????????基于一個勻加速直線運動對物體的位置進行預(yù)測，設(shè)當前為X0（當前狀態(tài)），X為預(yù)測位置。若是物體做理想的勻加速直線運動，則可使用下式進行預(yù)測：
?將其擴展到三維空間中有：
?????????上述的方程反應(yīng)了輸入數(shù)據(jù)（位移或坐標）對輸出數(shù)據(jù)之間的關(guān)系，這樣描述上一個系統(tǒng)狀態(tài)和下一個系統(tǒng)狀態(tài)之間關(guān)系的方程稱為動態(tài)模型或狀態(tài)空間模型。
????????但是，實際情況不會是抽象分析中的理想環(huán)境，還包括各種各樣的誤差影響，若想在實際情況中獲得高精度的預(yù)測，就必須要將噪聲考慮其中。此處主要將噪聲分為兩類：
????????測量噪聲：是指在測量過程中的噪聲，比如在上述引例中，需要對速度和時間進行測量，而對這些量的觀測存在一個隨機誤差，測量中包含的誤差稱為測量噪聲。（在建模過程中隨機誤差即測量噪聲服從正態(tài)分布）
????????過程噪聲：由于外部環(huán)境的原因，使得物體并非做預(yù)想的嚴格的勻速直線運動（即由于外界環(huán)境影響，物體運動并非完全與運動方程對齊），這種動態(tài)模型的誤差（不確定性）稱為過程噪聲
二、g-h(-k)濾波器
? ? ? ? 1）引例2——黃金稱重
????????對黃金進行稱重。假設(shè)現(xiàn)在有一塊黃金，每日對其進行稱重，稱重過程（測量過程）只包含隨機誤差，不包含系統(tǒng)誤差，那么每日的黃金重量可能如下：
?????????連續(xù)觀測n天，我們選取前n天的平均值（每天的觀測值記為Zi）作為第n天黃金重量的估計值。
?????????第一個下標表示對第n天的估計值，第二個下標表示基于多少天的數(shù)據(jù)，若基于前n-1天的觀測值對第n天的數(shù)據(jù)進行估計則記為Xn,n-1：
?????????由于我們認定黃金的重量是恒定不變的，所以狀態(tài)空間模型(基于當前狀態(tài)值對下一狀態(tài)的預(yù)測)為：
?由于預(yù)測時需要再重新累加求和，由此我們可以對式子進行改寫：
?對上式證明如下：
?再顧及狀態(tài)空間模型有：
?上述式子可以描述為：
????????該方程被稱為狀態(tài)更新方程，其中的（Measurement-Predicted Value of the current state）被稱為測量殘差。其也表示了基于該時刻預(yù)測值和觀測值獲得該時刻的估計值的方法。其中1/n稱為卡爾曼增益，在本例子中會隨著迭代而改變。所以可寫為如下方程：?
?????????——數(shù)值示例（搬運于網(wǎng)站，看不清可直接跳轉(zhuǎn)網(wǎng)頁）The alpha - beta - gamma filter (kalmanfilter.net)https://www.kalmanfilter.net/alphabeta.html
······
? ? ? ? 2）引例3——一維模式下的等速飛機追蹤
????????在一維對等速飛機進行追蹤。有一雷達對飛機進行跟蹤，如下圖:
Xn表示時間n內(nèi)飛機的飛行距離，飛機的速度可以用微分方式獲得：?
?????????則其動態(tài)模型為：
?
上述方程組被稱為過渡方程或預(yù)測方程（也稱狀態(tài)外推方程），也是卡爾曼五個方程之一，這個方程組完成由本狀態(tài)向下一狀態(tài)的預(yù)測。
（回顧：到現(xiàn)在已經(jīng)學了兩個卡爾曼方程。第一個方程為狀態(tài)更新方程，他是基于上一個狀態(tài)對當前狀態(tài)的預(yù)測值和當前狀態(tài)的觀測值實現(xiàn)對當前狀態(tài)值的估計；第二個方程是預(yù)測方程，他是基于當前狀態(tài)和觀測值實現(xiàn)對下一狀態(tài)的預(yù)測。）
? ? ? ? 示例：
?????????上述說明，在速度不變的情況下，本次飛機雷達測距為30000m，在時間間隔5s后飛機雷達測距應(yīng)該為30200m（預(yù)測值）。此番工作就是基于當前狀態(tài)對下一狀態(tài)的預(yù)測。
????????但是，5s后飛機雷達測距為30110m，和預(yù)測值存在90m的誤差，那為什么會產(chǎn)生誤差呢。有兩個可能：要么是雷達測量不準；或者是飛機速度發(fā)生了改變。那究竟是哪一種可能呢？
?????????寫出速度的狀態(tài)更新方程：
?????????我對其解釋為：
?關(guān)于對卡爾曼增益的確定：
?????????雷達誤差是一種隨機誤差服從正態(tài)分布，若誤差在雷達測量的標準差外，則可能是由速度變化引起的，反之。卡爾曼增益取決于其雷達的測量精度，若精度越高，則說明殘差就越重要，卡爾曼增益就應(yīng)該越大；精度越低，說明測量的權(quán)重不高，沒有那么重要，所以殘差就沒那么重要，卡爾曼增益就應(yīng)該越小。
????????如果飛機速度確實從40m / s變?yōu)?2m / s，我們在10個循環(huán)后看到這一點（運行上面的等式10次β= 0.1）。如果間隙是由測量誤差引起的，則連續(xù)測量將在預(yù)測位置的前面或后面。因此，平均而言，目標速度不會改變。
????????然后寫出距離的狀態(tài)更新公式。
?????????此處的參數(shù)卡爾曼增益表示雷達的測量精度，是一個常數(shù)。當其為1時，說明測量極其精確，觀測值即為估計值；當其為0是，說明測量毫無意義。
????????因此，對于飛機的位置和速度，我們都得到一個狀態(tài)更新公式：
?????????該方程組構(gòu)成了雷達跟蹤器的狀態(tài)更新方程。它們也被稱為α?β（也稱g-h）跟蹤更新公式或α?β跟蹤過濾方程.
? ? ? ? 算例：The alpha - beta - gamma filter (kalmanfilter.net)
? ? ? ? python實現(xiàn)算例：kalman_g_h_filter.py （文末獲?。?/strong>
? ? ? ? 3）引例4——勻加速飛機軌跡預(yù)測
????????若用上式（引例3的模型）進行“先勻速再加速”的軌跡預(yù)測，效果如下：
?
????????能很明顯的發(fā)現(xiàn)實際的測量值和估計值之間存在這很大的差距，因為使用勻速運動的模型來預(yù)測勻加速運動，所以會導致預(yù)測會產(chǎn)生誤差，這差距被稱為滯后誤差或歸為系統(tǒng)誤差。
? ? ? ? 現(xiàn)在我們來討論勻加速運動模型的建模。
????????基于上述所言，我們現(xiàn)在討論對勻加速直線運動的飛機的跟蹤建模。因運動中引入了加速度，所以狀態(tài)外推方程（預(yù)測方程）即為：
?????????對應(yīng)即可得到其狀態(tài)更新公式，回顧上述引例，在黃金稱重例子中，我們只有一個狀態(tài)即黃金的重量，所以狀態(tài)更新公式和外推（預(yù)測）公式相應(yīng)只有一個；在飛機的勻速運動引例中，飛機的狀態(tài)包括位移x和速度v，所以其狀態(tài)更新公式和外推（預(yù)測）公式相應(yīng)的都有兩個；依次類推，在勻加速運動中又引入了狀態(tài)——加速度a，所以在本模型中，外推（預(yù)測）公式和狀態(tài)更新公式都有三個，如下：
?????????方程組中的1式不用解釋，2式在引例3中引入速度更新公式時做出了解釋。對3式來說，理解如下：
演算示例：
????????選取一個場景：一架飛機以50m / s的恒定速度移動15秒，然后以8m / s的恒定加速度加速再延長 35 秒。參數(shù)如下：
?
?
?。。。
結(jié)果圖如下：
?
python實現(xiàn)算例：kalman_g_h_k_filter.py? (文末獲?。?/strong>
對g-h-(k)濾波器的總結(jié)。
??????? 建模步驟總結(jié)：
??????? ——step1首先基于啟動數(shù)據(jù)，利用狀態(tài)外推方程（預(yù)測方程）獲得下一狀態(tài)預(yù)測值。
??????? ——step2基于預(yù)測值和觀測值，使用狀態(tài)更新方程獲得當前狀態(tài)的估計值。
??????? ——step3完成當前狀態(tài)的估計之后，基于當前狀態(tài)估計值使用預(yù)測方程獲得對下一狀態(tài)的預(yù)測值。
??????? ——step4重復步驟2和步驟3。
??????? 在建模過程中，g-h-k濾波器的參數(shù)選擇至關(guān)重要。
g_h_k濾波器優(yōu)化論文：
?（文末獲取）
三、一維卡爾曼濾波器
? ? ? ? 1）引入觀測噪聲（Measurement Error）
????????回顧引例2，在對黃金的稱重過程中，其結(jié)果如下：
我們可以明顯的發(fā)現(xiàn)測量值和真實值之間存在很大的差距，我們將這種差距稱為測量誤差，也就是測量噪聲。我們將測量誤差用測量不確定度來表示，記為r。在本例中，我們可以通過稱的商家獲得（商家一般都會有自己儀器的測量誤差）或推導獲得。假設(shè)方差為m*m，那么標準差為m，根據(jù)隨機誤差服從正態(tài)分布可得下圖：
?????????由正態(tài)分布理論可知，若真值屬于觀測值左右一個標準差范圍內(nèi)（l-m，l+m），則認為觀測是合理的，上圖10次觀測中，有8次符合條件。所以，測量的不確定都r即為方差m*m。
????????現(xiàn)在推導卡爾曼增益方程，在原來的引例中，參數(shù)取的是1/n，在卡爾曼濾波過程中，卡爾曼增益是迭代計算的，如下式即為卡爾曼增益方程：
????????其中，r為測量不確定度，p為外推估計的不確定度（用于衡量估計值與真實值的誤差，可理解為外推估計的權(quán)重）。r為方差大于零，所以卡爾曼增益范圍為0至1。
??????? 綜上狀態(tài)更新方程可變?yōu)椋?/p>
?????????外推估計的不確定（p）的更新
????????上式被稱為協(xié)方差更新方程，每次濾波器迭代時，1-K是小于1的，所以估計的不確定性在減小。當測量不確定度（r，等于方差）很大時，K值很小，1-K就更接近1，所以濾波器收斂速度慢，反之。?
????????現(xiàn)在，我們來討論估計不確度的外推。首先我們梳理一下，當上一狀態(tài)完成外推估計（預(yù)測）后，獲得外推估計不確定度Pn,n-1，進而計算出卡爾曼增益K，再根據(jù)卡爾曼增益獲得當前估計的不確定度Pn,n。而我們當前狀態(tài)仍然需要對下一狀態(tài)進行外推估計（預(yù)測），所以我們需要計算出Pn+1,n。
????????以引例3勻速飛行飛機為例，狀態(tài)包括位移和速度，其狀態(tài)外推方程為:
?基于外推方程可以得到估計的不確定度計算公式：
????????（關(guān)于協(xié)方差外推公式的理解：如下圖）?
????????估計不確定度的外推方程稱為協(xié)方差外推方程。至此五個卡爾曼方程介紹結(jié)束。
在此，我們將所學的五個卡爾曼方程匯總起來：
首先是狀態(tài)更新方程：實現(xiàn)對預(yù)測值和觀測值的加權(quán)擬合（濾波）。
第二個是狀態(tài)外推（預(yù)測）方程：基于當前狀態(tài)估計值對下一個狀態(tài)進行估計。
（以引例3為例）
第三是在引入觀測噪聲后，計算卡爾曼增益的卡爾曼增益方程。
第四是對估計不確定度的更新方程，即協(xié)方差更新方程。
第五是將估計不確定度外推到下一狀態(tài)的協(xié)方差外推方程，（以引例3為例）
? ? ? ? 例子可見官網(wǎng)。
? ? ? ? 2）引入過程噪聲（Process Noise）
????????綜上所述，我們討論了含有觀測噪聲的情況下的卡爾曼濾波建模方法，并做了算例分析。但正如最先所講，不僅包括觀測噪聲，還包括過程噪聲，動態(tài)模型（狀態(tài)外推方程）的不確定性稱為過程噪聲，只有都考慮到才是完整的卡爾曼濾波模型。
??????? 從理論上講，過程噪聲造成的影響是使得估計的不確定度（p）變大，所以在協(xié)方差外推方程中，需要加入過程噪聲的影響，因為觀測噪聲和過程噪聲顯然不相關(guān)，所以直接加上。
????????如下所列為恒定狀態(tài)下：
?????????對于引例3而言，對于位移外推的不確定度如下：
顧及過程噪聲：?
上述中的q為過程噪聲的方差。
?????????對協(xié)方差外推方程進行更新后，即為完整的卡爾曼濾波模型。
? ? ? ? 3）引例5
迭代結(jié)果：
????????從迭代結(jié)果可知，估計數(shù)值和觀測值和真值都存在很大的差距，這種差距被稱為滯后誤差。上述我們在利用勻速的模型來分析加速飛行的飛機時結(jié)果也出現(xiàn)過滯后誤差。所以，分析其原因，是狀態(tài)方程和實際變化不符所造成的影響，也即模型誤差。
????????在本例中，我們可以很容易的將狀態(tài)外推方程改正，只需在后面加上0.1乘以時間即可，但是在具體的實際問題中，我們可能并不知道正確的空間模型是什么，不能得到嚴格正確的狀態(tài)外推方程，只知道當前模型存在滯后誤差。此時，就可以通過上文介紹的過程噪聲對模型進行改正，將過程噪聲的值變大，反映著狀態(tài)方程的不確定性程度越高。
????????上述引例中，將過程噪聲方差從0.0001改為0.15。在進行卡爾曼濾波迭代計算，結(jié)果如下：
四、多維卡爾曼濾波器
? ? ? ? 1）多維情況下的狀態(tài)外推方程
????????首先介紹多維情況下的狀態(tài)外推方程：
? ? ? ? 2）多維情況下的協(xié)方差外推方程
????????多維情況下的協(xié)方差外推方程：
? ? ? ? 3）多維情況下的狀態(tài)更新方程
????????狀態(tài)更新方程：
? ? ? ? 4）協(xié)方差更新方程
????????協(xié)方差更新方程：
?給出推導：
簡化版更新方程：
? ? ? ? 5）卡爾曼增益方程
卡爾曼增益方程：
卡爾曼結(jié)構(gòu)示意圖：
? ? ? ? 6）引例6
? ? ? ? ? ? 汽車定位例子。Examples (kalmanfilter.net)https://www.kalmanfilter.net/multiExamples.html
? ? ? ? python實現(xiàn)算例：kalman_vehicle_location.py
? ? ? ? 7）引例7
? ? ? ? ? ? ? ?火箭高度示例。
Examples (kalmanfilter.net)https://www.kalmanfilter.net/multiExamples.html
? ? ? ? python實現(xiàn)算例：rocket_altitude.py
?淺薄之見，敬請指正，共勉！
資源獲?。üP記、代碼、論文）：文章來源：http://www.zghlxwxcb.cn/news/detail-631825.html
鏈接：https://pan.baidu.com/s/1cgmW9DIIZGC98ArCrUTQ7A?pwd=kalm?
提取碼：kalm?
--來自百度網(wǎng)盤超級會員V4的分享文章來源地址http://www.zghlxwxcb.cn/news/detail-631825.html
到了這里，關(guān)于算法介紹及實現(xiàn)——卡爾曼濾波的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
