文章目錄

2、線性卡爾曼濾波（Linear Kalman Filter, KF）

前言—最優狀態估計與濾波

卡爾曼濾波器在Python的filterpy模塊裏面全都有：線性卡爾曼，擴展卡爾曼和無極卡爾曼。這裏記錄自己的學習。

1、最小均方濾波（Least Mean Square, LMS）

基本原理

最小均方濾波大致思路：對於一大段受到噪聲干擾的數據，如果能從中獲取到一小段不含噪聲的理想數據，則可以通過理想數據和受干擾的數據訓練出來一個濾波器，這個濾波器通過學習可以自己濾掉干擾噪聲。這和神經網絡或者機器學習有相似的地方。原理圖如下所示：

d(n)爲理想數據序列，w(n)爲實際數據序列。

LMS設計步驟

對於LMS濾波器，首先提幾個很重要的參數：濾波器的階數—ord（有的也叫抽頭）、訓練步長train_len，學習率learning_rate，後面一一用到再介紹。現在假設有一個包含噪聲的序列X，然後還知道序列X中X(1)~X(m)（假設序列編號是從1開始，且X是列向量）這一段序列的真實值爲X_ideal。則設計LMS濾波器的步驟爲：

1、首先初定一個階數ord和訓練步長train_len(隨便確定兩個整數即可，但要求滿足ord+train_len<=m)；
2、定義一個權值矩陣(列向量)W，大小爲（ord，1），初值全部設置爲0；
3、計算序列X的自相關矩陣，即：A = X*tranpose(X)，同時計算矩陣A的最大特徵值eigA_max，則學習率learning_rate = 1/abs(eigA_max)；

4、接下來則開始一個循環：

for (i=1；i<=train；i++)
		W = W - 2*learning_rate*(tranpose(X(i：i+ord))*W-X_ideal(i:i+ord))

循環結束後的W即爲訓練完的權值矩陣。

5、最後，利用訓練完的權值矩陣則直接可以對X序列進行濾波了，即：
```
X_ideal(k) = tranpose(X(k-ord,k))*W
```

上面可以看出，濾波輸出爲當前測量值和之前若干測量值的加權求和，這種權值就是通過學習訓練得到，尤其是第4步更新權值的算法，就是梯度下降，這種過程和神經網絡的訓練很類似。這裏的性能函數爲：

仿真代碼

這裏序列是1000個數據點，用於訓練的數據點是125個。

import numpy as np
import matplotlib.pyplot as plt


class LMSFilter():
    def __init__(self, ideal_sequence, real_sequence, filter_ord, learning_rate, train_len):
        '''
        :param ideal_sequence:理想輸出序列
        :param real_sequence:實際輸出序列
        :param filter_ord:濾波器階數
        :param learning_rate:學習率
        :param train_len:訓練長度
        '''
        self.ideal_sequence = ideal_sequence
        self.real_sequence = real_sequence
        self.filter_ord = filter_ord
        self.learning_rate = learning_rate
        self.train_len = train_len
        self.__Train()

    def __Train(self):
        self.W = np.zeros(self.filter_ord)#權值矩陣
        for i in range(self.filter_ord - 1, self.filter_ord + self.train_len):
            measured = self.real_sequence[i - self.filter_ord + 1: i+1]
            y_ideal = self.ideal_sequence[i]
            y_real = np.matmul(measured, self.W)
            error = y_real - y_ideal
            self.W = self.W - 2*self.learning_rate*error*measured

    def Filter(self):
        '''
        :return:返回濾波值和濾波誤差
        '''
        self.y_filtered = np.zeros(len(self.real_sequence))#定義一個矩陣存放濾波後的值
        self.y_filtered[:] = None#初始化，用於訓練的數據段不需要輸出。
        for i in range(self.filter_ord + self.train_len, len(self.real_sequence)):
            measured = self.real_sequence[i - self.filter_ord + 1: i+1]
            self.y_filtered[i] = np.matmul(measured, self.W)
        self.error = self.ideal_sequence - self.y_filtered
        return self.y_filtered, self.error


if __name__=="__main__":
    t = np.linspace(0,10,1000)#時間序列
    noise = 50*np.random.randn(1000)#噪聲序列
    X_ideal = 100*np.sin(t)#理想序列
    Measured = X_ideal + noise#實際測量序列
    A = np.array([Measured])#求輸入序自相關矩陣的特徵值
    B = np.matmul(np.transpose(A),A)
    Eig = np.linalg.eig(B)
    learning_rate = 1/max(abs(Eig[0]))#學習率取最大特徵值的倒數
    Example = LMSFilter(X_ideal, Measured, 25, learning_rate, 100)#初始化一個LMS，並開始訓練
    Filtered, Error = Example.Filter()#返回尋訓練後的LMS濾波器
    F1, = plt.plot(t, Measured, color='r')
    F2, = plt.plot(t, Filtered, color='b')
    F3, = plt.plot(t, X_ideal, color='k')
    F4, = plt.plot(t, Error, color='y')
    plt.legend(handles=[F1, F2, F3, F4], labels=['Measured', 'Filtered', 'Ideal', 'Error'])
    plt.show()

結果：

可以發現前面有125個點沒有輸出，這些點用於訓練了，所以不拿來再作爲測試數據（類似神經網絡裏面的測試集和訓練集）。可以發現，濾波效果有，但不是很好，這裏的噪聲爲平穩隨機噪聲。LMS的幾個參數對濾波效果有很大的影響，可以自己改改相應試一試。

2、線性卡爾曼濾波（Linear Kalman Filter, KF）

使用示例

考慮一個真實的物理系統：一輛汽車，當汽車運動起來後，車就具有速度，同時車在大地座標系的位置也會發生改變。現在，考慮這樣一個問題。如果有一套GPS安裝在車上面，就可以實時獲得車輛的座標和速度。但是，對於任何一個測量信號入股不進行濾波的話將會有噪聲成分干擾測量。此外，GPS是一種低頻信號（10Hz左右），想想當汽車以120km/h速度行駛時候，在120/3.6x0.1=3.3m內是無法知道汽車的真實位置。因此，卡爾曼濾波器一個很重要的應用就是GPS與慣導的融合（GPS與IMU融合介紹）。接下來，就以這種融合做一個很簡單線性卡爾曼濾波的示例（示例很簡單，並不是嚴格的GPS與IMU融合）。

基本步驟

線性卡爾曼濾波器是一種基於模型的濾波器，在使用時必須建立起研究對象相對精確的狀態空間方程。線性卡爾曼濾波的理論推導網上很多，這裏不重複。卡爾曼的基本步驟是“計算先驗狀態和協方差”—>“計算卡爾曼增益”—>“計算後驗狀態和協方差”。

仿真模型

仿真模型爲一個在平面二維運動汽車，汽車的狀態有X座標，Y座標，X方向速度和Y方向速度，這裏假設利用GPS測量汽車的座標信息，利用IMU測量汽車的加速度Ax和Ay。汽車運動方程具體矩陣A、B、C參數見程序[Main.py]

# Main.py
# linear system
# X(k + 1) = AX(k) + BU(k)
# Y(K + 1) = CX(k + 1)
from LKFilter import *
import matplotlib.pyplot as plt
import math

if __name__=="__main__":
    T = 0.01#時間採樣步長
    N = 0#計數器，乘以T後用來輸出連續時間
    A = np.matrix([1, 0, T, 0, 0, 1, 0, T, 0, 0, 1, 0, 0, 0, 0, 1]).reshape(4, 4)#狀態矩陣
    B = np.matrix([0.5 * T ** 2, 0, 0, 0.5 * T ** 2, T, 0, 0, T]).reshape(4, 2)#輸入矩陣
    C = np.matrix([1, 0, 0, 0, 0, 1, 0, 0]).reshape(2, 4)#輸出矩陣
    I = np.eye(4)#單位矩陣
    Q = np.diag((0.0, 0.0, 0.0, 0.0))#系統噪聲協方差矩陣
    R = np.diag((0.1, 0.1))#測量噪聲協方差矩陣
    X_est = np.matrix([0.0, 0., 0.0, 0.]).reshape(4, 1)  # 後驗狀態初始化
    X_pre = np.matrix([0., 0., 0., 0.]).reshape(4, 1)  # 先驗狀態初始化
    X_true = np.matrix([0., 0., 0., 0.]).reshape(4, 1)  # 真實狀態初始化
    Y_observed = np.matrix([0., 0.]).reshape(2, 1)  # 測量值初始化
    P0 = np.diag((10, 10, 0, 0))  # 初始狀態協方差矩陣

    Example = KFilter(A, B, C, Q, R, T, P0, X_true, X_est)#實例化一個線性卡爾曼濾波器

    while True:
        N+=1
        Ax = 30 * np.sin(2 * math.pi * N*T)#x方向加速度
        Ay = 90 * np.cos(2 * math.pi * N*T)#y方向加速度
        U = np.array([Ax, Ay]).reshape(2, 1)#組合成輸入向量
        W = np.matmul(np.sqrt(Q), np.random.randn(4).reshape(4, 1))#系統噪聲矩陣，這裏放在while循環裏面，每次都可以產生一個隨機矩陣
        V = np.matmul(np.sqrt(R), np.random.randn(2).reshape(2, 1))#測量噪聲矩陣
        '''利用上述的矩陣來構建一個仿真系統'''
        X_true_tem = np.matmul(A, X_true[:, -1]) + np.matmul(B, U) + W
        X_true = np.column_stack((X_true, X_true_tem))#真實的狀態序列，這裏用了column_stack，就是將每一個的更新的狀態放在X_true後面，便於最後畫圖
        Y_observed_tem = np.matmul(C, X_true[:, -1]) + V
        Y_observed = np.column_stack((Y_observed, Y_observed_tem))
        '''調用線性Kalman濾波'''
        Result = Example.Kalman(Y_observed[:, -1], U)#這個方法的輸入參數是當前時刻系統的測量值Y_observed[:, -1]和輸入U，返回的是時間序列，所有時刻的觀測狀態與測量值
        '''畫圖，注意Result中返回的值屬於numpy.matrix型數據不是array型，在plot的時候要轉換一下'''
        plt.clf()
        plt.figure(1)
        plt.plot(np.array(Result[0][0, :])[0], np.array(Result[1][0, :])[0], label='Estimated X')
        plt.plot(np.array(Result[0][0, :])[0], np.array(Result[2][0, :])[0], label='Measured X')
        plt.plot(np.array(Result[0][0, :])[0], np.array(X_true[0, :])[0], label='Real X')
        plt.xlabel('Time[s]')
        plt.ylabel('X coordinate[m]')
        if N == 1:
            plt.legend()
        plt.figure(2)
        plt.plot(np.array(Result[1][0, :])[0], np.array(Result[1][1, :])[0], label='Estimated X-Y')
        plt.plot(np.array(Result[2][0, :])[0], np.array(Result[2][1, :])[0], label='Measured X-Y')
        plt.plot(np.array(X_true[0, :])[0], np.array(X_true[1, :])[0], label='Real X-Y')
        plt.xlabel('X coordinate[m]')
        plt.ylabel('Y coordinate[m]')
        plt.pause(0.01)
        plt.ioff()

線性卡爾曼濾波算法，文件名KFilter.py。

# Linear System Kalman Filter
# X(k + 1) = AX(k) + BU(k) + W(k)
# Y(K + 1) = CX(k + 1) + V(k)
# Developed by OldYoung
import numpy as np

class KFilter:
    def __init__(self, A, B, C, Q, R, T, P0_ini, X_true_ini, X_est_ini):
        '''
        :param A:State matrix
        :param B:Input matrix
        :param C:Transfer matrix
        :param Q:System noise covariance
        :param R:Measurement noise covariance
        :param T:Sample time
        :param P0_ini:Initial covariance matrix of state
        :param X_true_ini:Initial value of true states
        :param X_est_ini:Initial value of estimated states
        '''
        self.A = A
        self.B = B
        self.C = C
        self.Q = Q
        self.R = R
        self.A_shape = A.shape
        self.C_shape = C.shape
        self.P0 = P0_ini
        self.T = T
        self.N = 0
        self.I = np.eye(self.A_shape[0])
        self.t = np.matrix(np.zeros((1, 1)))
        self.Y_measured = np.matrix([0.] * self.C_shape[0]).reshape(self.C_shape[0], 1)
        self.Y_true = np.matrix([0.] * self.C_shape[0]).reshape(self.C_shape[0], 1)
        self.X_estimated = X_est_ini
        self.X_true = X_true_ini

    def Kalman(self, Y_measured, U):
        '''
        :param Y_measured: Measured output
        :param U:Input
        :return:[time, estimated states, measured output]
        '''
        self.N += 1
        self.t = np.column_stack((self.t, np.matrix([self.N * self.T])))
        self.Y_measured = np.column_stack((self.Y_measured, Y_measured))
        self.X_pre = np.matmul(self.A, self.X_estimated[:, -1]) + np.matmul(self.B, U)#預測
        self.P_pre = np.matmul(np.matmul(self.A, self.P0), np.transpose(self.A)) + self.Q#預測
        self.K = np.matmul(np.matmul(self.P_pre, np.transpose(self.C)),
                           np.linalg.pinv(np.matmul(np.matmul(self.C, self.P_pre), np.transpose(self.C)) + self.R))#計算卡爾曼增益
        self.X_est_tem = self.X_pre + np.matmul(self.K, self.Y_measured[:, -1] - np.matmul(self.C, self.X_pre))#校正
        self.X_estimated = np.column_stack((self.X_estimated, self.X_est_tem))
        self.P0 = np.matmul(np.matmul(self.I - np.matmul(self.K, self.C), self.P_pre),
                            np.transpose(self.I - np.matmul(self.K, self.C))) + np.matmul(np.matmul(self.K, self.R), np.transpose(self.K))#更新協方差矩陣
        return [self.t, self.X_estimated, self.Y_measured]

仿真結果

可以看到，卡爾曼濾波後的位置與真實位置很接近。

“t—X”軸觀測結果：
“X—Y”座標系觀測結果：

線性卡爾曼的侷限

上面結果表明線性卡爾曼濾波具有很好的效果。但是，線性卡爾曼是一種基於模型的濾波方法。也就是說，如果沒有系統建模方程是不能用卡爾曼濾波（和LMS這些基於數據的濾波器是不一樣的）。並且，如果建立的模型不夠精確，也會導致卡爾曼濾波效果變差甚至發散。此外，設計濾波器的時候，系統噪聲和測量噪聲要求爲高斯白噪聲且需要知道噪聲的統計信息，這在實際應用中很難滿足要求。

噪聲影響

爲了對比，將上面程序Main.py的17、24、32行程序做一下改動，增加一個噪聲R1，假設這是測量噪聲的真實值，並帶入測量矩陣V，但是在給濾波器實例化傳遞參數的時候仍然給R。可以發現，此時的卡爾曼濾波效果相對變差了一些。值得注意的是，系統噪聲一直設爲0，因爲系統噪聲本身就很難測量和估計，這裏就不研究了

R = np.diag((0.1, 0.1))  # 假定測量噪聲協方差矩陣
R1 = np.diag((1, 1))#真實測量噪聲協方差矩陣
Example = KFilter(A, B, C, Q, R, T, P0, X_true, X_est)#實例化一個線性卡爾曼濾波器
V = np.matmul(np.sqrt(R1), np.random.randn(2).reshape(2, 1))#測量噪聲矩陣

建模誤差

同樣，將上面程序Main.py的13、34行程序做一下改動，B看作是我們建立的模型的輸入矩陣，而B1是真實模型輸入矩陣。同樣，用一個不精確的B去初始化卡爾曼濾波器。可以發現，此時的卡爾曼濾波器的效果變得更差，和真實值有很大的出入。

B = np.matrix([0.5 * T ** 2, 0, 0, 0.5 * T ** 2, T, 0, 0, T]).reshape(4, 2)#建模輸入矩陣
B1 = np.matrix([0.5 * T ** 2, 0, 0, 0.5 * T ** 2, 1.2*T, 0, 0, T]).reshape(4, 2)#真實輸入矩陣
Example = KFilter(A, B, C, Q, R, T, P0, X_true, X_est)#實例化一個線性卡爾曼濾波器
X_true_tem = np.matmul(A, X_true[:, -1]) + np.matmul(B1, U) + W

總結：線性卡爾曼濾波最早是用在航空航天領域，可以花巨資獲取精確的數學模型，效果很好。但後來移植到普通工業應用時，受成本和技術限制，不能獲得精確的系統模型和噪聲統計特性，上述侷限就暴露出來。換句話說，線性卡爾曼濾波器的魯棒性較差（魯棒性：控制器或觀測器在系統未建模動態和不確定干擾影響下能保持較好性能的能力）。H無窮大卡爾曼濾波器正是爲了提升傳統卡爾曼的魯棒性而產生的。

[Python|最優狀態估計與濾波學習筆記] 最小均方濾波，卡爾曼濾波，神經網絡濾波

文章目錄

前言—最優狀態估計與濾波

1、最小均方濾波（Least Mean Square, LMS）

基本原理

LMS設計步驟

仿真代碼

2、線性卡爾曼濾波（Linear Kalman Filter, KF）

使用示例

基本步驟

仿真模型

仿真結果

線性卡爾曼的侷限

噪聲影響

建模誤差

3、H無窮卡爾曼濾波（H-infinite KF）

4、擴展卡爾曼濾波（Extend Kalman Filter, EKF）

5、無跡卡爾曼濾波（Unscented Kalman Filter, UKF）

6、神經網絡濾波（Neural Networks Filter, NNF）

持續更新中。。。

[設計模式|C#&Java]設計模式學習筆記

[Python|多線程學習筆記]線程池threadingpool與同步鎖

[Python|自動駕駛|ADAS]曲線軌跡設計

[設計模式|C#]設計模式學習筆記

[Python|最優狀態估計與濾波學習筆記] 最小均方濾波，卡爾曼濾波，神經網絡濾波

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結