Kalman Filter學原理學習

1. Kalman Filter 歷史

Kalman濾波器的歷史，最早要追溯到17世紀，Roger Cotes開始研究最小均方問題。但由於缺少實際案例的支撐（那個時候哪來那麼多雷達啊啥的這些訊號啊），Cotes的研究讓人看著顯得很模糊，因此在估計理論的發展中影響很小。17世紀中葉，最小均方估計（Least squares Estimation）理論逐步完善，Tobias Mayer在1750年將其用於月球運動的估計，Leonard Euler在1749年、Pierre Laplace在1787分別用於木星和土星的運動估計。Roger Boscovich在1755用最小均方估計地球的大小。1777年，77歲的Daniel Bernoulli（大名鼎鼎的伯努利）發明了最大似然估計演算法。遞迴的最小均方估計理論是由Karl Gauss建立在1809年（好吧，他聲稱在1795年就完成了），當時還有Adrien Legendre在1805年完成了這項工作，Robert Adrain在1808年完成的。
在1880年，丹麥的天文學家Thorvald Nicolai Thiele在之前最小均方估計的基礎上開發了一個遞迴演算法，與Kalman濾波非常相似。在某些標量的情況下，Thiele的濾波器與Kalman濾波器時等價的，Thiele提出了估計過程噪聲和測量噪聲中方差的方法（過程噪聲和測量噪聲是Kalman濾波器中關鍵的概念）。上面提到的這麼多研究估計理論的先驅，大多是天文學家而非數學家。現在，大部分的理論貢獻都源自於實際的工程。“There is nothing so practical as a good theory”，應該就是“實踐是檢驗真理的唯一標準”之類吧。
現在，我們的控制論大Wiener終於出場了，還有那個叫Kolmogorov（柯爾莫戈洛夫）的神人。在19世紀40年代，Wiener設計了Wiener濾波器，然而，Wiener濾波器不是在狀態空間進行的（這個學過Wiener濾波的就知道，它是直接從觀測空間z(n)=s(n)+w(n)進行的濾波），Wiener是穩態過程，它假設測量是通過過去無限多個值估計得到的。Wiener濾波器比Kalman濾波器具有更高的自然統計特性。這些也限制其只是更接近理想的模型，要直接用於實際工程中需要足夠的先驗知識（要預知協方差矩陣），美國NASA曾花費多年的時間研究維納理論，但依然沒有在空間導航中看到維納理論的實際應用。
在1950末期，大部分工作開始對維納濾波器中協方差的先驗知識通過狀態空間模型進行描述。通過狀態空間表述後的演算法就和今天看到的Kalman濾波已經極其相似了。Johns Hopkins大學首先將這個演算法用在了導彈跟蹤中，那時在RAND公司工作的Peter Swerling將它用在了衛星軌道估計，Swerling實際上已經推匯出了（1959年發表的）無噪聲系統動力學的Kalman濾波器，在他的應用中，他還考慮了使用非線性系統動力學和和測量方程。可以這樣說，Swerling和發明Kalman濾波器是失之交臂，一線之隔。在kalman濾波器聞名於世之後，他還寫信到AIAA Journal聲討要獲得Kalman濾波器發明的榮譽（然而這時已經給濾波器命名Kalman了）。總結其失之交臂的原因，主要是Swerling沒有直接在論文中提出Kalman濾波器的理論，而只是在實踐中應用。
Rudolph Kalman在1960年發現了離散時間系統的Kalman濾波器，這就是我們在今天各種教材上都能看到的，1961年Kalman和Bucy又推導了連續時間的Kalman濾波器。Ruslan Stratonovich也在1960年也從最大似然估計的角度推匯出了Kalman濾波器方程。

2. Kalman Filter 白話理解

引自：Highgear
一片綠油油的草地上有一條曲折的小徑,通向一棵大樹.一個要求被提出: 從起點沿著小徑走到樹下。“很簡單。” A說，於是他絲毫不差地沿著小徑走到了樹下。現在，難度被增加了：蒙上眼。“也不難，我當過特種兵。” B說，於是他歪歪扭扭地走到了樹旁。“唉，好久不練，生疏了。” （只憑自己的預測能力）。“看我的，我有 DIY 的 GPS！” C說，於是他像個醉漢似地歪歪扭扭的走到了樹旁。“唉，這個 GPS 沒做好，漂移太大。”（只依靠外界有很大噪聲的測量）“我來試試。” 旁邊一也當過特種兵的拿過 GPS, 蒙上眼，居然沿著小徑很順滑的走到了樹下。（自己能預測+測量結果的反饋）
“這麼厲害！你是什麼人?”
“卡爾曼 ! ”
“卡爾曼？！你就是卡爾曼？”眾人大吃一驚。 “我是說這個 GPS 卡而慢。”

引自：Kent Zeng
假設你有兩個感測器，測的是同一個訊號。可是它們每次的讀數都不太一樣，怎麼辦？
取平均。
再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？
加權平均。
怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，（此處省略若干數學公式），你可以得到一個“最優”的權重。接下來，重點來了：假設你只有一個感測器，但是你還有一個數學模型。模型可以幫你算出一個值，但也不是那麼準。怎麼辦？
把模型算出來的值，和感測器測出的值，（就像兩個感測器那樣），取加權平均。
OK，最後一點說明：你的模型其實只是一個步長的，也就是說，知道x(k)，我可以求x(k+1)。
問題是x(k)是多少呢？答案：x(k)就是你上一步卡爾曼濾波得到的、所謂加權平均之後的那個對x在k時刻的最佳估計值。
於是迭代也有了。這就是卡爾曼濾波

引自：周嘯
假設我養了一隻豬：

一週前，這隻豬的體重是46±0.5kg。注意，在這裡我用了±0.5，表示其實我對這隻豬一週前的體重並不是那麼確定的，也就是說，46kg這個體重有0.5kg的誤差。
現在，我又養了這隻豬一個星期。那麼我想要知道它一個星期之後多重，又大概有多少的誤差？

為了得到一週後的體重，我有兩種方法：一是根據我多年的養豬經驗得到的豬體重公式推求出一個大概的值，另一個就是直接去稱它的體重。當然，兩種方法都有一定的誤差。假設經驗公式得到的體重是48kg，誤差2kg；直接稱體重得到的是49kg，誤差1kg：

可是，我是一個處女座的人，不管是經驗公式得到的值，還是直接稱量得到的值，我都覺得不夠準。我希望有一種方法，可以同時結合這隻豬一週前的體重、用經驗公式估計的值以及直接稱量得到的值，綜合考慮，得出一個最接近豬真實體重的，誤差最小的值。這就是卡爾曼濾波要完成的任務。現在我們來把養豬的模型抽象成數學公式：

上圖的左邊，上一週的豬的體重，可以抽象為也k-1時刻的狀態值，用k-1時刻的最優估計值加上一個誤差項來表示，右邊同理。其中，

P=E[ekeTk](31)$$\begin{array}{}\text{(31)}& P=E[e_k{e}_k^T]\end{array}$$

這一項表示的是估計值的協方差。這裡要說明兩點：1，上圖中所有的變數都是用粗體，表示這是一個向量或者一個矩陣；2，之所以用（列）向量而非一個數來表示狀態值，是因為，雖然一隻豬的體重可以用一個值來表示，但是在實際的應用中很多狀態並不是一個數就能表示的（比如導彈在空間中的位置，同時有x、y、z三個座標）。上圖中右邊表示k時刻的狀態值，這個值可以通過預測模組（也就是根據經驗公式估計豬的體重）和糾錯模組（也就是直接去稱量豬的體重值）來估計。同樣，預測模組和糾錯模組都有著對應的誤差和誤差協方差矩陣。卡爾曼濾波要做的，就是根據貝葉斯估計的相關理論，同時考慮預測模組和糾錯模組的協方差，對誤差小的項賦予較大的權重，對誤差大的項賦予較小的權重，並使預測的誤差最小。

具體的實現過程如下：

知乎肖暢的回答也很好，推薦檢視，這裡附上幾張簡略圖
在t=0時刻，原始位置

在t=1時刻，預測位置

在t=1時刻，測量位置

結合預測和測量

3. 先驗知識

卡爾曼濾波是一種高效率的遞迴濾波器(自迴歸濾波器), 它能夠從一系列的不完全及包含噪聲的測量中，估計動態系統的狀態。卡爾曼濾波的一個典型例項是從一組有限的，包含噪聲的，對物體位置的觀察序列（可能有偏差）預測出物體的位置的座標及速度。

3.1 正太分佈

在概率論中，正態（或高斯）分佈是一個非常普遍的連續概率分佈。先來簡單看一元高斯分佈，其概率密度為：

p(x)∼N(u,σ2)(21)$$\begin{array}{}\text{(21)}& p(x)\sim N(u,{\sigma }^2)\end{array}$$
f(x|u,σ2)=12πσ2−−−−√e−(x−u)22σ2(22)$$\begin{array}{}\text{(22)}& f(x|u,{\sigma }^2)=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{-\frac{(x-u{)}^2}{2{\sigma }^2}}\end{array}$$
其中，u$u$ 是分佈的均值或期望; σ$\sigma$ 是標準差; σ2${\sigma }^2$ 是方差。下圖為一元高斯的一個示意圖。

再談多元高斯分佈，k維隨機向量 X=[X1，X2，...，Xk]T$X=[X1，X2，...，Xk{]}^T$ 的多元正態分佈：
X∼N(u,Σ)(23)$$\begin{array}{}\text{(23)}& X\sim N(u,\mathrm{\Sigma })\end{array}$$
這裡舉例二元高斯分佈，其概率密度為：

正太分佈的性質

1 經過線性變換高斯分佈仍然為高斯分佈

X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)(5)$$\begin{array}{}\text{(5)}& \begin{array}{l}X\sim N(\mu ,\mathrm{\Sigma })\\ Y=AX+B\end{array}\}\Rightarrow Y\sim N(A\mu +B,A\mathrm{\Sigma }{A}^T)\end{array}$$
2 高斯變數線性組合仍為高斯分佈
X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1+X2)∼

相關推薦

卡爾曼濾波（Kalman Filter）原理理解和測試

Kalman Filter學原理學習 1. Kalman Filter 歷史 Kalman濾波器的歷史，最早要追溯到17世紀，Roger Cotes開始研究最小均方問題。但由於缺少實際案例的支撐（那個時候哪來那麼多雷達啊啥的這些訊號啊），Cotes

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

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

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

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

卡爾曼濾波（Kalman Filter）的通俗解釋

假設你有兩個感測器，測的是同一個訊號。可是它們每次的讀數都不太一樣，怎麼辦？ 取平均。 再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？ 加權平均。 怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，（此處省略若干數學公式

卡爾曼濾波（一）

舉個例子，上一時刻房間溫度最優值為26度，由於溫度變化緩慢，則可以根據經驗估計本次溫度也為26度，即四式右邊第一項，而此時溫度計讀數為28度，那麼本時刻溫度應該是多少？不知道溫度計精度的情況下，可以對兩個資料求平均，為27度，即T=26+0.5*（28-26）=27。這裡0.5就是本例子中的修正係數，那麼假如

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

簡介 　　已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。 　　為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開

慣性導航——擴充套件卡爾曼濾波（一）

對於無人機的慣性導航系統，系統的狀態方程是非線性的，根據擴充套件卡爾曼濾波方程： Predict x^k|k−1Pk|k−1=f(x^k−1|k−1,uk−1)=Fk−1Pk−1|k−1FT

擴充套件卡爾曼濾波（EKF）

首先進行文件下載 仔細閱讀文件，理解文件中所述內容。 我對文件的matlab程式碼進行了簡單調整如下： %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mpu6050姿態解算與卡爾曼濾波（1）數學

定義地理座標系n系：x軸指向東，y軸指向北，z軸指向天。在mpu6050晶片上定義載體座標系b系。那麼b系的姿態就是指n系與b系相對的旋轉關係，即如何由n系旋轉到b系。 描述這種旋轉關係通常使用尤拉角(ψ,θ,γ)T，姿態矩陣T(3x3)，四元數Q=(q0，q

[Math]理解卡爾曼濾波器 (Understanding Kalman Filter)

想要 文章 開始 and 結果 update tla 多少 play 1. 卡爾曼濾波器介紹 卡爾曼濾波器的介紹， 見 Wiki 這篇文章主要是翻譯了 Understanding the Basis of the Kalman Filter Via a Simple

OpenCV之Kalman fileter（卡爾曼濾波）

原理的通俗解釋 用一個網上流傳溫度估計的例子（根據 k-1 時刻的溫度值，估算 k 時刻的是實際溫度值），為了通俗會意，我這裡儘量避免使用專業術語。 假設我們相信溫度是恆定的，所以得到 k 時刻的溫度預測值是跟 k-1 時刻一樣的，假設是23 度，同時該值的高斯噪聲的偏差是 5 度。然後，你

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

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

卡爾曼濾波原理及應用（一）

出於科研需要，打算開始學習卡爾曼濾波（Kalmam Filter）。很早之前就聽說過卡爾曼濾波,但一直沒能深入學習，這次終於有機會了，哈哈。 1.卡爾曼濾波的發展過程 卡爾曼濾波的本質屬於"估計"範疇.先介紹下估計，所謂“估計”問題，就是對收到隨機干擾和隨機測量誤差作用的物理系統，按照某種效

卡爾曼濾波—原理介紹（轉載）

原文連結為：https://blog.csdn.net/heyijia0327/article/details/17487467 這是我看到的寫的比較好的卡爾曼濾波的介紹。 卡爾曼濾波（Kalman filter）是一種高效率的遞迴濾波器（自迴歸濾波器），它能夠從一系列的不完全及包含噪聲的測量中

Kalman Filter : 理解卡爾曼濾波的三重境界

第一重：初見Kalman 假設我養了一隻豬： 一週前，這隻豬的體重是46±0.5kg。注意，在這裡我用了±0.5，表示其實我對這隻豬一週前的體重並不是那麼確定的，也就是說，46kg這個體重有0.5kg的誤差。現在，我又養了這隻豬一個星期。那麼我想要知道它

無人駕駛四 擴充套件卡爾曼濾波在目標車輛運動狀態識別中的運用（python程式）

# coding=utf-8 import numpy as np import numdifftools as nd import math dataset = [] # read the measurement data, use 0.0 to stand LI

無人駕駛汽車系統入門（一）——卡爾曼濾波與目標追蹤

前言：隨著深度學習近幾年來的突破性進展，無人駕駛汽車也在這些年開始不斷向商用化推進。很顯然，無人駕駛汽車已經不是遙不可及的“未來技術”了，未來10年

卡爾曼濾波初探（一）

卡爾曼濾波初探 基於時域的線性模型預測 這裡先給出幾個概念（初看的時候很多部落格都沒有這方面說明，若你看到下面懵逼的時候，不妨上來再看看？）  預測：就是根據已有的①經驗、②公式、③以及上一個時間（）下檢測物件的狀態的最優估計等資訊，從而得到一個對下一個

Kalman filter卡爾曼濾波的原理說明

在學習卡爾曼濾波器之前，首先看看為什麼叫“卡爾曼”。跟其他著名的理論（例如傅立葉變換，泰勒級數等等）一樣，卡爾曼也是一個人的名字，而跟他們不同的是，他是個現代人！ 卡爾曼全名Rudolf Emil Kalman，匈牙利數學家，1930年出生於匈牙利首都布達佩斯。1953，1954年於麻省理工學院分別獲得電

無人駕駛汽車系統入門（二）——高階運動模型和擴充套件卡爾曼濾波

前言：上一篇文章的最後我們提到卡爾曼濾波存在著一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。所以之前為了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模