Kalman實際應用總結

時間 2019-11-09

標籤 kalman 實際應用總結简体版

原文原文鏈接

目錄html

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

網上大部分都是理論和簡單的例子，不多看到實戰的信息git

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

Kalman理論介紹

一. 簡單理論介紹理論

二. 昇華理論介紹

Kalman基本應用

一. Kalman跟蹤/濾波

對單個數據濾波，沒法創建運動學模型算法

經過創建和自身相關的狀態方程便可app

是一種平滑操做（上一時刻和當前時刻的關係）dom

舉例：機器學習

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

速度，加速度，角速度$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預測/融合（單傳感器）

[x] 運動學模型
[x] 單一傳感器
[x] 速度$、，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()

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