卡爾曼濾波預測t + 2時間步長

Question

我有一個關於使用卡爾曼濾波器來預測t + 2值的問題。 眾所周知，基本的卡爾曼濾波器有兩個步驟，即預測和更新。 預測部分可以基於xt-1生成xt。 這是我在Internet上找到的一些示例代碼。

import numpy as np

class KalmanFilter(object):
    def __init__(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):

        if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")

        self.n = F.shape[1]
        self.m = H.shape[1]

        self.F = F
        self.H = H
        self.B = 0 if B is None else B
        self.Q = np.eye(self.n) if Q is None else Q
        self.R = np.eye(self.n) if R is None else R
        self.P = np.eye(self.n) if P is None else P
        self.x = np.zeros((self.n, 1)) if x0 is None else x0

    def predict(self, u = 0):
        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
        return self.x

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        I = np.eye(self.n)
        self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), 
            (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)

def example():
    dt = 1.0/60
    F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
    H = np.array([1, 0, 0]).reshape(1, 3)
    Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
    R = np.array([0.5]).reshape(1, 1)

    x = np.linspace(-10, 10, 100)
    measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)

    kf = KalmanFilter(F = F, H = H, Q = Q, R = R)
    predictions = []

    for z in measurements:
        predictions.append(np.dot(H,  kf.predict())[0])
        kf.update(z)

    import matplotlib.pyplot as plt
    plt.plot(range(len(measurements)), measurements, label = 'Measurements')
    plt.plot(range(len(predictions)), np.array(predictions), label = 'Kalman Filter Prediction')
    plt.legend()
    plt.show()

if __name__ == '__main__':
    example()

在此問題中，我們使用t-1的值來預測t並使用t的值進行更新。 如果我想基於t預測t + 1的值。 我相應地更改了一些內容：

import numpy as np

class KalmanFilter(object):
    def __init__(self, F = None, F_1 = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):

        if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")

        self.n = F.shape[1]
        self.m = H.shape[1]

        self.F = F
        self.F_1 = F_1
        self.H = H
        self.B = 0 if B is None else B
        self.Q = np.eye(self.n) if Q is None else Q
        self.R = np.eye(self.n) if R is None else R       
        self.P = np.eye(self.n) if P is None else P
        self.x = np.zeros((self.n, 1)) if x0 is None else x0

    def predict(self, u = 0):
        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
        self.P = np.dot(np.dot(self.F, self.P), self.F_1) + self.Q
        return self.x

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        I = np.eye(self.n)
        self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), 
                        (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)


def example():
    dt = 1.0/60
    F_0 = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
    F = np.dot(F_0, F_0)
    F_1 = np.dot(F_0.T, F_0.T)
    H = np.array([1, 0, 0]).reshape(1, 3)
    Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
    R = np.array([0.5]).reshape(1, 1)

    x = np.linspace(-10, 10, 100)
    measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)

    kf = KalmanFilter(F = F, F_1 = F_1, H = H, Q = Q, R = R)
    predictions = []

    for i in range(1, len(measurements), 2):
            predictions.append(np.dot(H,  kf.predict())[0])
            kf.update(measurements[i])

    import matplotlib.pyplot as plt
    plt.plot(range(len(measurements)), measurements, label = 'Measurements')
    plt.plot(range(len(predictions)), np.array(predictions), label = 'Kalman Filter Prediction')
    plt.legend()
    plt.show()

if __name__ == '__main__':
    example()

主要變化是以下兩個：

1. 我改變了F矩陣。

2. 我使用了t + 1時間步長的值來更新我的結果。

但是，我得到的結果的長度僅為原始測量值的一半。 因為我有點跳來更新它們。

我現在有點困惑。 有人有建議或解決方案嗎？ 非常感謝

Answer 1

我看到以下問題：

1. 基於x(t)的x(t+1)的預測實際上與基於x(t-1)的x(t)的預測相同。 一切都取決於時間步長（ dt ）的定義
2. 您無需更改F矩陣，這在第一個代碼中是正確的。 但這取決於您的dt 。
3. 在第二個代碼中，您通過self.F_1替換了self.FT 但是T代表F transpose 。 不好 這就是為什么您的結果向量可能具有另一個維度的原因。

因此，如果您要預測一段時間，那么僅需要知道此時間即可。 這將是您的新dt ，當然會改變您的F和Q

如果您以時間步長dt測量，並且您想查看每秒鍾的測量結果下降會發生什么，您有兩種選擇：

選項1

更改預測步長的dt ，使其等於最后兩次測量之間的時間差，並使用新值dt重新計算F矩陣（請注意：在這種情況下，您將需要更改Q矩陣，因為系統會獲得更大的不確定性在更大的時間跨度）。

選項2

幾次以原始時間步dt執行預測，以填充兩次測量之間的時間段。 因此，如果您只想每秒處理一次，那么您的代碼將如下所示

for i in range(1, len(measurements), 2):
    kf.predict()
    kf.predict()
    kf.update(measurements[i])

更新

從評論到您的問題：

i       t      todo

1    0*dt      init
2    1*dt      predict
3    2*dt      predict, update
4    3*dt      predict
5    4*dt      predict, update

你是說這個案子嗎

看一下這個帖子 。 它顯示了如果您預測很多而沒有新更新會發生什么。