2. 程式人生 > >023卡爾曼濾波方程的推導

023卡爾曼濾波方程的推導

阿新 • • 發佈：2018-12-12

  博主參考嚴恭敏老師的講義進行學習卡爾曼濾波，在講義的基礎上加以詳細推導與說明。

  首先給出隨機系統狀態空間模型（注意該模型沒有給出控制部分）： (1){Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vk \tag{1} \begin{cases} X_k = \varPhi_{k/k-1} X_{k-1} + \varGamma_{k/k-1} W_{k-1}\\ Z_k = H_k X_k + V_k \end{cases}

注意：k/k-1表示從時刻k-1到k時刻的一步轉移。

1、k-1時刻

目標：求解狀態估計誤差的均方誤差陣

  真值：Xk−1X_{k-1}   狀態估計：X^k−1\hat X_{k-1} ，我們求得的狀態估計基本不會等於真值，所以我們的狀態估計就是最後求得的、可以用的狀態；   狀態估計誤差（為了便於區分，在此用e）： (2)ek−1=Xk−1−X^k−1 \tag{2} e_{k-1} = X_{k-1} - \hat X_{k-1} \\   狀態估計誤差的均方誤差陣：Pk−1P_{k-1} (3)Pk−1=E[ek−1ek−1T]=E[(Xk−1−X^k−1)（Xk−1−X

^k−1）T] \tag{3} P_{k-1}=E[e_{k-1} e_{k-1}^T] = E[(X_{k-1} - \hat X_{k-1}) （ X_{k-1} - \hat X_{k-1}）^T]

注意：Pk−1P_{k-1}是狀態估計誤差（真值-狀態估計值）的均方誤差陣。

2、由k-1時刻預測k時刻狀態

目標：求解預測誤差的均方誤差陣

  對k時刻的狀態作預測： (4)X^k/k−1=Φk/k−1X^k−1 \tag{4} \hat X_{k/k-1} = \varPhi_{k/k-1} \hat X_{k-1}

  零均值白噪聲不會影響預測。另外注意X^k/k−1\hat X_{k/k-1}還不是最終的系統狀態，說白了現在預測的這個狀態還是不能用的，必須經過後面的修正得到X^k\hat X_{k}才能用，這也是他們下標的區別所在了。   預測誤差： (5)ek/k−1=Xk−X^k/k−1 \tag{5} e_{k/k-1} = X_k - \hat X_{k/k-1} \\   預測誤差的均方誤差陣：Pk/k−1P_{k/k-1} (6)Pk/k−1=E[ek/k−1ek/k−1T]=E[(Xk−X^k/k−1)(Xk−1−X^k/k−1)T] \tag{6} \begin{aligned} P_{k/k-1} &= E[e_{k/k-1} e_{k/k-1}^T] \\ &= E[(X_k - \hat X_{k/k-1}) ( X_{k-1} - \hat X_{k/k-1})^T] \end{aligned}   由公式(1)和公式(4)，並結合公式(2)可知： Xk−X^k/k−1=Φk/k−1Xk−1+Γk/k−1Wk−1−Φk/k−1X^k−1=Φk/k−1ek−1+Γk/k−1Wk−1 \begin{aligned} X_k - \hat X_{k/k-1} &= \varPhi_{k/k-1} X_{k-1} + \varGamma_{k/k-1} W_{k-1} - \varPhi_{k/k-1} \hat X_{k-1}\\ &= \varPhi_{k/k-1} e_{k-1} + \varGamma_{k/k-1} W_{k-1} \end{aligned}

注意：是ek−1e_{k-1}而不是ek/k−1e_{k/k-1}

  所以： (Xk−X^k/k−1)(Xk−1−X^k/k−1)T=(Φk/k−1ek−1+Γk/k−1Wk−1)(Φk/k−1ek−1+Γk/k−1Wk−1)T=(Φk/k−1ek−1+Γk/k−1Wk−1)(ek−1TΦk/k−1T+Wk−1TΓk/k−1T)=Φk/k−1ek−1ek−1TΦk/k−1T+Φk/k−1ek−1Wk−1TΓk/k−1T+Γk/k−1Wk−1ek−1TΦk/k−1T+Γk/k−1Wk−1Wk−1TΓk/k−1T \begin{aligned} (X_k - \hat X_{k/k-1}) ( X_{k-1} -\hat X_{k/k-1})^T &= (\varPhi_{k/k-1} e_{k-1} + \varGamma_{k/k-1} W_{k-1})(\varPhi_{k/k-1} e_{k-1} + \varGamma_{k/k-1} W_{k-1})^T\\ &= (\varPhi_{k/k-1} e_{k-1} + \varGamma_{k/k-1} W_{k-1}) (e_{k-1}^T \varPhi_{k/k-1}^T + W_{k-1}^T \varGamma_{k/k-1}^T)\\ &= \varPhi_{k/k-1} e_{k-1} e_{k-1}^T \varPhi_{k/k-1}^T + \varPhi_{k/k-1} e_{k-1} W_{k-1}^T \varGamma_{k/k-1}^T \\ &+ \varGamma_{k/k-1} W_{k-1}e_{k-1}^T \varPhi_{k/k-1}^T + \varGamma_{k/k-1} W_{k-1}W_{k-1}^T \varGamma_{k/k-1}^T \end{aligned}

相關推薦

023卡爾曼濾波方程的推導

  博主參考嚴恭敏老師的講義進行學習卡爾曼濾波，在講義的基礎上加以詳細推導與說明。   首先給出隨機系統狀態空間模型（注意該模型沒有給出控制部分）： (1){Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vk \tag{1} \begin{ca

卡爾曼濾波 -- 從推導到應用(一) 到 (二)

/************************************************************************************************************************************

卡爾曼濾波 -- 從推導到應用(一)

前言          卡爾曼濾波器是在估計線性系統狀態的過程中，以最小均方誤差為目的而推匯出的幾個遞推數學等式,也可以從貝葉斯推斷的角度來推導。          本文將分為兩部分：第一部分，結合例子

卡爾曼濾波的理解以及推導過程

針對的系統為： 狀態方程       X(k)=AX(k-1)+Bu(k-1)+W(k-1) 測量方程       Z(k)=HX(k)+V(k)  &

卡爾曼濾波（Kalman Filter）原理與公式推導

公式推導 領域 公式 不一定 技術 精度 原理 應用 定性 一、背景---卡爾曼濾波的意義 隨著傳感技術、機器人、自動駕駛以及航空航天等技術的不斷發展，對控制系統的精度及穩定性的要求也越來越高。卡爾曼濾波作為一種狀態最優估計的方法，其應用也越來越普遍，如在無人機、機器人等領

卡爾曼濾波基本原理及公式推導

一、卡爾曼濾波基本原理 既然是濾波，那肯定就是一種提純資料的東西。怎麼理解呢，如果現在有一個任務，需要知道家裡橘子樹今年長了多少個橘子。你想到去年、前年、大前年這三年你把橘子吃到過年，按每天吃3個來算，大概知道每年橘子樹產了多少橘子，今年的情況應該也差不多。這叫數學模型預測法；不過你懶得去想去年

數據會有一些波動和噪點采用卡爾曼濾波算法進行

卡爾曼濾波實現多項式擬合Matlab

nom and kalman ffi 樣本 矩陣 協方差 數組 fontsize %%%%%%%%%%%%%Q3:多項式系數估計%%%%%%%%%%%%%%%% %%%%%%%%%%2016/07/21%%%%%%%%%%%%%%%%%%% clc;clear;

卡爾曼濾波的一個簡單demo： 恒定加速度模型

obi vtt efk rtp end atp cee cdn bs4 p { margin-bottom: 0.1in; direction: ltr; color: rgb(0, 0, 10); line-height: 120%; text-align: left }

卡爾曼濾波3

比較 應該 想要 不能 模型 不同 隨著 而已 風景 假設你去一個風景區旅遊時候，種下一棵果樹，但顯然從果樹苗到果實不是一天兩天能完成對。需要慢慢長高。而你又不可能經常去看，但又想知道樹的高度，哪怎麽辦？ 所以要解決的問題是：如何正確估計一棵果樹的高度？ 我們想要知道的果

卡爾曼濾波2

推導 概率 精確 情況 多維 結果 width tex 為什麽 作者：肖暢鏈接：https://www.zhihu.com/question/23971601/answer/46480923來源：知乎著作權歸作者所有。商業轉載請聯系作者獲得授權，非商業轉載請註明出處。

卡爾曼濾波

nbsp 應用 一周 log -1 現在 richtext http 一是 假設我養了一只豬： 一周前，這只豬的體重是46±0.5kg。註意，在這裏我用了±0.5，表示其實我對這只豬一周前的體重並不是那麽確定的，也就是說，46k

卡爾曼濾波的原理說明

收集 引入 濾波 div 概率 一個人 pdf span net 在學習卡爾曼濾波器之前，首先看看為什麽叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！卡爾曼全名Ru

【概率機器人】3.1 卡爾曼濾波、擴展卡爾曼濾波和無跡卡爾曼濾波

取出 嘗試 bar return tar 簡化 exp 回顧 clas 這一章將介紹卡爾曼濾波、擴展卡爾曼濾波以及無跡卡爾曼濾波，並從貝葉斯濾波的角度來進行分析並完成數學推導。如果您對貝葉斯濾波不了解，可以查閱相關書籍或閱讀 【概率機器人 2 遞歸狀態估計】。 這三種濾波方

MATLAB-卡爾曼濾波簡單運用示例

south noi 增加 jacob mage 影響 方差 sqrt mark 1、角度和弧度之間的轉換公式？ 設角度為 angle，弧度為 radian radian = angle * pi / 180; angle = radian * 180 / pi; 所以在ma

cv2使用卡爾曼濾波（Kalman Filter）捕捉滑鼠運動

本文主要介紹在cv2中使用Kalman濾波捕捉滑鼠運動。 cv2.KalmanFilter(dynamParams=None,#狀態的維度 measureParams=None, #測量的維度 controlParams=None,#控制的維度 type=None)#矩陣的型別

卡爾曼濾波MATLAB程式碼實現

沒有大量的公式推導，個人感覺也沒有必要，我們從小推導過很多公式，試著想想我們還能回憶起幾個？個人認為只需要記住公式的用法，作用，知道有這個公式就可以。用的時候我們可以隨時去查。所以樓主參考網上資料結合一個小例子整理出卡爾曼濾波的MATLAB程式碼實現。看懂這些程式碼我們需要對卡爾曼濾波演算法有基本的

通俗理解卡爾曼濾波及其演算法實現（帶例項解析）

1．簡介(Brief Introduction) 在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！ 卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於

初學者的卡爾曼濾波——擴充套件卡爾曼濾波

簡介 轉自：http://www.cnblogs.com/ymxiansen/p/5368547.html 　　已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著

詳解卡爾曼濾波

輕鬆一下看一個例子： 一片綠油油的草地上有一條曲折的小徑，通向一棵大樹。一個要求被提出：從起點沿著小徑走到樹下。 “很簡單。” A說，於是他絲毫不差地沿著小徑走到了樹下。 現在，難度被增加了：蒙上眼。 “也不難，我當過特種兵。” B說，於是他歪歪扭扭地走到了樹 ……