讀書筆記-學習《Kalman-and-Bayesian-Filters-in-Python》

4 人贊了文章

鏈接：rlabbe/Kalman-and-Bayesian-Filters-in-Python

github上只是一個介紹，真正的互動式閱讀方法，是點擊"launch binder"的圖片按鈕，就進入到

https://hub.mybinder.org/user/rlabbe-kalman-a-lters-in-python-cf0n6uul/tree?token=HGVEKvpXQjee0TCiqbnt8w

這裡，列出了所有的章節，點擊任何一個章節，進入該章節，就可以閱讀並在線修改代碼，在線運行。在文本框中修改代碼後，按Ctrl+Enter即可運行。

上述方法比較慢，有時候會死。此外，還有一種方法可以快速看（僅僅看不能在線修改代碼），即將.ipynb文件的url複製到https://nbviewer.jupyter.org/頁面中的框中。

如何使用作者的成果：

1）預先安裝NumPy，SciPy，matplotlib。這都是一些開源標準庫。

2）安裝作者的主要成果，FilterPy。這是一個了不起的多種類濾波器集合。

安裝方法：在Ubuntu中，sudo pip install filterpy。

使用方法：from filterpy.xxx import yyy即可。xxx是某個子模塊（例如卡爾曼子模塊），yyy是某個類，例如卡爾曼濾波器類。

3）下載本書。以便運行書中的例子。

本書中的代碼在kf_book子目錄下，它裡面包含了作者寫的一些用於顯示結果的圖表類。

如果要寫一個測試常式test.py，則可以把kf_book目錄複製到test.py的同級目錄下，然後例如from kf_book.mkf_internal import plot_track的方式引用其中的圖表類。

00-Preface 前言

這章很簡單，就是介紹numpy scipy等基礎知識。

01-The g-h Filter g-h濾波

用一個體重變化的例子，來講述g-h濾波。它是所有濾波的基礎，卡爾曼濾波等等都是它的一種。

g-h濾波實際上是一個套路，它的過程是：

1）預測一步。

2）利用測量值更新一步。再回到1。

g-h濾波中有2個參數，g是測量值的權重，h是預測值的權重。

一種演算法的描述是：

初始化

1。初始化過濾器的狀態

2。初始化我們對狀態的信念

預測

1。使用系統行為預測下一步的狀態

2。調整信念來考慮預測的不確定性

更新

1。得到一個測量值和其精度的信念

2。估計狀態與測量之間的殘差

3。新估計是在殘差線上的某個地方

02-Discrete Bayes Filter 離散貝葉斯濾波

卡爾曼濾波器屬於一個稱為貝葉斯濾波器的濾波器族。

這裡距離一個測量狗狗在走廊哪個位置的例子，來說明「預測」-「更新」模型是如何運作的。

引入了卷積，作為概率預測模型，下面就是一個核為[0.1,0.7,0.2]的離散概率擴散模型，意思是，每個位置的概率質量，在下一時刻，0.8的概率來自自己，0.2的概率來自左右鄰居。

from filterpy.discrete_bayes import predict

belief = [.05, .05, .05, .3, .55, .3, .05, .05, .05, .05]

prior = predict(belief, offset=1, kernel=[.1, .8, .1])

從結果[0.05 0.05 0.05 0.1 0.4 0.15 0.05 0.05 0.05 0.05]上看，右邊概率大一些，所以，predict是一個概率推的模型，不是一個拉的模型。

這裡又引入了一個火車在軌道上行走的例子，這個例子更加通用，在每次迭代中，做以下代碼：

robot.move(distance=move_distance) #操縱火車向前走4步

prior = predict(posterior, move_distance, kernel) #用自帶的predict()預測4步之後的概率質量，其中kernel=[0.1,0.8,0.1]，表示一步的概率離散程度。

m = robot.sense() #測量一下車輛所在的位置，注意這個函數有9成正確率，一成錯誤1格的概率。

likelihood = lh_hallway(track, m, sensor_accuracy) #利用測量得到的m，對所有的離散位置，計算車輛再此處的可能性。

posterior = update(likelihood, prior) #利用自帶的update()函數，對概率質量進行一次貝葉斯後驗迭代。

我用自己的語言來描述依稀離散貝葉斯濾波：

預先設定

狗只能在n個位置。

狗在每個位置的概率，用一個概率質量函數來描述。所以，這個概率質量函數，用來描述狗的位置。

1）初始化，給出初始的概率質量函數，來描述狗的位置。

2）預測狗向右走一步，就是調用一下預測動作。就是把用來描述狗位置的概率質量函數，變化一下。一般情況下，概率質量會變的更加扁平（模糊）。

3）測量一下狗的位置，得到的是一個確定的位置值，雖然這個位置僅僅有一定精確度。

4）利用這個位置值和測量精確度，我們計算出每個位置的likelihood。

5）利用這個likelihood，我們用貝葉斯法則，更新一下概率質量函數。

6）轉到2）

這本書主要是關於卡爾曼濾波器。它使用的數學是不同的，但是邏輯與本章中使用的完全一樣。它使用貝葉斯推理來從測量和過程模型的組合中形成估計。

03-Gaussian Probabilities 高斯分布

用scipy.stats

兩個正態分布相加，還是一個正態分布，μ = μ1 + μ2，σ^2 = σ1^2 + σ2^2

兩個正態分布相乘，還是一個正態分布，μ = (σ1^2 * μ2 + σ2^2 * μ1)/(σ1^2 + σ2^2), σ^2 = (σ1^2 * σ2^2) / (σ1^2 + σ2^2)

04-One Dimensional Kalman Filters 一維卡爾曼濾波

from numpy.random import randn

randn(25)將返回一個25個元素的數組，填充了從均值為0和方差為1的單變數「正態」（高斯）分布中採樣的隨機浮點數。

randn()則返回一個數字，均值為0和方差為1的一個隨機採樣。

randn(3,5)則返一個矩陣，填充了均值為0和方差為1的二維高斯分布中的隨機採樣浮點數。

一個非常簡單有趣的高斯分布預測：

我的狗在10米處，方差是0.2米，我們記為N(10,0.2**2);

狗的速度是15米/秒，每秒的標準誤差07米，則每秒移動的距離可以記為N(15,0.7**2);

那麼，狗在一秒之後的位置就是 N(10+15,0.2**2+0.7**2)。

卡爾曼增益

K = σ1^2/(σ1^2 + σ2^2) K就是卡爾曼增益，

用來表示平均值：μ = K * μ2 + (1 - K) * μ1 = μ1 + K * (μ2 - μ1)

這是Kalman濾波器的關鍵所在。K是一個縮放項，它選擇一個介於μ2和μ1之間的值。

舉個例子，當測量值很准，即方差比先驗值方差小9倍時，σ2^2 = 9 * σ1^2， K = 0.9，則μ = 0.9*μ2 + 0.1*μ1，即後驗值非常接近測量值。

用來表示方差：σ^2 = (1 - K) * σ1^2

這裡，當K越大（測量值越精確），新的方差就越小。

卡爾曼代碼

def update(prior, measurement):

x, P = prior # mean and variance of prior

z, R = measurement # mean and variance of measurement

y = z - x # residual

K = P / (P + R) # Kalman gain

x = x + K*y # posterior

P = (1 - K) * P # posterior variance

return gaussian(x, P)

def predict(posterior, movement):

x, P = posterior # mean and variance of posterior

dx, Q = movement # mean and variance of movement

x = x + dx

P = P + Q

return gaussian(x, P)

從上述卡爾曼增益計算新的平均值的公式上，可以看出，卡爾曼濾波就是g-h濾波的一種。

下面是作者寫的卡爾曼類的使用例子

import filterpy.kalman as kf

x, P = kf.predict(x=10., P=3., u=1., Q=2.**2)

x, P = kf.update(x=x, P=P, z=12., R=3.5**2)

05-Multivariate-Gaussians 多維高斯

numpy.cov()獲得協方差矩陣，注意，它是無偏估計的。

多維正態分布的方程，和一維的類似。SciPy的STATS模塊的multivariate_normal()實現了多維正態方程

import scipy

mu = [2.0, 7.0] # 狗位置是二維正態分布，均值在x=2, y=7處

P = [[8., 0.],

[0., 3.]] # 狗的協方差矩陣，x方向上不確定性大一些，y方向上不確定性小一些，但xy之間無關聯

x = [2.5, 7.3] #

print({:.4f}.format(scipy.stats.multivariate_normal(mu, P).pdf(x)))

# 得到 0.0315， 表示在(2.5, 7.3)這個點，概率密度的值是0.0315

書中用等高線來描述二維高斯分布的形狀。

皮爾森係數 ρxy = COV(X,Y) / (σx * σy) 這個值在-1到1之間，為0時表示x y不相關，越接近-1表示越是負相關，越接近1表示越是正相關。

用皮爾森係數可以解釋等高線橢圓的瘦度--當ρxy接近-1或1時，橢圓越瘦。

例如，當σx^2=1 σy^2=2時，COV(X,Y)=1.414時，橢圓最瘦，接近一個直線。

我們可以使用SIPY.STATS.PARSONR函數來計算皮爾森係數

多維高斯卡爾曼濾波公式，和一維的類似：

μ = Σ2(Σ1+Σ2)^?1 μ1 + Σ1(Σ1+Σ2)^?1 μ2

Σ = Σ1(Σ1+Σ2)^?1 Σ2

這裡提出了一個雷達測飛機二維位置的例子。雷達測方位很准但測距離不準，所以，測出的位置其實就是一個橢圓的正態分布，用一個協方差矩陣表示。

開始飛機是一個大圓。

左下角有一個雷達，測一下，得到一個東北西南向的橢圓，橢圓和大圓相乘，得到更小的橢圓，為更新值。

右下角又有一個雷達，測一下，得到一個西北東南向的橢圓，該橢圓和之前更新的東北西南向的小橢圓相乘，得到一個陰影相交部分的很小的近似圓形的橢圓。

2個雷達最好是正交的，這裡有一個非正交的例子，會發現得到的陰影重疊部分變大，不利於縮小範圍。

二維高斯的誤差，我們用橢圓表示；三維高斯的誤差，可以用橢球表示。

06-多維卡爾曼濾波

過程

Initialization

1. Initialize the state of the filter

2. Initialize our belief in the state Predict

1. Use process model to predict state at the next time step

2. Adjust belief to account for the uncertainty in prediction Update

1. Get a measurement and associated belief about its accuracy

2. Compute residual between estimated state and measurement

3. Compute scaling factor based on whether the measurement or prediction is more accurate

4. set state between the prediction and measurement based on scaling factor

5. update belief in the state based on how certain we are in the measurement

公式

預測

x1 = F x + B u

P1 = F P T(F) + Q

x,P是均值和協方差矩陣. 對應著一維卡爾曼的x和σ2.

F是狀態轉移函數。當乘以x時，它計算先驗。

Q是過程協方差。

更新

y = z - H x1

K = P1 T(H) (H P1 T(H) + R)^-1

x = x1 + K y

P = (I - K H) P1

H是測量函數。如果你從等式中移除HH，你應該能夠看到這些方程也是相似的。

z、R是測量均值和雜訊協方差。

y、K是殘差和卡爾曼增益

這個公式的含義和一維卡爾曼的意義完全相同

用高斯表示狀態和誤差的估計

用高斯表示測量及其誤差

用高斯表示過程模型

使用過程模型預測下一個狀態（先驗）

在測量與先驗之間形成一個估計部分

你作為設計師的任務是設計狀態（X，P），過程（F，Q），測量（Z，R），和測量函數H。如果系統有控制輸入，例如機器人，你也將設計B和U。

一個追蹤狗的例子

狗有2個狀態：位置和速度

我們設計狗的初始狀態：

x = [10, 4.5] # 位於10米處，速度為4.5

P = np.diag([500., 49.]) #diag()生成對角矩陣。500表示我們對初始位置的不確信；49是因為，我們假設狗最大速度是21m/s，取3σ=21, σ=7, σ^2=49

過程模型設計：

參考：scipy.linalg.solve()可以用來解簡單的矩陣方程Ax=B

注意，x1 = F x, 我們作為卡爾曼濾波器設計者的工作是指定F，使得x1 == F x，對我們的系統進行預測。

我們認為 x1 = x + v * dt

v1 = v

所以有, F = [ [1, dt], [0, 1] ]

關於白雜訊，我們專門寫了一個函數，來計算：

from filterpy.common import Q_discrete_white_noise

Q = Q_discrete_white_noise(dim=2, dt=1., var=2.35) # 得到一個二維的，1秒的，方差是2.35的白雜訊矩陣

此外，我們的狗沒有任何控制輸入，所以B=0, u=0，當然，0也是他們的默認值。

總結：我們作為設計師，任務就是指定矩陣。

X，P：狀態和協方差

F、Q：過程模型和雜訊協方差

B，u：可選地，控制輸入和函數

測量函數的設計

我們的工作，是設計2個矩陣。

測量空間

有時候測量空間和計算空間不一致。例如，我們需要測電壓，但需要通過溫度來簡介測量電壓。

大部分情況下，測量是不可逆的，例如，可以把電壓轉成溫度值，但不能把預測的溫度值反過來轉成電壓值來進行計算。

又比如，考慮給目標提供範圍的感測器。將一個範圍轉換成一個位置是不可能的——一個圓中無限數量的位置將產生相同的範圍。

在我們的追蹤狗的例子中，測量只能得到位置值。所以，測量空間是1維的，而預測空間是2維的。

預測值x和測量值z都是向量，所以我們需要一個轉換來計算殘差：

y=z?Hx

其中y為殘差，x為先驗，z為測量，H為測量函數。因此，我們取先驗，將其轉換為測量，通過與H相乘，並從測量中減去。

我們的狀態x有2個值[位置、速度]，但我們的感測器只能測位置，所以我們的z是一個只有一個元素的向量[z]

因為，y=z-Hx，所以，H就必須是一個[? ?]的一行二列的矩陣了。

我們測量得到的位置和預測的位置是一致的，因此乘以1就可以得到，而我們測量不到速度，所以就乘以0，所以 H=[1 0]

設計測量

我們測量的z是均值，還要有測量的協方差矩陣R。

z很容易，我們只測量到了位置，所以z=[位置]。如果，我們能同時得到速度的話，z=[位置, 速度]

協方差矩陣R具有m*m的尺寸，其中m是感測器的數量。這是一個協方差矩陣來解釋感測器之間的相關性。我們只有1個感測器，所以R是：[σ^2]

現在，我們假設這個感測器的的方差是5，所以R = [5]

如果，我們有了2個獨立的位置感測器，第一個方差為5，第二個方差為3，我們會寫：

R = [[5 0], [0, 3]]

使用例子

from filterpy.kalman import KalmanFilter

KalmanFilter是作者封裝的一個類，只要把各種矩陣都填好，就可以迭代它了。

更新方程

系統不確定性

看下面兩個類似的表達式：

S = H P1 T(H) + R # 坐標系統線性變換引起的協方差的改變

P1 = F P T(F) + Q # 狀態轉換引起的協方差的改變

卡爾曼增益

K = P1 T(H) S^-1 # 0到1之間的實數

下面的方式，可以幫助理解K

K ≈ (P1 / S) T(H)

K ≈ (預測的不確定性 / 測量的不確定性) T(H)

所以，當測量非常精確時，K就很大，導致結果向測量值那邊靠攏。

總的來說，K就是，將先驗的不確定性與先驗和測量的不確定性的總和相除。

殘差

y = z ? H x1

測量函數將一個預測值轉換成一個測量值。然後用真的測量值減去它。

在我們的狗例子中，就是把2維的預測值轉換成1維的測量值，然後用真的測量值減去它。

狀態更新

x = x1 + K y

我們選擇我們的新狀態沿殘差，按卡爾曼增益縮放。縮放由Ky來執行，Ky既對殘差進行縮放，又用K中的T(H)項將其轉換成狀態空間。

協方差更新

P = (I ? K H) P1

我們可以這樣來理解它：P=(1?cK)P1，如果測量值更精確，就K大，則(1?cK)小，則更新的P就小（狀態精確）；反之，測量值不精確，就K小，則(1-cK)大，則更新的P就不夠小（狀態模糊）

一個不用FilterPy的定位狗的例子

dt = 1. # 每步1秒

R_var = 10 # 測量方差

Q_var = 0.01 # 預測方差

x = np.array([[10.0, 4.5]]).T # 開始時，在10米處，速度4.5

P = np.diag([500, 49]) # 開始時，位置方差500，速度方差49

F = np.array([[1, dt],

[0, 1]]) # 用牛頓公式定義轉換矩陣

H = np.array([[1., 0.]]) # 只測量位置，H是把2維的預測值轉換成為1維的測量值。

R = np.array([[R_var]]) # 測量方差

Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var) # 用預定義的方式，計算過程協方差矩陣

from numpy import dot

from scipy.linalg import inv

count = 50

track, zs = compute_dog_data(R_var, Q_var, count) # 獲得隨機生成的狗的位置測量值

xs, cov = [], []

for z in zs:

# predict

x = dot(F, x)

P = dot(F, P).dot(F.T) + Q

#update

S = dot(H, P).dot(H.T) + R

K = dot(P, H.T).dot(inv(S))

y = z - dot(H, x)

x += dot(K, y)

P = P - dot(K, H).dot(P)

xs.append(x)

cov.append(P)

xs, cov = np.array(xs), np.array(cov)

plot_track(xs[:, 0], track, zs, cov, plot_P=False, dt=dt)

過濾器初始化

一個好的辦法是，在獲取第一個測量值z後，用這個測量值作為初始化值x。

如果存在H，則x = H^-1 z。矩陣求逆需要一個正方形矩陣，但H很少是正方形的，所以我們可以用scipy.linalg.pinv()來求偽逆。

初始速度，可以用第一個測量值和第二個測量值的差計算出來。

P的初始值，其中位置的方差，可以用測量方差R；其中速度的方差，最大速度平方是一個合理的值。

成批處理

卡爾曼濾波器被設計為遞歸演算法-隨著新的測量進來，我們立即創建一個新的估計。但是有一組數據是很常見的，這些數據已經被收集，我們想要過濾。卡爾曼濾波器可以在批處理模式下運行，其中所有的測量都被立即過濾。我們已經在Kalman過濾器中實現了這一點。

一個例子如下：

count = 50

track, zs = compute_dog_data(10, .2, count)

P = np.diag([500., 49.])

f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)

xs, _, _, _ = f.batch_filter(zs)

平滑結果

有一種情況，需要用到之後發生的事情來對當前的信號做決定。例如，汽車行駛過程中，當GPS跳向左邊，需要判斷是否真左轉了還是一個噪音。此時，就需要之後的幾幀數據作為判定依據。

Kalman濾波器實現了該演算法的一種形式，稱為RTS平滑器 rts_smoother()，使用它通過從batch_filter()步驟計算出的均值和協方差，並接收平滑的均值、協方差和卡爾曼增益

一個例子如下：

from numpy.random import seed

count = 50

seed(8923)

P = np.diag([500., 49.])

f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)

track, zs = compute_dog_data(3., .02, count)

Xs, Covs, _, _ = f.batch_filter(zs)

Ms, Ps, _, _ = f.rts_smoother(Xs, Covs)

book_plots.plot_measurements(zs)

plt.plot(Xs[:, 0], ls=--, label=Kalman Position)

plt.plot(Ms[:, 0], label=RTS Position)

plt.legend(loc=4);

討論與總結

多元高斯允許我們同時處理多個維度，包括空間和其他維度（速度等）。我們做了一個關鍵的見解：隱藏變數有能力顯著提高過濾器的精度。這是可能的，因為隱藏變數與觀察到的變數相關。

R和Q的設計通常很有挑戰性。感測器常常不是高斯型，通常有胖尾（稱為峰度）和歪斜。Q的情況更可怕，你不知道狗下一步可能怎樣。

這些問題導致一些研究人員和工程師貶低地稱Kalman過濾器為「泥球」。

另一個要知道的術語——卡爾曼濾波器會變得自鳴得意。

設計一個線性濾波器的矩陣是一個實驗過程，而不是一個數學過程。使用數學來建立初始值，但是你需要進行實驗。

07-卡爾曼濾波數學

動態系統的狀態空間表示

由高階方程生成一階方程

我們最簡單的動力學方程就是x=v了，它可以轉換成x=Ax+Bu+w的形式。

卡爾曼提出了狀態空間方法，對於高階的動力學微分方程，把它轉換成1階微分方程組。

例如對微分方程 x?6x+9x=u

我們把最高次項拿到左邊，變成 x=6x-9x+u

在令x1(u)=x x2(u)=x，我們知道x1=x2, x2=x，所以就有 x2 = 6 x2 - 9 x1 + t，因此，我們的一階方程組是

x1 = x2

x2= 6 x2 - 9 x1 + t

如果你稍微練習一下，你就會熟練掌握它。隔離最高項，定義一個新的變數及其導數，然後替換。

狀態空間形式的一階微分方程

x1=x x1=x2 x2=x3 ... 微分方程最高階是n的話，就可以轉化成n次1階微分方程組。就成為以下形式

x = Ax+Bu

其中A是系統動力學矩陣

求時不變系統的基本矩陣

我們已經知道了A，如何求F？使F滿足遞推關係 new_x = F(Δt) x，Δt拿掉就可以簡寫為 new_x = F x

換言之，A是一組連續微分方程，我們需要F是一組離散線性方程組，計算離散時間步長中A的變化。

一般來說，有三種常用的方法來尋找卡爾曼濾波器的這種矩陣。最常用的技術是矩陣指數法。線性時不變理論，也稱為LTI系統理論，是第二種技術。最後，有數值技術。

矩陣指數

先來解一個微分方程 x = kx，得到x=c0 e^(kt)，具體解的時候，可以利用在線積分計算器https://zh.numberempire.com/integralcalculator.php

同理，當x為向量的時候，x=Ax也可以解得x = e^(At) x0，所以，當F = e^(At)時，就有 new_x = F x。

e^(At)稱之為矩陣指數。它可以用這個冪級數來計算：

e^(At) = I + At + (At)^2 / 2! + (At)^3 / 3! + ...

來個例子，勻速直線運動的牛頓方程：

T([x, v]) = [[0 1],[0 0]] * T([x, v])

上式中，A = [[0 1],[0 0]]，表示x=v, v=0，是一個勻速直線運動。

F = e^(At) = I + At + + (At)^2 / 2! + ...

注意，上式中A^2 = [[0 0],[0 0]]，所以，

F = e^(At) = I + At = [[1 0],[0 1]] + [[0 1],[0 0]]t = [[1 t],[0 1]]，果然，和常識一致。

scipy.linalg.expm()可以計算矩陣指數。舉例如下：

import numpy as np

from scipy.linalg import expm

dt = 0.1

A = np.array([[0, 1],

[0, 0]])

expm(A*dt)

# 結果是array([[ 1.0, 0.1],[ 0.0, 1.0]])

時不變

如果系統的行為依賴於時間，我們可以說動態系統是由一階微分方程描述的

g(t) = x

然而，如果系統是時不變的，方程的形式是：

f(x) = x

時間不變數意味著什麼？考慮一個家庭立體聲。如果在時間t中輸入一個信號x，它將輸出一些信號f（x）。如果在時間t+Δt中執行輸入，則輸出信號將是相同的f（x）。

一個反例是x(t)=sin(t)，f(x)=t x(t)，此時，t不同的時候，同樣的x，輸出不同的f(x)。

我們可以通過積分各邊來求解這些方程。 v=x這個方程很好做，但x=f(x)就較難了。

數值解

x=Ax+Gw

C. F. van Loan開發了一種既能找到數值也能找到Q的技術。代碼如下：

from filterpy.common import van_loan_discretization

A = np.array([[0., 1.], [-1., 0.]])

G = np.array([[0.], [2.]]) # white noise scaling

F, Q = van_loan_discretization(A, G, dt=0.1)

過程雜訊矩陣的設計

一般來說，Q矩陣的設計是卡爾曼濾波器設計中最困難的方面之一。

這是由於幾個因素造成的。首先，數學需要一個良好的信號理論基礎。第二，我們試圖在我們沒有什麼信息的情況下模擬雜訊。

我們將考慮兩種不同的雜訊模型。

連續白雜訊模型

我們使用牛頓方程來模擬運動學系統。然後我們可以假設加速度對於每個離散的時間步長是恆定的。換言之，我們假設速度的微小變化隨著時間的推移平均為0（零均值）

分段白雜訊模型

另一個模型假定，最高階項（例如，加速度）在每個時間段的持續時間是恆定的，但對於每個時間段是不同的，並且每一個在時間段之間是不相關的。

使用過濾器計算Q

from filterpy.common import Q_continuous_white_noise

from filterpy.common import Q_discrete_white_noise

Q = Q_continuous_white_noise(dim=2, dt=1, spectral_density=1)

08-設計卡爾曼濾波

跟蹤機器人

我們的例子，是跟蹤一個在二維平面上移動的機器人。

選擇狀態變數

[x,y]太簡單了。我們使用[x, vx, y, vy]。因為我們是每隔1秒測量一下，所以，vx就是兩幀x的差，vy也類似。

設計狀態轉移函數F

x = 1x + Δt vx + 0y + 0 vy

vx = 0x + 1 vx + 0y + 0 vy

y = 0x + 0 vx + 1y + Δt vy

vy = 0x + 0 vx + 0y + 1 vy

所以，我們有

from filterpy.kalman import KalmanFilter

tracker = KalmanFilter(dim_x=4, dim_z=2) #預測是4維的，測量是2維的

dt = 1. # time step 1 second

tracker.F = np.array([[1, dt, 0, 0],

[0, 1, 0, 0],

[0, 0, 1, dt],

[0, 0, 0, 1]])

過程雜訊矩陣Q的設計

FiTyPy可以為我們計算Q矩陣。為了簡單起見，我假設雜訊是離散時間Wiener過程——它對於每個時間周期是恆定的。如果不清楚，請重新訪問卡爾曼濾波器數學章節。

from scipy.linalg import block_diag

from filterpy.common import Q_discrete_white_noise

q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)

tracker.Q = block_diag(q, q)

print(tracker.Q)

測量函數H的設計

我們假設，測量的單位是英尺而預測的單位是米。1英尺=0.3048米。

tracker.H = np.array([[1/0.3048, 0, 0, 0],

[0, 0, 1/0.3048, 0]])

測量雜訊矩陣R的設計

我們假設，測量x,y方向上的誤差是獨立的，是5m2。

tracker.R = np.array([[5., 0],

[0, 5]])

初始條件

我們假設開始在[0,0]處，速度為0。這個假設不很有信心，所以方差很大。

tracker.x = np.array([[0, 0, 0, 0]]).T

tracker.P = np.eye(4) * 500.

實現過濾器

設計已經完成，現在我們只需編寫代碼來運行過濾器並輸出我們選擇的格式的數據。我們將運行代碼30次迭代。

濾波器的階

0階，靜態系統。例如測量靜止物體的位置。

1階，有一階導數，比如速度。例如測量勻速行動的小車位置。

2階，有二階導數，比如加速度。例如測量勻加速運動的小車的位置。

我們模擬出一個一階的測量數據。然後用0階濾波器和2階濾波器去濾波，結果發現，0階始終落後，2階雖然基本吻合，但殘差比1階大很多且有時無法收斂。

低階濾波器可以跟蹤更高階的過程，只要增加足夠的過程雜訊，並且保持離散化周期小（100個採樣每秒，通常是局部線性的）。

我們再次模擬出一個二階的測量輸出。然後用1階的濾波器去濾波，我們把Q增大了10倍，結果效果還不錯。

不良測量的檢測與剔除

有時候，會得到偏離過遠的測量值。這時候要考慮不要他。

首先，讓我們介紹兩個術語。我們正在討論門控。門是一種公式或演算法，它決定測量的好壞。只有好的測量才能通過大門。這個過程稱為門控。

在實踐中，測量不是純高斯的，所以3個標準偏差的門很可能會丟棄一些好的測量結果。我將很快詳細說明，現在我們將使用4個標準偏差。

通過計算馬氏距離（一個樣本距離中心的有幾個標準差），來進行去除壞的測量結果。

門控與數據關聯策略

有矩形門（計算壓力小）、策略門（不太壞）、橢圓形門（計算壓力大但更準確），此外還有各種門，例如汽車，將有一個小得多的2D餅狀的形狀突出在它前面。

如何選擇門是一個藝術。在5個維度上，矩形門是接受橢球的壞測量的兩倍。

如果需要快，並且壞數據多，可以採用2門法：第一個門是大矩形們，用來做粗過濾。第二個橢圓門，來做更昂貴的馬氏距離計算。

歸一化估計誤差平方（NEES）

首先，只有在模擬系統里，才有真值，才有NEES

x = x_true - x_filtered 先得到真值和濾波結果的差

? = T(x) P^?1 x 求出NEES，從式子上可以看出，P小會導致NEES變大。?是一個標量。

NEES的意義是：取所有NEES值的平均值，它們應該小於X的維數。例如x是四維的如[x, vx, y, vy]，那麼NEES均值應該小於4才是好的濾波器。

提供了filterpy.stats.NEES。這是過濾器性能的一個極好的度量，並且應該儘可能使用，特別是在生產代碼中，當您需要在運行時對過濾器進行評估時。

似然函數

濾波器的殘差和系統不確定性定義為

y = z - H x

S = H P T(H) + R

然後用類似高斯方程的方法計算可能性

from scipy.stats import multivariate_normal

hx = np.dot(H, x).flatten()

S = np.dot(H, P).dot(H.T) + R

likelihood = multivariate_normal.pdf(z.flatten(), mean=hx, cov=S)

用plt.plot(s.log_likelihood);語句可以把一次濾波過程中的每幀的likelihood列印出來，如果likelihood變大則表示濾波器好，如果接近0則表示濾波器壞。

控制輸入

x = F x + B u

這裡用軌道上的機器人的例子來描述控制。如果發一個速度指令，機器人可以立即變化為指定的速度v1。那麼，

x = x + v1 Δt

v = v1

則可以令 u = [v]， B = T([Δt, 1])

這是一種簡化；典型的控制輸入是轉向角的變化和加速度的變化。這引入了非線性，我們將在後面的章節中學習處理。

感測器融合

我們有兩個尺度的濾波器，一個精確，一個不精確。我們應該如何將其納入卡爾曼濾波器？

例1：假設一輛車上有GPS和速度脈衝（車輪轉一圈給一個脈衝）。我們假設速度脈衝測量得到的也是絕對位置x，則令預測值為[x, v]，測量值為[x_gps, x_wheel]，即可。

例2：假設空間一個點在(100,100)處，我們有2個雷達，一個在(50,50)處，一個在(150,50)處。雷達的角度誤差在0.5度，距離誤差在3米。

練習：你能過濾GPS輸出嗎？

GPS的輸出，其實是經過了過濾的。如果再過濾一次，會不會更好？這裡做了一個實驗，把GPS過濾一次，將結果作為輸入，再過濾一次，再將結果作為輸入，再過濾一次。

結果發現，得到的曲線，越來越平滑，僅此而已，並沒有越來越正確。

這是因為，卡爾曼濾波要求輸入是時間無關的。而濾波的結果，則是時間相關的。

非平穩過程

如果我們的數據率以某種不可預測的方式發生變化呢？或者，如果我們有兩個感測器，每個感測器以不同的速率運行？如果測量誤差發生了什麼變化？

解決方案很簡單，在濾波器迭代過程中，修改參數矩陣F或其他參數即可。

感測器融合：不同的數據速率

兩個不同的感測器類以相同的速率輸出數據是罕見的。假設位置感測器產生3 Hz的更新，並且車輪在7赫茲下更新。實驗了一個勻速運動的軌道小車的例子。

這個代碼是非常重要的，值得看看！

追蹤球

一個例子，我們把一個球，斜向天空拋出。不考慮空氣摩擦力，我們可以用Runge-Kutta方法計算出軌跡作為真值。然後用卡爾曼濾波。

選擇狀態變數：

我們用歐拉法代替Runge-Kutta法來寫離散方程，因為足夠精確且不需要編寫自定義預測函數。這意味著我們需要將Y的加速度合併到卡爾曼濾波器中，而不是X。

所以，狀態變數是：x=[x x y y y]T

但因為重力是已知常數，所以，不需要y，把他放到控制變數矩陣B中間即可。所以，狀態變數是：x=[x x y y]T

設計狀態轉移函數

F=[[1 dt 0 0],

[1 1 0 0],

[1 0 1 dt],

[1 0 0 1]]

控制輸入功能的設計

我們將使用控制輸入來解釋重力的作用。把Bu這個詞加到X中，以解釋X因重力變化的多少。可以說，Bu包含[dx, dvx, dy, dvy]。

如果我們看離散方程，我們看到重力隻影響Y的速度。因此，我們希望產品Bu等於[0 0 0 -g*dt]。

最終的設計 B = [0 0 0 dt]T u = [0 0 0 -g]

測量函數的設計

z = [x, y]T

H = [[1 0 0 0],

[0 0 1 0]]

測量雜訊矩陣的設計

假設誤差在x和y中是獨立的，都為0.5米平方。

R = [[0.5 0],

[0 0.5]]

過程雜訊矩陣的設計

我們假設一個球在真空中運動，所以不應該有過程雜訊。我們有4個狀態變數，所以我們需要一個4×4全0的協方差矩陣。

追蹤一個空氣中的球

這次，把空氣阻力也算上了。有一個複雜的公式來生成球的新軌跡。新軌跡比老軌跡要低矮一些，球先落地。

但，還用上次的那個濾波器來濾波，發現，當R大的時候，相信預測值多一些，導致軌跡高了遠了。當R小的時候，則軌跡偏的不太多。

為了解決這個問題，我們把球受到的阻力想像成過程雜訊Q，原先Q為0，當Q為0.01的時候，軌跡好了一些，當Q為0.1的時候，軌跡和真值基本重合了。

當我們使過程雜訊高於系統中的實際雜訊時，濾波器將選擇對測量結果進行更高的稱量。如果你在測量中沒有太多噪音，這對你可能有效。

然而，考慮下一個情節，我增加了測量中的雜訊。結果就悲催了。

有了這一點，當然可以使用過程雜訊來處理系統中的小非線性。這是卡爾曼濾波器的「黑色藝術」的一部分。

09-非線性濾波

講述了幾個非線性的例子，表線性卡爾曼濾波應用時無法收斂。

對於非線性問題，作者更加傾向於使用UKF（無跡卡爾曼濾波）而不是EKF（擴展卡爾曼濾波）。UKF已經取得了巨大的成功。

UKF和粒子濾波器。他們更容易實施，理解和使用，而且他們通常比EKF更穩定。

10-無跡卡爾曼濾波

x是高斯分布，但f(x)不是高斯分布。每次更新，我們生成500,000個點，將它們傳遞給函數，然後計算結果的均值和方差。這被稱為蒙特卡洛方法。

它被一些卡爾曼濾波器設計所使用，例如集成濾波器和粒子濾波器。

這種方法確實有效，但它在計算上非常昂貴。集成濾波器和粒子濾波器使用巧妙的技術來顯著降低這種維度，但計算負擔仍然非常大。

無跡卡爾曼濾波器使用西格瑪點，但通過使用確定性方法來選擇點大大減少了計算量。

西格瑪點 - 從分布中抽樣

from numpy.random import multivariate_normal

快速示例

在線性卡爾曼濾波中，我們使用H作為測量到狀態的轉換函數。注意，這裡說的是「函數」，所以，對於線性H就是一個矩陣，對於非線性，就是一個函數。

這裡給出了一個例子，是一個線性的情況，但用函數代替H矩陣，用線性卡爾曼濾波器來做濾波。

選擇Sigma點

如何選擇，有無數種辦法。更一般的方法是計算加權平均值。即選擇若干個西格瑪點，並給每個西格瑪點加上權值wm（用於計算均值）,wc（用於計算協方差矩陣）

無跡變換

目前，假設存在用於選擇西格瑪點和權重的演算法。西格馬點如何實現過濾器？

簡而言之，無跡變換需要從一些任意的概率分布中抽取點，並將它們傳遞給一個任意的非線性函數，並為每個變換點產生一個高斯函數。我希望你可以設想我們如何使用它來實現非線性卡爾曼濾波器。一旦我們有了高斯，我們已經開發出來的所有數學工具都會發揮作用！

Unscented變換的準確性

MerweScaledSigmaPoints() JulierSigmaPoints()這些函數，都可以產生5個西格瑪點，以及相應的Wm, Wc。

unscented_transform() 可以將西格瑪點變換後的結果，和Wm Wc混合在一起，生成結果的均值和協方差矩陣。

結果非常棒！用500000個點模擬出來的效果，和用5個西格瑪點模擬出來的效果幾乎一致。我對此的理解是：

從一個分布中，找出5個點，給予不同的權值；然後對這5個點執行非線性變換，把5個結果，用先前的權值組合起來，居然均值和協方差，和總樣本相差無幾！

所以，找出哪5個點，給予什麼樣的權重，就是無跡變換的核心了。

Van der Merwe的縮放西格瑪點演算法

Rudolph Van der Merwe在2004發明的方法，它在各種問題中表現良好，並且在性能和準確性之間有良好的折衷。

該公式使用3個參數來控制西格瑪點如何分布和加權 α , β, and κ.

西格瑪點的數目為2n+1個。在二維情況下，就是5個西格瑪點。第一個是均值，其他4個按一定公式得到，對稱分布在四周。

11-擴展卡爾曼濾波

12-粒子過濾器

適合粒子過濾器的實際情況是：

multimodal：我們想要同時跟蹤零個，一個或多個對象。

遮擋：一個對象可以隱藏另一個對象，從而導致對多個對象進行一次測量。

非線性行為：飛機受到風的衝擊，球以拋物線運動，人們相互碰撞。

非線性測量：雷達給我們到物體的距離。將它轉換為（x，y，z）坐標需要非線性的平方根。

非高斯雜訊：當物體在背景中移動時，計算機視覺會將部分背景誤認為物體。

連續的：物體的位置和速度（即狀態空間）可以平滑地隨時間變化。

多變數：我們想追蹤幾個屬性，比如位置，速度，轉彎率等。

未知的過程模型：我們可能不知道系統的過程模型

我們所學的過濾器都不能很好地處理所有這些限制。

離散貝葉斯過濾器：這具有大部分屬性。它是多模式的，可以處理非線性測量，並且可以擴展以處理非線性行為。但是，它是離散的和單變數的。

卡爾曼濾波器：卡爾曼濾波器針對具有高斯雜訊的單峰線性系統產生最優估計。這些都不適用於我們的問題。

Unscented卡爾曼濾波器：UKF處理非線性，連續，多變數問題。但是，它不是多模式的，也不處理遮擋。它可以處理適度非高斯的雜訊，但對於非高斯分布或非線性問題並不適用。

擴展卡爾曼濾波器：EKF具有與UKF相同的優勢和局限性，但它對強非線性和非高斯雜訊更為敏感。

通用粒子濾波演算法

1. 隨機生成一堆粒子

粒子可以擁有你需要估計的位置，航向和/或任何其他狀態變數。每個人都有一個權重（概率），表明它與系統的實際狀態匹配的可能性。用相同的重量初始化每一個。

2.預測粒子的下一個狀態

根據預測真實系統的行為方式移動粒子。

3.更新

根據測量值更新粒子的權重。與測量非常匹配的粒子的權重要高於與測量結果不匹配的粒子的權重。

4.重新取樣

丟棄非常不可能的顆粒並用更可能的顆粒的副本代替它們。

5.計算估算

可選地，計算該組粒子的加權均值和協方差以得到狀態估計。

13-平滑

建議始終使用RTS平滑

14-自適應濾波

一個例子，車輛勻速直線運動，但在一個特定的位置進行了加速度轉向。現在有3種濾波方案：

1）不考慮加速度的濾波方案。這種方案在轉向後飛了，很久才能回到正確的軌跡。

2）上述方案，把Q加大。這種方案更相信測量值，所以轉完後很快就能跟上軌跡，但平時受測量誤差影響，不夠平滑。

3）採用考慮加速度的濾波方案。這種方案在轉向後立即跟上，但是在穩態行為期間以非常嘈雜的輸出為代價。

看來我們贏不了。恆速濾波器在目標加速時不能快速反應，但恆定的加速濾波器在零加速狀態期間將雜訊誤解為加速而不是噪音。

檢測機動

所謂機動發生的時候，就是物體運動模型和預測模型差別產生的時候，此時，殘差最大。

可調過程噪音

我們將考慮的第一種方法將使用低階模型，並根據是否正在進行機動動作來調整過程噪音。當殘差變得「很大」（對於大的合理定義），我們會增加過程噪音。這將導致過濾器偏好過程預測的測量結果，並且過濾器將嚴密跟蹤信號。當殘差很小時，我們將縮小過程噪音。

連續調整

第一種方法（來自Bar-Shalom [1]）使用以下等式對殘差的平方進行歸一化。

殘差一旦超過某個極限（例如4個標準差），我們將放大Q（例如乘以1000倍）。如果殘差又低於該極限，則恢復Q。

該濾波器的性能明顯好於等速濾波器。操縱開始後，等速濾波器花費大約10秒時間重新獲取信號。自適應濾波器在一秒鐘內完成相同的操作。

連續調整 - 標準偏差版本

另一種非常類似的方法來自ZrAgman〔2〕，基於測量誤差協方差的標準偏差設置極限。

如果殘差的絕對值大於上述計算的標準偏差的某個倍數，我們將過程雜訊增加一個固定的量，重新計算Q，並繼續。

衰落記憶過濾器

如果目標是機動的，它並不總是像過程模型預測的那樣工作。在這種情況下，記住所有過去的測量和估算是一種負擔。

衰落記憶過濾器通過減少舊的測量權重來解決這個問題，並且對於最近的測量更重要。

方法是在P的變換過程中，增加一個乘法因子a。如果a=1，則就是普通卡爾曼濾波。一般a比1大一點點，例如1.01

用於上述「車輛勻速直線運動，但在一個特定的位置進行了加速度轉向」的例子，a=1.02效果好了一些，a=1.05效果更好了。

附錄A 安裝

這本書需要IPython，Jupyter，NumPy，SciPy，SymPy和Matplotlib。這些都是開源的。

我已經在Ubuntu中安裝了NumPy，SciPy，matplotlib。IPython，Jupyter如果不用來做互動式網頁看書的話，不用安裝。SymPy是用於執行符號數學的Python包，尚未安裝。

安裝 FilterPy(作者的成果)

sudo pip install filterpy