Kalman實際應用總結

文章目錄

• 友情提示
• • Kalman理論介紹
  • • 一. 簡單理論介紹理論
    • 二. 升華理論介紹
  • Kalman基本應用
  • • 一. Kalman跟蹤/濾波
    • 二. Kalman預測/融合（單傳感器）
    • 三. Kalman多傳感器融合A
    • 四. Kalman多傳感器融合B
    • 五. Kalman多傳感器融合C
    • 六. Kalman多傳感器融合D
    • 七. Extend Kalman
  • 本文總結

友情提示

友情提示不針對第三方，為了給讀者更好的體驗

• 建議去我的部落格園進行閱讀
• 微信位址
• GitHub位址
• 歡迎大家關注我的微信公衆号

理論部分不詳細說明，網上大部分都給出很好的解釋

網上大部分都是理論和簡單的例子，很少看到實戰的資訊

本博文是筆者實際使用的總結，如有錯誤，請不吝指教

Kalman理論介紹

一. 簡單理論介紹理論

• B站大神的講解，視訊很簡單
• 詳細理論英文版
• 詳細理論中文版
• 好的博文一
• 好的博文二
• 車輛行及推演

二. 升華理論介紹

• 優達學城的筆記
• 徐亦達機器學習Kalman Filter 卡爾曼濾波
• 無人駕駛大神筆記EKF
• 英文文獻一
• 英文文獻二
• 車輛運動導航算法
• 很多文獻忘記記錄,有侵權請告知

Kalman基本應用

一. Kalman跟蹤/濾波

對單個資料濾波，無法建立運動學模型

通過建立和自身相關的狀态方程即可

是一種平滑操作（上一時刻和目前時刻的關系）

舉例：

對一個平面運動的質點進行跟蹤（ 、 X 、 Y 、X、Y 、X、Y）？

• 速度$、，v、\alpha，\omega $都是未知狀态

求解：

fig = plt.figure()
axis = fig.add_subplot(1,1,1)

func_data = lambda x : x + x^2
z = np.mat(func_data(np.arange(1,100)))
x_mat = np.mat([[0,],[0.]])#狀态矩陣[x,delta_x]
p_mat = np.mat([[1, 0], [0, 1]])#狀态協方差矩陣
f_mat = np.mat([[1, 1],[0.,1.]])#狀态轉移矩陣
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
h_mat = np.mat([1.,0])# 觀測矩陣[x]
r_mat = np.mat([1])#觀測協方差矩陣
result = []
for i in range(z.shape[1]):
    x_predict = f_mat * x_mat
    p_predict = f_mat * p_mat * f_mat.T + q_mat
   
    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)  
    
    x_mat = x_predict + kalman *(z[0, i] - h_mat * x_predict)
    p_mat = (np.eye(2) - kalman * h_mat) * p_predict
    result.append(x_predict[0,0])

axis.plot(result,label='predict')
axis.plot(z.tolist()[0],label='groundtruth')
axis.legend()
           

二. Kalman預測/融合（單傳感器）

• 運動學模型
• 單一傳感器
• 速度$、，v、\alpha，\omega $推導可知

舉例一：

• 問題和例子文檔

一個運動小車的位置和速度的測量等資訊可以被測量(一個傳感器)，也可以通過牛頓運動學方程進行解算，這兩個到底誰占的比例高？使用Kalman的協方差矩陣進行比例的計算。。。。具體看文檔

舉例二：

• stackoverflow上的一個例子

import numpy as np
import matplotlib.pyplot as plt

def kalman_xy(x, P, measurement, R,
              motion = np.matrix('0. 0. 0. 0.').T,
              Q = np.matrix(np.eye(4))):
    """
    Parameters:    
    x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
    P: initial uncertainty convariance matrix
    measurement: observed position
    R: measurement noise 
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    """
    return kalman(x, P, measurement, R, motion, Q,
                  F = np.matrix('''
                      1. 0. 1. 0.;
                      0. 1. 0. 1.;
                      0. 0. 1. 0.;
                      0. 0. 0. 1.
                      '''),
                  H = np.matrix('''
                      1. 0. 0. 0.;
                      0. 1. 0. 0.'''))

def kalman(x, P, measurement, R, motion, Q, F, H):
    '''
    Parameters:
    x: initial state
    P: initial uncertainty convariance matrix
    measurement: observed position (same shape as H*x)
    R: measurement noise (same shape as H)
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    F: next state function: x_prime = F*x
    H: measurement function: position = H*x

    Return: the updated and predicted new values for (x, P)

    See also http://en.wikipedia.org/wiki/Kalman_filter

    This version of kalman can be applied to many different situations by
    appropriately defining F and H 
    '''
    # UPDATE x, P based on measurement m    
    # distance between measured and current position-belief
    y = np.matrix(measurement).T - H * x
    S = H * P * H.T + R  # residual convariance
    K = P * H.T * S.I    # Kalman gain
    x = x + K*y
    I = np.matrix(np.eye(F.shape[0])) # identity matrix
    P = (I - K*H)*P

    # PREDICT x, P based on motion
    x = F*x + motion
    P = F*P*F.T + Q

    return x, P

def demo_kalman_xy():
    x = np.matrix('0. 0. 0. 0.').T 
    P = np.matrix(np.eye(4))*1000 # initial uncertainty

    N = 20
    true_x = np.linspace(0.0, 10.0, N)
    true_y = true_x**2
    observed_x = true_x + 0.05*np.random.random(N)*true_x
    observed_y = true_y + 0.05*np.random.random(N)*true_y
    plt.plot(observed_x, observed_y, 'ro')
    result = []
    R = 0.01**2
    for meas in zip(observed_x, observed_y):
        x, P = kalman_xy(x, P, meas, R)
        result.append((x[:2]).tolist())
    kalman_x, kalman_y = zip(*result)
    plt.plot(kalman_x, kalman_y, 'g-')
    plt.show()

demo_kalman_xy()
           

這部分比較簡單，網上的例子大部分都是基于此的。。。

三. Kalman多傳感器融合A

• 運動學模型
• 多個傳感器
• 傳感器時間序列不同

舉例：

以汽車跟蹤為例，目标是知道汽車時刻的狀态 x = ( p x , p y , v x , v y ) x=(p_x,p_y,v_x,v_y) x=(px​,py​,vx​,vy​) x = ( p x , p y , v x , v y ) x=(p_x,p_y,v_x,v_y) x=(px​,py​,vx​,vy​)

已知的傳感器有 、 l i d a r 、 r a d a r 、lidar、radar 、lidar、radar。

l i d a r lidar lidar：笛卡爾坐标系。可檢測到位置，沒有速度資訊。其測量值 z = ( p x , p y ) z = ( p x , p y ) z=(px,py)z=(px,py) z=(px,py)z=(px,py)。

r a d a r radar radar：極坐标系。可檢測到距離，角度，速度資訊，但是精度較低。其測量值 z = ( ρ , ϕ , ρ ˙ ) z = ( ρ , ϕ , ρ ˙ ) z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙) z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)，

這是優達學城的一個例子，具體我也沒視訊網址。

m a t l a b matlab matlab代碼位址在這裡， p y t h o n python python代碼在這裡

注意：

​ 這裡相當于建立了兩個模型，一個線性模型，一個非線性模型，在不同的時刻使用不同的傳感器進行更新

​ 其實就是單個傳感器合并到一起了。。。。

四. Kalman多傳感器融合B

• 無運動學模型
• 多傳感器
• 傳感器時序相同

舉例：

一個小車做不均則運動（速度、加速度、角速度等都是可變的），現在有兩個傳感器：儀器A和儀器B，他們都能測量 ω \omega ω 和 v v v ,那麼如何進行融合兩個傳感器呢？

• 具體的代碼這裡不友善給出，有需要可以一起讨論

這裡其實和Kalman的濾波比較類似，就是把兩個傳感器當做一個傳感器的不同時間序列 T 1 , T 2 T_1,T_2 T1​,T2​ 時刻測量的資料，然後濾波操作。

五. Kalman多傳感器融合C

• 無運動學模型
• 多傳感器
• 傳感器時序相同

條件和Kalman多傳感器融合B相同，單處理方式不同

由于部分傳感器精度不同，進行特定的取舍很有必要（親身經曆）

假設求取小車的 ω \omega ω 和 v v v

傳感器A對 ω \omega ω 測量較為準确

傳感器B對 v v v 測量較為準确

解決：

其實我們如果直接按照Kalman多傳感器融合B進行操作的話，誤差基本不會縮小，可能還會增加

這個時候筆者的解決方案是把傳感器A和B當做一個整體傳感器C，傳感器C測量的 ω \omega ω 是A的，測量的 v v v 是B的

那麼我們就把這個合起來的傳感器C進行濾波就行了

實測可用。。。

六. Kalman多傳感器融合D

• 運動學模型
• 多傳感器
• 傳感器時序相同

看到網上很多人問這個問題，這裡筆者沒有親自實作，隻是做了猜想，不正确還望讀者指正

解決：

由于卡爾曼隻能一次融合兩個資訊（預測和觀測），是以隻能進行如下想法

1. 進行兩次融合，一次是預測和傳感器A，一次結果和傳感器B（這部分就是多傳感器B）
2. 進行一次融合，預測和新的傳感器C（Kalman多傳感器融合C）

七. Extend Kalman

• 運動學模型不是線性的
• 使用雅克比代替狀态矩陣和觀測矩陣

注意：

筆者認為這種情況比較少見，因為 t t t 趨向于 ϵ \epsilon ϵ ，是以可以認為在無窮小的區間都近似于很恒定的

實在沒辦法的時候就使用EKF，原理都很簡單，計算代價大許多

後續可以使用UKF進行操作，這部分筆者還未嘗試

本文總結

最後來個簡單的總結，什麼是卡爾曼 K K K？

兩個相同資訊：A 和 B

都滿足 y = k x + b y=kx+b y=kx+b

那麼如何得到 y y y ?

正常來說： y = ( A + B ) / 2 ∗ x + b y=(A+B)/2*x+b y=(A+B)/2∗x+b

但是好像不是非常好，這個 ( A + B ) / 2 (A+B)/2 (A+B)/2總是不變的，假如他們某個時刻占比改變了呢？

這個時候 K a l m a n Kalman Kalman的作用的展現了，他計算A和B的關系（看公式吧）

得出一個系數 K K K 這個 K K K 和A、B相關

此時： y = K ∗ x + b y=K*x+b y=K∗x+b

輸入的A、B不同，那麼 K K K也不同

完畢！！！