卡尔曼滤波器预测t + 2时间步长

时间:2019-06-06 01:45:13

标签: python kalman-filter

我对使用卡尔曼滤波器预测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时间步长的值来更新结果。

但是,我得到的结果的长度仅为原始测量值的一半。因为我有点跳来更新它们。

我现在有点困惑。有人有建议或解决方案吗?非常感谢

1 个答案:

答案 0 :(得分:0)

我看到以下问题:

  1. 基于x(t+1)的{​​{1}}的预测实际上与基于x(t)的{​​{1}}的预测相同。一切都取决于时间步长(x(t))的定义
  2. 您不需要更改F矩阵,这在第一个代码中是正确的。但这取决于您的x(t-1)
  3. 在第二个代码中,您将dt替换为dt。但是self.F.T代表self.F_1中的T。不好这就是为什么您的结果向量可能具有另一个维度的原因。

因此,如果您要预测一段时间,那么仅需要知道此时间即可。这将是您的新transpose,当然它将更改您的Fdt

如果您的测量是在时间步长F下进行的,并且您想查看每秒钟的测量结果下降会发生什么,则有两种选择:

选项1

更改预测步骤的Q,使其等于最后两次测量之间的时间差,然后使用新值dt重新计算dt矩阵(请注意:在这种情况下:您将需要更改您的Q矩阵,因为系统会在较大的时间步长中获得更大的不确定性。

选项2

使用原始时间步长F进行几次预测,以填充两次测量之间的时间段。因此,如果您只想每秒钟处理一次,您的代码就会像

dt

更新

对来自评论的问题:

dt

你是说这个案子吗?

看看this post。它显示了如果您预测很多而没有新更新会发生什么情况。